<commit_before>#include "internal.hpp"
#include "macros.hpp"
#include "message.hpp"

namespace CaDiCaL {

// Vivification is a special case of asymmetric tautology elimination.  It
// strengthens and removes irredundant clauses proven redundant through unit
// propagation.  The original algorithm is due to a paper by Piette, Hamadi
// published at ECAI'08.  We have an inprocessing version, e.g., it does not
// necessarily run-to-completion.  It does no conflict analysis and uses a
// new heuristic for selecting clauses to vivify.

/*------------------------------------------------------------------------*/

struct ClauseScore {
  Clause * clause;
  long score;
  ClauseScore (Clause * c) : clause (c), score (0) { }
};

struct less_clause_score {
  bool operator () (const ClauseScore & a, const ClauseScore & b) const {
    return a.score < b.score;
  }
};

// Check whether negated literal occurs less often.
//
struct less_negated_noccs2 {
  Internal * internal;
  less_negated_noccs2 (Internal * i) : internal (i) { }
  bool operator () (int a, int b) {
    long u = internal->noccs2 (-a), v = internal->noccs2 (-b);
    if (u < v) return true;
    if (u > v) return false;
    int i = abs (a), j = abs (b);
    if (i < j) return true;
    if (i > j) return false;
    return a < b;
  }
};

/*------------------------------------------------------------------------*/

void Internal::vivify () {

  assert (opts.vivify);

  SWITCH_AND_START (search, simplify, vivify);

  assert (!vivifying);
  vivifying = true;

  stats.vivifications++;
  if (level) backtrack ();

  // Refill the schedule every time.  Unchecked clauses are 'saved' by
  // setting their 'vivify' bit, such that they can be tried next time.
  //
  vector<ClauseScore> schedule;

  const const_clause_iterator end = clauses.end ();
  const_clause_iterator i;

  // After an arithmetic increasing number of calls to 'vivify' we
  // reschedule all clauses, instead of only those not tried before.  Then
  // this limit is increased by one. The argument is that we should focus on
  // clauses with many occurrences of their negated literals (also long
  // clauses), but in the limit eventually still should vivify all clauses.
  //
  bool reschedule_all = !lim.vivifywaitreset;
  if (reschedule_all) lim.vivifywaitreset = ++inc.vivifywaitreset;
  else lim.vivifywaitreset--;

  // In the first round check whether there are still clauses left, which
  // are scheduled to but have not been vivified yet.  In the second round
  // if no such clauses are found in the first round.  This is also
  // performed if rescheduling is forced because 'reschedule_all' is  true.
  //
  for (int round = 0; schedule.empty () && round <= 1; round++) {
    for (i = clauses.begin (); i != end; i++) {
      Clause * c = *i;
      if (c->garbage) continue;
      if (c->redundant) continue;
      if (c->size == 2) continue;	// see also (NO-BINARY) below
      if (!reschedule_all && !round && !c->vivify) continue;
      schedule.push_back (c);
      c->vivify = true;
    }
  }
  shrink_vector (schedule);

  // Count the number of occurrences of literals in all irredundant clauses,
  // particularly the irredundant binary clauses, responsible for most of
  // the propagation usually.
  //
  init_noccs2 ();

  for (i = clauses.begin (); i != end; i++) {
    Clause * c = *i;
    if (c->garbage) continue;
    if (c->redundant) continue;
    const const_literal_iterator eoc = c->end ();
    const_literal_iterator j;
    for (j = c->begin (); j != eoc; j++)
      noccs2 (*j)++;
  }

  // Then update the score of the candidate clauses by adding up the number
  // of occurrences of the negation of its literals.
  //
  const vector<ClauseScore>::const_iterator eos = schedule.end ();
  vector<ClauseScore>::iterator k;

  for (k = schedule.begin (); k != eos; k++) {
    ClauseScore & cs = *k;
    const const_literal_iterator eoc = cs.clause->end ();
    const_literal_iterator j;
    long score = 0;
    for (j = cs.clause->begin (); j != eoc; j++)
      score += noccs2 (-*j);
    cs.score = score;
  }

  // Now sort candidates, with best clause (many negative occurrences) last.
  //
  stable_sort (schedule.begin (), schedule.end (), less_clause_score ());

  long scheduled = schedule.size ();
  VRB ("vivification", stats.vivifications,
    "scheduled %ld clauses to be vivified %.0f%%",
    scheduled, percent (scheduled, stats.irredundant));

  // We need to make sure to propagate only over irredundant clauses.
  //
  flush_redundant_watches ();
  size_t old_propagated = propagated;	// see [RE-PROPAGATE] below.

  // Counters, for what happened.
  //
  long checked = 0, subsumed = 0, strengthened = 0, units = 0;

  // Temporarily save and sort the candidate clause literals here.
  //
  vector<int> sorted;

  // Limit the number of propagations during vivification as in 'probe'.
  //
  long delta = opts.vivifyreleff * stats.propagations.search;
  if (delta < opts.vivifymineff) delta = opts.vivifymineff;
  if (delta > opts.vivifymaxeff) delta = opts.vivifymaxeff;
  long limit = stats.propagations.vivify + delta;

  while (!unsat &&
         !schedule.empty () &&
         stats.propagations.vivify < limit) {

    // Next candidate clause to vivify.
    //
    Clause * c = schedule.back ().clause;
    schedule.pop_back ();
    assert (c->vivify);
    c->vivify = false;

    assert (!c->garbage);
    assert (!c->redundant);
    assert (c->size > 2);		// see
    assert (!level);

    // First check whether it is already satisfied.
    //
    const const_literal_iterator eoc = c->end ();
    const_literal_iterator j;
    bool satisfied = false;

    sorted.clear ();

    for (j = c->begin (); !satisfied && j != eoc; j++) {
      const int lit = *j, tmp = val (lit);
      if (tmp > 0) satisfied = true;
      else if (!tmp) sorted.push_back (lit);
    }

    if (satisfied) { mark_garbage (c); continue; }

    // The actual vivification checking is performed here, by assuming the
    // negation of each of the remaining literals of the clause in turn and
    // propagating it.  If a conflict occurs or another literal in the
    // clause becomes assigned during propagation, we can stop.
    //
    LOG (c, "vivification checking");
    checked++;

    // Sort the literals of the candidate with respect to the largest number
    // of negative occurrences.  The idea is that more negative occurrences
    // lead to more propagations and thus potentially higher earlier effect.
    //
    sort (sorted.begin (), sorted.end (), less_negated_noccs2 (this));

    // Make sure to ignore this clause during propagation.  This is not that
    // easy for binary clauses (NO-BINARY), e.g., ignoring binary clauses,
    // without changing 'propagate' and actually we also do not want to
    // remove binary clauses which are subsumed.  Those are hyper binary
    // resolvents and should be kept as learned clauses instead unless they
    // are transitive in the binary implication graph (TODO).
    //
    c->ignore = true;		// see also (NO-BINARY) above

    bool redundant = false;	// determined to be redundant / subsumed
    int remove = 0;		// at least literal 'remove' can be removed

    while (!redundant && !remove && !sorted.empty ()) {
      const int lit = sorted.back (), tmp = val (lit);
      sorted.pop_back ();
      if (tmp > 0) {
	LOG ("redundant since literal %d already true", lit);
	redundant = true;
      } else if (tmp < 0) {
	LOG ("removing at least literal %d which is already false", lit);
	remove = lit;
      } else {
	assume_decision (-lit);
	if (propagate ()) continue;
	LOG ("redundant since propagation produced conflict");
	redundant = true;
	conflict = 0;
      }
    }

    if (redundant) {
REDUNDANT:
      subsumed++;
      LOG (c, "redundant asymmetric tautology");
      mark_garbage (c);
    } else if (remove) {
      strengthened++;
      assert (level);
      assert (clause.empty ());
#ifndef NDEBUG
      bool found = false;
#endif
      // There might be other literals implied to false (or even root level
      // falsified).  Those should be removed in addition to 'remove'.
      //
      for (j = c->begin (); j != eoc; j++) {
	const int other = *j, tmp = val (other);
	Var & v = var (other);
	if (tmp > 0) {
	  LOG ("redundant since literal %d already true", other);
	  clause.clear ();
	  goto REDUNDANT;
	}
	if (tmp < 0 && !v.level) continue;
	if (tmp < 0 && v.level && v.reason) {
	  assert (v.level);
	  assert (v.reason);
	  LOG ("flushing literal %d", other);
	  mark_removed (other);
#ifndef NDEBUG
	  if (other == remove) found = true;
#endif
	} else clause.push_back (other);
      }
      assert (found);
      assert (!clause.empty ());
      backtrack ();
      if (clause.size () == 1) {
	const int unit = clause[0];
	LOG (c, "vivification shrunken to unit %d", unit);
	assert (!val (unit));
	assign_unit (unit);
	units++;
	bool ok = propagate ();
	if (!ok) learn_empty_clause ();
      } else {
#ifdef LOGGING
	Clause * d =
#endif
	new_clause_as (c);
	LOG (c, "before vivification");
	LOG (d, "after vivification");
      }
      clause.clear ();
      mark_garbage (c);
    }
    if (level) backtrack ();
    c->ignore = false;
  }

  if (!unsat) {

    erase_vector (sorted);
    reset_noccs2 ();
    erase_vector (schedule);
    disconnect_watches ();
    connect_watches ();

    // [RE-PROPAGATE] Since redundant clause were disconnected during
    // propagating vivified units above, we have propagate all those fixed
    // literals again after connecting the redundant clauses back.
    // Otherwise, the invariants for watching and blocking literals break.
    //
    if (old_propagated < propagated) {
      propagated = old_propagated;
      if (!propagate ()) {
	LOG ("propagating vivified units leads to conflict");
	learn_empty_clause ();
      }
    }
  }

  VRB ("vivification", stats.vivifications,
    "checked %ld clauses %.02f%% out of scheduled",
    checked, percent (checked, scheduled));
  if (units)
  VRB ("vivification", stats.vivifications,
    "found %ld units %.02f%% out of checked",
    units, percent (units, checked));
  VRB ("vivification", stats.vivifications,
    "subsumed %ld clauses %.02f%% out of checked",
    subsumed, percent (subsumed, checked));
  VRB ("vivification", stats.vivifications,
    "strengthened %ld clauses %.02f%% out of checked",
    strengthened, percent (strengthened, checked));

  stats.vivifychecks += checked;
  stats.vivifysubs += subsumed;
  stats.vivifystrs += strengthened;
  stats.vivifyunits += units;

  stats.subsumed += subsumed;
  stats.strengthened += strengthened;

  assert (vivifying);
  vivifying = false;

  report ('v');
  STOP_AND_SWITCH (vivify, simplify, search);
}

};
<commit_msg>fixed score<commit_after>#include "internal.hpp"
#include "macros.hpp"
#include "message.hpp"

namespace CaDiCaL {

// Vivification is a special case of asymmetric tautology elimination.  It
// strengthens and removes irredundant clauses proven redundant through unit
// propagation.  The original algorithm is due to a paper by Piette, Hamadi
// published at ECAI'08.  We have an inprocessing version, e.g., it does not
// necessarily run-to-completion.  It does no conflict analysis and uses a
// new heuristic for selecting clauses to vivify.

/*------------------------------------------------------------------------*/

struct ClauseScore {
  Clause * clause;
  double score;
  ClauseScore (Clause * c) : clause (c), score (0) { }
};

struct less_clause_score {
  bool operator () (const ClauseScore & a, const ClauseScore & b) const {
    return a.score < b.score;
  }
};

// Check whether negated literal occurs less often.
//
struct less_negated_noccs2 {
  Internal * internal;
  less_negated_noccs2 (Internal * i) : internal (i) { }
  bool operator () (int a, int b) {
    long u = internal->noccs2 (-a), v = internal->noccs2 (-b);
    if (u < v) return true;
    if (u > v) return false;
    int i = abs (a), j = abs (b);
    if (i < j) return true;
    if (i > j) return false;
    return a < b;
  }
};

/*------------------------------------------------------------------------*/

void Internal::vivify () {

  assert (opts.vivify);

  SWITCH_AND_START (search, simplify, vivify);

  assert (!vivifying);
  vivifying = true;

  stats.vivifications++;
  if (level) backtrack ();

  // Refill the schedule every time.  Unchecked clauses are 'saved' by
  // setting their 'vivify' bit, such that they can be tried next time.
  //
  vector<ClauseScore> schedule;

  const const_clause_iterator end = clauses.end ();
  const_clause_iterator i;

  // After an arithmetic increasing number of calls to 'vivify' we
  // reschedule all clauses, instead of only those not tried before.  Then
  // this limit is increased by one. The argument is that we should focus on
  // clauses with many occurrences of their negated literals (also long
  // clauses), but in the limit eventually still should vivify all clauses.
  //
  bool reschedule_all = !lim.vivifywaitreset;
  if (reschedule_all) lim.vivifywaitreset = ++inc.vivifywaitreset;
  else lim.vivifywaitreset--;

  // In the first round check whether there are still clauses left, which
  // are scheduled to but have not been vivified yet.  In the second round
  // if no such clauses are found in the first round.  This is also
  // performed if rescheduling is forced because 'reschedule_all' is  true.
  //
  for (int round = 0; schedule.empty () && round <= 1; round++) {
    for (i = clauses.begin (); i != end; i++) {
      Clause * c = *i;
      if (c->garbage) continue;
      if (c->redundant) continue;
      if (c->size == 2) continue;	// see also (NO-BINARY) below
      if (!reschedule_all && !round && !c->vivify) continue;
      schedule.push_back (c);
      c->vivify = true;
    }
  }
  shrink_vector (schedule);

  // Count the number of occurrences of literals in all irredundant clauses,
  // particularly the irredundant binary clauses, responsible for most of
  // the propagation usually.
  //
  init_noccs2 ();

  for (i = clauses.begin (); i != end; i++) {
    Clause * c = *i;
    if (c->garbage) continue;
    if (c->redundant) continue;

    // Compute score as 'pow (2, -c->size)' without 'math.h' (it actually
    // could be faster even, since second argument is always an integer).
    // This is the same score the Jeroslow-Wang heuristic would give.
    //
    double base = 0.5, score = 0.25;
    for (int n = c->size - 2; n; n >>= 1) {
      if (n & 1) score *= base;
      base *= base;
    }

    const const_literal_iterator eoc = c->end ();
    const_literal_iterator j;
    for (j = c->begin (); j != eoc; j++)
      noccs2 (*j) += score;
  }

  // Then update the score of the candidate clauses by adding up the number
  // of occurrences of the negation of its literals.
  //
  const vector<ClauseScore>::const_iterator eos = schedule.end ();
  vector<ClauseScore>::iterator k;

  for (k = schedule.begin (); k != eos; k++) {
    ClauseScore & cs = *k;
    const const_literal_iterator eoc = cs.clause->end ();
    const_literal_iterator j;
    long score = 0;
    for (j = cs.clause->begin (); j != eoc; j++)
      score += noccs2 (-*j);
    cs.score = score;
  }

  // Now sort candidates, with best clause (many negative occurrences) last.
  //
  stable_sort (schedule.begin (), schedule.end (), less_clause_score ());

  long scheduled = schedule.size ();
  VRB ("vivification", stats.vivifications,
    "scheduled %ld clauses to be vivified %.0f%%",
    scheduled, percent (scheduled, stats.irredundant));

  // We need to make sure to propagate only over irredundant clauses.
  //
  flush_redundant_watches ();
  size_t old_propagated = propagated;	// see [RE-PROPAGATE] below.

  // Counters, for what happened.
  //
  long checked = 0, subsumed = 0, strengthened = 0, units = 0;

  // Temporarily save and sort the candidate clause literals here.
  //
  vector<int> sorted;

  // Limit the number of propagations during vivification as in 'probe'.
  //
  long delta = opts.vivifyreleff * stats.propagations.search;
  if (delta < opts.vivifymineff) delta = opts.vivifymineff;
  if (delta > opts.vivifymaxeff) delta = opts.vivifymaxeff;
  long limit = stats.propagations.vivify + delta;

  while (!unsat &&
         !schedule.empty () &&
         stats.propagations.vivify < limit) {

    // Next candidate clause to vivify.
    //
    Clause * c = schedule.back ().clause;
    schedule.pop_back ();
    assert (c->vivify);
    c->vivify = false;

    assert (!c->garbage);
    assert (!c->redundant);
    assert (c->size > 2);		// see
    assert (!level);

    // First check whether it is already satisfied.
    //
    const const_literal_iterator eoc = c->end ();
    const_literal_iterator j;
    bool satisfied = false;

    sorted.clear ();

    for (j = c->begin (); !satisfied && j != eoc; j++) {
      const int lit = *j, tmp = val (lit);
      if (tmp > 0) satisfied = true;
      else if (!tmp) sorted.push_back (lit);
    }

    if (satisfied) { mark_garbage (c); continue; }

    // The actual vivification checking is performed here, by assuming the
    // negation of each of the remaining literals of the clause in turn and
    // propagating it.  If a conflict occurs or another literal in the
    // clause becomes assigned during propagation, we can stop.
    //
    LOG (c, "vivification checking");
    checked++;

    // Sort the literals of the candidate with respect to the largest number
    // of negative occurrences.  The idea is that more negative occurrences
    // lead to more propagations and thus potentially higher earlier effect.
    //
    sort (sorted.begin (), sorted.end (), less_negated_noccs2 (this));

    // Make sure to ignore this clause during propagation.  This is not that
    // easy for binary clauses (NO-BINARY), e.g., ignoring binary clauses,
    // without changing 'propagate' and actually we also do not want to
    // remove binary clauses which are subsumed.  Those are hyper binary
    // resolvents and should be kept as learned clauses instead unless they
    // are transitive in the binary implication graph (TODO).
    //
    c->ignore = true;		// see also (NO-BINARY) above

    bool redundant = false;	// determined to be redundant / subsumed
    int remove = 0;		// at least literal 'remove' can be removed

    while (!redundant && !remove && !sorted.empty ()) {
      const int lit = sorted.back (), tmp = val (lit);
      sorted.pop_back ();
      if (tmp > 0) {
	LOG ("redundant since literal %d already true", lit);
	redundant = true;
      } else if (tmp < 0) {
	LOG ("removing at least literal %d which is already false", lit);
	remove = lit;
      } else {
	assume_decision (-lit);
	if (propagate ()) continue;
	LOG ("redundant since propagation produced conflict");
	redundant = true;
	conflict = 0;
      }
    }

    if (redundant) {
REDUNDANT:
      subsumed++;
      LOG (c, "redundant asymmetric tautology");
      mark_garbage (c);
    } else if (remove) {
      strengthened++;
      assert (level);
      assert (clause.empty ());
#ifndef NDEBUG
      bool found = false;
#endif
      // There might be other literals implied to false (or even root level
      // falsified).  Those should be removed in addition to 'remove'.
      //
      for (j = c->begin (); j != eoc; j++) {
	const int other = *j, tmp = val (other);
	Var & v = var (other);
	if (tmp > 0) {
	  LOG ("redundant since literal %d already true", other);
	  clause.clear ();
	  goto REDUNDANT;
	}
	if (tmp < 0 && !v.level) continue;
	if (tmp < 0 && v.level && v.reason) {
	  assert (v.level);
	  assert (v.reason);
	  LOG ("flushing literal %d", other);
	  mark_removed (other);
#ifndef NDEBUG
	  if (other == remove) found = true;
#endif
	} else clause.push_back (other);
      }
      assert (found);
      assert (!clause.empty ());
      backtrack ();
      if (clause.size () == 1) {
	const int unit = clause[0];
	LOG (c, "vivification shrunken to unit %d", unit);
	assert (!val (unit));
	assign_unit (unit);
	units++;
	bool ok = propagate ();
	if (!ok) learn_empty_clause ();
      } else {
#ifdef LOGGING
	Clause * d =
#endif
	new_clause_as (c);
	LOG (c, "before vivification");
	LOG (d, "after vivification");
      }
      clause.clear ();
      mark_garbage (c);
    }
    if (level) backtrack ();
    c->ignore = false;
  }

  if (!unsat) {

    erase_vector (sorted);
    reset_noccs2 ();
    erase_vector (schedule);
    disconnect_watches ();
    connect_watches ();

    // [RE-PROPAGATE] Since redundant clause were disconnected during
    // propagating vivified units above, we have propagate all those fixed
    // literals again after connecting the redundant clauses back.
    // Otherwise, the invariants for watching and blocking literals break.
    //
    if (old_propagated < propagated) {
      propagated = old_propagated;
      if (!propagate ()) {
	LOG ("propagating vivified units leads to conflict");
	learn_empty_clause ();
      }
    }
  }

  VRB ("vivification", stats.vivifications,
    "checked %ld clauses %.02f%% out of scheduled",
    checked, percent (checked, scheduled));
  if (units)
  VRB ("vivification", stats.vivifications,
    "found %ld units %.02f%% out of checked",
    units, percent (units, checked));
  VRB ("vivification", stats.vivifications,
    "subsumed %ld clauses %.02f%% out of checked",
    subsumed, percent (subsumed, checked));
  VRB ("vivification", stats.vivifications,
    "strengthened %ld clauses %.02f%% out of checked",
    strengthened, percent (strengthened, checked));

  stats.vivifychecks += checked;
  stats.vivifysubs += subsumed;
  stats.vivifystrs += strengthened;
  stats.vivifyunits += units;

  stats.subsumed += subsumed;
  stats.strengthened += strengthened;

  assert (vivifying);
  vivifying = false;

  report ('v');
  STOP_AND_SWITCH (vivify, simplify, search);
}

};
<|endoftext|>


<commit_before>#include "internal.hpp"
#include "macros.hpp"
#include "message.hpp"

namespace CaDiCaL {

// Vivification is a special case of asymmetric tautology elimination.  It
// strengthens and removes irredundant clauses proven redundant through unit
// propagation.  The original algorithm is due to a paper by Piette, Hamadi
// published at ECAI'08.  We have an inprocessing version, e.g., it does not
// necessarily run-to-completion.  It does no conflict analysis and uses a
// new heuristic for selecting clauses to vivify.

/*------------------------------------------------------------------------*/

struct ClauseScore {
  Clause * clause;
  double score;
  ClauseScore (Clause * c) : clause (c), score (0) { }
};

struct less_clause_score {
  bool operator () (const ClauseScore & a, const ClauseScore & b) const {
    return a.score < b.score;
  }
};

// Check whether negated literal occurs less often.
//
struct less_negated_noccs2 {
  Internal * internal;
  less_negated_noccs2 (Internal * i) : internal (i) { }
  bool operator () (int a, int b) {
    long u = internal->noccs2 (-a), v = internal->noccs2 (-b);
    if (u < v) return true;
    if (u > v) return false;
    int i = abs (a), j = abs (b);
    if (i < j) return true;
    if (i > j) return false;
    return a < b;
  }
};

/*------------------------------------------------------------------------*/

void Internal::vivify () {

  assert (opts.vivify);

  SWITCH_AND_START (search, simplify, vivify);

  assert (!vivifying);
  vivifying = true;

  stats.vivifications++;
  if (level) backtrack ();

  // Refill the schedule every time.  Unchecked clauses are 'saved' by
  // setting their 'vivify' bit, such that they can be tried next time.
  //
  vector<ClauseScore> schedule;

  const const_clause_iterator end = clauses.end ();
  const_clause_iterator i;

  // After an arithmetic increasing number of calls to 'vivify' we
  // reschedule all clauses, instead of only those not tried before.  Then
  // this limit is increased by one. The argument is that we should focus on
  // clauses with many occurrences of their negated literals (also long
  // clauses), but in the limit eventually still should vivify all clauses.
  //
  bool reschedule_all = !lim.vivify_wait_reschedule;
  if (reschedule_all) {
    VRB ("vivify", stats.vivifications,
      "forced to reschedule all clauses");
    lim.vivify_wait_reschedule = ++inc.vivify_wait_reschedule;
  } else lim.vivify_wait_reschedule--;

  // In the first round check whether there are still clauses left, which
  // are scheduled to but have not been vivified yet.  In the second round
  // if no such clauses are found in the first round.  This is also
  // performed if rescheduling is forced because 'reschedule_all' is  true.
  //
  for (int round = 0; schedule.empty () && round <= 1; round++) {
    for (i = clauses.begin (); i != end; i++) {
      Clause * c = *i;
      if (c->garbage) continue;
      if (c->redundant) continue;
      if (c->size == 2) continue;       // see also [NO-BINARY] below
      if (!reschedule_all && !round && !c->vivify) continue;
      schedule.push_back (c);
      c->vivify = true;
    }
  }
  shrink_vector (schedule);

  // Count the number of occurrences of literals in all irredundant clauses,
  // particularly the irredundant binary clauses, responsible for most of
  // the propagation usually.
  //
  init_noccs2 ();

  for (i = clauses.begin (); i != end; i++) {
    Clause * c = *i;
    if (c->garbage) continue;
    if (c->redundant) continue;

    // Compute score as 'pow (2, -c->size)' without 'math.h' (it actually
    // could be faster even, since second argument is always an integer).
    // This is the same score the Jeroslow-Wang heuristic would give.
    //
    double base = 0.5, score = 0.25;
    for (int n = c->size - 2; n; n >>= 1) {
      if (n & 1) score *= base;
      base *= base;
    }

    const const_literal_iterator eoc = c->end ();
    const_literal_iterator j;
    for (j = c->begin (); j != eoc; j++)
      noccs2 (*j) += score;
  }

  // Then update the score of the candidate clauses by adding up the number
  // of occurrences of the negation of its literals.
  //
  const vector<ClauseScore>::const_iterator eos = schedule.end ();
  vector<ClauseScore>::iterator k;

  for (k = schedule.begin (); k != eos; k++) {
    ClauseScore & cs = *k;
    const const_literal_iterator eoc = cs.clause->end ();
    const_literal_iterator j;
    long score = 0;
    for (j = cs.clause->begin (); j != eoc; j++)
      score += noccs2 (-*j);
    cs.score = score;
  }

  // Now sort candidates, with best clause (many negative occurrences) last.
  //
  stable_sort (schedule.begin (), schedule.end (), less_clause_score ());

  long scheduled = schedule.size ();
  VRB ("vivify", stats.vivifications,
    "scheduled %ld clauses to be vivified %.0f%%",
    scheduled, percent (scheduled, stats.irredundant));

  // We need to make sure to propagate only over irredundant clauses.
  //
  flush_redundant_watches ();
  size_t old_propagated = propagated;   // see [RE-PROPAGATE] below.

  // Counters, for what happened.
  //
  long checked = 0, subsumed = 0, strengthened = 0, units = 0;

  // Temporarily save and sort the candidate clause literals here.
  //
  vector<int> sorted;

  // Limit the number of propagations during vivification as in 'probe'.
  //
  long delta = opts.vivifyreleff * stats.propagations.search;
  if (delta < opts.vivifymineff) delta = opts.vivifymineff;
  if (delta > opts.vivifymaxeff) delta = opts.vivifymaxeff;
  long limit = stats.propagations.vivify + delta;

  while (!unsat &&
         !schedule.empty () &&
         stats.propagations.vivify < limit) {

    // Next candidate clause to vivify.
    //
    Clause * c = schedule.back ().clause;
    schedule.pop_back ();
    assert (c->vivify);
    c->vivify = false;

    assert (!c->garbage);
    assert (!c->redundant);
    assert (c->size > 2);               // see [NO-BINARY] above
    assert (!level);

    // First check whether it is already satisfied.
    //
    const const_literal_iterator eoc = c->end ();
    const_literal_iterator j;
    bool satisfied = false;

    sorted.clear ();

    for (j = c->begin (); !satisfied && j != eoc; j++) {
      const int lit = *j, tmp = val (lit);
      if (tmp > 0) satisfied = true;
      else if (!tmp) sorted.push_back (lit);
    }

    if (satisfied) { mark_garbage (c); continue; }

    // The actual vivification checking is performed here, by assuming the
    // negation of each of the remaining literals of the clause in turn and
    // propagating it.  If a conflict occurs or another literal in the
    // clause becomes assigned during propagation, we can stop.
    //
    LOG (c, "vivification checking");
    checked++;

    // Sort the literals of the candidate with respect to the largest number
    // of negative occurrences.  The idea is that more negative occurrences
    // lead to more propagations and thus potentially higher earlier effect.
    //
    sort (sorted.begin (), sorted.end (), less_negated_noccs2 (this));

    // Make sure to ignore this clause during propagation.  This is not that
    // easy for binary clauses [NO-BINARY], e.g., ignoring binary clauses,
    // without changing 'propagate' and actually we also do not want to
    // remove binary clauses which are subsumed.  Those are hyper binary
    // resolvents and should be kept as learned clauses instead unless they
    // are transitive in the binary implication graph (TODO).
    //
    c->ignore = true;           // see also [NO-BINARY] above

    bool redundant = false;     // determined to be redundant / subsumed
    int remove = 0;             // at least literal 'remove' can be removed

    while (!redundant && !remove && !sorted.empty ()) {
      const int lit = sorted.back (), tmp = val (lit);
      sorted.pop_back ();
      if (tmp > 0) {
        LOG ("redundant since literal %d already true", lit);
        redundant = true;
      } else if (tmp < 0) {
        LOG ("removing at least literal %d which is already false", lit);
        remove = lit;
      } else {
        assume_decision (-lit);
        if (propagate ()) continue;
        LOG ("redundant since propagation produced conflict");
        redundant = true;
        conflict = 0;
      }
    }

    if (redundant) {
REDUNDANT:
      subsumed++;
      LOG (c, "redundant asymmetric tautology");
      mark_garbage (c);
    } else if (remove) {
      assert (level);
      assert (clause.empty ());
#ifndef NDEBUG
      bool found = false;
#endif
      // There might be other literals implied to false (or even root level
      // falsified).  Those should be removed in addition to 'remove'.  It
      // might further be possible that a latter to be assumed literal is
      // already forced to true in which case the clause is actually
      // redundant (we solved this by bad style 'goto' programming).
      //
      for (j = c->begin (); j != eoc; j++) {
        const int other = *j, tmp = val (other);
        Var & v = var (other);
        if (tmp > 0) {
          LOG ("redundant since literal %d already true", other);
          clause.clear ();
          goto REDUNDANT;
        }
        if (tmp < 0 && !v.level) continue;
        if (tmp < 0 && v.level && v.reason) {
          assert (v.level);
          assert (v.reason);
          LOG ("flushing literal %d", other);
          mark_removed (other);
#ifndef NDEBUG
          if (other == remove) found = true;
#endif
        } else clause.push_back (other);
      }

      // Assume remaining literals and propagate them to see whether this
      // clause is actually redundant and does not have to be strengthened.

      if (!sorted.empty ()) {

	do {
	  const int lit = sorted.back (), tmp = val (lit);
	  sorted.pop_back ();
	  if (tmp < 0) continue;
	  assert (!tmp);
	  assume_decision (-lit);
	} while (!sorted.empty ());

	if (!propagate ()) {
          LOG ("redundant since propagating rest produces conflict");
	  clause.clear ();
	  conflict = 0;
	  goto REDUNDANT;
	}

	LOG ("propagating remaining negated literals without conflict");
      }

      strengthened++; // only now because of 'goto REDUNDANT' above

      assert (found);
      assert (!clause.empty ());
      backtrack ();
      if (clause.size () == 1) {
        const int unit = clause[0];
        LOG (c, "vivification shrunken to unit %d", unit);
        assert (!val (unit));
        assign_unit (unit);
        units++;
        bool ok = propagate ();
        if (!ok) learn_empty_clause ();
      } else {
#ifdef LOGGING
        Clause * d =
#endif
        new_clause_as (c);
        LOG (c, "before vivification");
        LOG (d, "after vivification");
      }
      clause.clear ();
      mark_garbage (c);
    }
    if (level) backtrack ();
    c->ignore = false;
  }

  if (!unsat) {

    erase_vector (sorted);
    reset_noccs2 ();
    erase_vector (schedule);
    disconnect_watches ();
    connect_watches ();

    // [RE-PROPAGATE] Since redundant clause were disconnected during
    // propagating vivified units above, we have propagate all those fixed
    // literals again after connecting the redundant clauses back.
    // Otherwise, the invariants for watching and blocking literals break.
    //
    if (old_propagated < propagated) {
      propagated = old_propagated;
      if (!propagate ()) {
        LOG ("propagating vivified units leads to conflict");
        learn_empty_clause ();
      }
    }
  }

  VRB ("vivify", stats.vivifications,
    "checked %ld clauses %.02f%% out of scheduled",
    checked, percent (checked, scheduled));
  if (units)
  VRB ("vivify", stats.vivifications,
    "found %ld units %.02f%% out of checked",
    units, percent (units, checked));
  VRB ("vivify", stats.vivifications,
    "subsumed %ld clauses %.02f%% out of checked",
    subsumed, percent (subsumed, checked));
  VRB ("vivify", stats.vivifications,
    "strengthened %ld clauses %.02f%% out of checked",
    strengthened, percent (strengthened, checked));

  stats.vivifychecks += checked;
  stats.vivifysubs += subsumed;
  stats.vivifystrs += strengthened;
  stats.vivifyunits += units;

  stats.subsumed += subsumed;
  stats.strengthened += strengthened;

  assert (vivifying);
  vivifying = false;

  report ('v');
  STOP_AND_SWITCH (vivify, simplify, search);
}

};
<commit_msg>try redundant again<commit_after>#include "internal.hpp"
#include "macros.hpp"
#include "message.hpp"

namespace CaDiCaL {

// Vivification is a special case of asymmetric tautology elimination.  It
// strengthens and removes irredundant clauses proven redundant through unit
// propagation.  The original algorithm is due to a paper by Piette, Hamadi
// published at ECAI'08.  We have an inprocessing version, e.g., it does not
// necessarily run-to-completion.  It does no conflict analysis and uses a
// new heuristic for selecting clauses to vivify.

/*------------------------------------------------------------------------*/

struct ClauseScore {
  Clause * clause;
  double score;
  ClauseScore (Clause * c) : clause (c), score (0) { }
};

struct less_clause_score {
  bool operator () (const ClauseScore & a, const ClauseScore & b) const {
    return a.score < b.score;
  }
};

// Check whether negated literal occurs less often.
//
struct less_negated_noccs2 {
  Internal * internal;
  less_negated_noccs2 (Internal * i) : internal (i) { }
  bool operator () (int a, int b) {
    long u = internal->noccs2 (-a), v = internal->noccs2 (-b);
    if (u < v) return true;
    if (u > v) return false;
    int i = abs (a), j = abs (b);
    if (i < j) return true;
    if (i > j) return false;
    return a < b;
  }
};

/*------------------------------------------------------------------------*/

void Internal::vivify () {

  assert (opts.vivify);

  SWITCH_AND_START (search, simplify, vivify);

  assert (!vivifying);
  vivifying = true;

  stats.vivifications++;
  if (level) backtrack ();

  // Refill the schedule every time.  Unchecked clauses are 'saved' by
  // setting their 'vivify' bit, such that they can be tried next time.
  //
  vector<ClauseScore> schedule;

  const const_clause_iterator end = clauses.end ();
  const_clause_iterator i;

  // After an arithmetic increasing number of calls to 'vivify' we
  // reschedule all clauses, instead of only those not tried before.  Then
  // this limit is increased by one. The argument is that we should focus on
  // clauses with many occurrences of their negated literals (also long
  // clauses), but in the limit eventually still should vivify all clauses.
  //
  bool reschedule_all = !lim.vivify_wait_reschedule;
  if (reschedule_all) {
    VRB ("vivify", stats.vivifications,
      "forced to reschedule all clauses");
    lim.vivify_wait_reschedule = ++inc.vivify_wait_reschedule;
  } else lim.vivify_wait_reschedule--;

  // In the first round check whether there are still clauses left, which
  // are scheduled to but have not been vivified yet.  In the second round
  // if no such clauses are found in the first round.  This is also
  // performed if rescheduling is forced because 'reschedule_all' is  true.
  //
  for (int round = 0; schedule.empty () && round <= 1; round++) {
    for (i = clauses.begin (); i != end; i++) {
      Clause * c = *i;
      if (c->garbage) continue;
      if (c->redundant) continue;
      if (c->size == 2) continue;       // see also [NO-BINARY] below
      if (!reschedule_all && !round && !c->vivify) continue;
      schedule.push_back (c);
      c->vivify = true;
    }
  }
  shrink_vector (schedule);

  // Count the number of occurrences of literals in all irredundant clauses,
  // particularly the irredundant binary clauses, responsible for most of
  // the propagation usually.
  //
  init_noccs2 ();

  for (i = clauses.begin (); i != end; i++) {
    Clause * c = *i;
    if (c->garbage) continue;
    if (c->redundant) continue;

    // Compute score as 'pow (2, -c->size)' without 'math.h' (it actually
    // could be faster even, since second argument is always an integer).
    // This is the same score the Jeroslow-Wang heuristic would give.
    //
    double base = 0.5, score = 0.25;
    for (int n = c->size - 2; n; n >>= 1) {
      if (n & 1) score *= base;
      base *= base;
    }

    const const_literal_iterator eoc = c->end ();
    const_literal_iterator j;
    for (j = c->begin (); j != eoc; j++)
      noccs2 (*j) += score;
  }

  // Then update the score of the candidate clauses by adding up the number
  // of occurrences of the negation of its literals.
  //
  const vector<ClauseScore>::const_iterator eos = schedule.end ();
  vector<ClauseScore>::iterator k;

  for (k = schedule.begin (); k != eos; k++) {
    ClauseScore & cs = *k;
    const const_literal_iterator eoc = cs.clause->end ();
    const_literal_iterator j;
    long score = 0;
    for (j = cs.clause->begin (); j != eoc; j++)
      score += noccs2 (-*j);
    cs.score = score;
  }

  // Now sort candidates, with best clause (many negative occurrences) last.
  //
  stable_sort (schedule.begin (), schedule.end (), less_clause_score ());

  long scheduled = schedule.size ();
  VRB ("vivify", stats.vivifications,
    "scheduled %ld clauses to be vivified %.0f%%",
    scheduled, percent (scheduled, stats.irredundant));

  // We need to make sure to propagate only over irredundant clauses.
  //
  flush_redundant_watches ();
  size_t old_propagated = propagated;   // see [RE-PROPAGATE] below.

  // Counters, for what happened.
  //
  long checked = 0, subsumed = 0, strengthened = 0, units = 0;

  // Temporarily save and sort the candidate clause literals here.
  //
  vector<int> sorted;

  // Limit the number of propagations during vivification as in 'probe'.
  //
  long delta = opts.vivifyreleff * stats.propagations.search;
  if (delta < opts.vivifymineff) delta = opts.vivifymineff;
  if (delta > opts.vivifymaxeff) delta = opts.vivifymaxeff;
  long limit = stats.propagations.vivify + delta;

  while (!unsat &&
         !schedule.empty () &&
         stats.propagations.vivify < limit) {

    // Next candidate clause to vivify.
    //
    Clause * c = schedule.back ().clause;
    schedule.pop_back ();
    assert (c->vivify);
    c->vivify = false;

    assert (!c->garbage);
    assert (!c->redundant);
    assert (c->size > 2);               // see [NO-BINARY] above
    assert (!level);

    // First check whether it is already satisfied.
    //
    const const_literal_iterator eoc = c->end ();
    const_literal_iterator j;
    bool satisfied = false;

    sorted.clear ();

    for (j = c->begin (); !satisfied && j != eoc; j++) {
      const int lit = *j, tmp = val (lit);
      if (tmp > 0) satisfied = true;
      else if (!tmp) sorted.push_back (lit);
    }

    if (satisfied) { mark_garbage (c); continue; }

    // The actual vivification checking is performed here, by assuming the
    // negation of each of the remaining literals of the clause in turn and
    // propagating it.  If a conflict occurs or another literal in the
    // clause becomes assigned during propagation, we can stop.
    //
    LOG (c, "vivification checking");
    checked++;

    // Sort the literals of the candidate with respect to the largest number
    // of negative occurrences.  The idea is that more negative occurrences
    // lead to more propagations and thus potentially higher earlier effect.
    //
    sort (sorted.begin (), sorted.end (), less_negated_noccs2 (this));

    // Make sure to ignore this clause during propagation.  This is not that
    // easy for binary clauses [NO-BINARY], e.g., ignoring binary clauses,
    // without changing 'propagate' and actually we also do not want to
    // remove binary clauses which are subsumed.  Those are hyper binary
    // resolvents and should be kept as learned clauses instead unless they
    // are transitive in the binary implication graph (TODO).
    //
    c->ignore = true;           // see also [NO-BINARY] above

    bool redundant = false;     // determined to be redundant / subsumed
    int remove = 0;             // at least literal 'remove' can be removed

    while (!redundant && !remove && !sorted.empty ()) {
      const int lit = sorted.back (), tmp = val (lit);
      sorted.pop_back ();
      if (tmp > 0) {
        LOG ("redundant since literal %d already true", lit);
        redundant = true;
      } else if (tmp < 0) {
        LOG ("removing at least literal %d which is already false", lit);
        remove = lit;
      } else {
        assume_decision (-lit);
        if (propagate ()) continue;
        LOG ("redundant since propagation produced conflict");
        redundant = true;
        conflict = 0;
      }
    }

    if (redundant) {
REDUNDANT:
      subsumed++;
      LOG (c, "redundant asymmetric tautology");
      mark_garbage (c);
    } else if (remove) {
      assert (level);
      assert (clause.empty ());
#ifndef NDEBUG
      bool found = false;
#endif
      // There might be other literals implied to false (or even root level
      // falsified).  Those should be removed in addition to 'remove'.  It
      // might further be possible that a latter to be assumed literal is
      // already forced to true in which case the clause is actually
      // redundant (we solved this by bad style 'goto' programming).
      //
      for (j = c->begin (); j != eoc; j++) {
        const int other = *j, tmp = val (other);
        Var & v = var (other);
        if (tmp > 0) {
          LOG ("redundant since literal %d already true", other);
          clause.clear ();
          goto REDUNDANT;
        }
        if (tmp < 0 && !v.level) continue;
        if (tmp < 0 && v.level && v.reason) {
          assert (v.level);
          assert (v.reason);
          LOG ("flushing literal %d", other);
          mark_removed (other);
#ifndef NDEBUG
          if (other == remove) found = true;
#endif
        } else clause.push_back (other);
      }

      assert (found);

      // Assume remaining literals and propagate them to see whether this
      // clause is actually redundant and does not have to be strengthened.

      if (!sorted.empty ()) {

	do {
	  const int lit = sorted.back (), tmp = val (lit);
	  sorted.pop_back ();
	  if (tmp < 0) continue;
	  if (tmp > 0) {
	    LOG ("redundant since literal %d already true", lit);
	    redundant = true;
	  } else {
	    assume_decision (-lit);
	    redundant = !propagate ();
	  }
	} while (!redundant && !sorted.empty ());

	if (redundant) {
	  if (conflict) conflict = 0;
          LOG ("redundant since propagating rest produces conflict");
	  clause.clear ();
	  goto REDUNDANT;
	}

	LOG ("propagating remaining negated literals without conflict");
      }

      strengthened++; // only now because of 'goto REDUNDANT' above (twice)

      assert (!clause.empty ());
      backtrack ();
      if (clause.size () == 1) {
        const int unit = clause[0];
        LOG (c, "vivification shrunken to unit %d", unit);
        assert (!val (unit));
        assign_unit (unit);
        units++;
        bool ok = propagate ();
        if (!ok) learn_empty_clause ();
      } else {
#ifdef LOGGING
        Clause * d =
#endif
        new_clause_as (c);
        LOG (c, "before vivification");
        LOG (d, "after vivification");
      }
      clause.clear ();
      mark_garbage (c);
    }
    if (level) backtrack ();
    c->ignore = false;
  }

  if (!unsat) {

    erase_vector (sorted);
    reset_noccs2 ();
    erase_vector (schedule);
    disconnect_watches ();
    connect_watches ();

    // [RE-PROPAGATE] Since redundant clause were disconnected during
    // propagating vivified units above, we have propagate all those fixed
    // literals again after connecting the redundant clauses back.
    // Otherwise, the invariants for watching and blocking literals break.
    //
    if (old_propagated < propagated) {
      propagated = old_propagated;
      if (!propagate ()) {
        LOG ("propagating vivified units leads to conflict");
        learn_empty_clause ();
      }
    }
  }

  VRB ("vivify", stats.vivifications,
    "checked %ld clauses %.02f%% out of scheduled",
    checked, percent (checked, scheduled));
  if (units)
  VRB ("vivify", stats.vivifications,
    "found %ld units %.02f%% out of checked",
    units, percent (units, checked));
  VRB ("vivify", stats.vivifications,
    "subsumed %ld clauses %.02f%% out of checked",
    subsumed, percent (subsumed, checked));
  VRB ("vivify", stats.vivifications,
    "strengthened %ld clauses %.02f%% out of checked",
    strengthened, percent (strengthened, checked));

  stats.vivifychecks += checked;
  stats.vivifysubs += subsumed;
  stats.vivifystrs += strengthened;
  stats.vivifyunits += units;

  stats.subsumed += subsumed;
  stats.strengthened += strengthened;

  assert (vivifying);
  vivifying = false;

  report ('v');
  STOP_AND_SWITCH (vivify, simplify, search);
}

};
<|endoftext|>


<commit_before>#ifndef SOFA_COMPONENTS_LagrangianMultiplierContactConstraint_INL
#define SOFA_COMPONENTS_LagrangianMultiplierContactConstraint_INL

#include "LagrangianMultiplierContactConstraint.h"
#include "Sofa/Core/Constraint.inl"
#include "Sofa/Core/MechanicalObject.inl"
#include "Common/config.h"
#include <assert.h>
#include <GL/gl.h>
#include "GL/template.h"

namespace Sofa
{

namespace Components
{

template<class DataTypes>
void LagrangianMultiplierContactConstraint<DataTypes>::addContact(int m1, int m2, const Deriv& norm, Real dist, Real ks, Real mu_s, Real mu_v)
{
    int i = contacts.size();
    contacts.resize(i+1);
    lambda->resize(i+1);
    (*lambda->getX())[i] = 0;
    Contact& c = contacts[i];
    c.m1 = m1;
    c.m2 = m2;
    c.norm = norm;
    c.dist = dist;
    c.ks = ks;
    c.mu_s = mu_s;
    c.mu_v = mu_v;
    c.pen = 0;
}

template<class DataTypes>
void LagrangianMultiplierContactConstraint<DataTypes>::addForce()
{
    assert(this->object1);
    assert(this->object2);
    VecDeriv& f1 = *this->object1->getF();
    VecCoord& p1 = *this->object1->getX();
    VecDeriv& v1 = *this->object1->getV();
    VecDeriv& f2 = *this->object2->getF();
    VecCoord& p2 = *this->object2->getX();
    VecDeriv& v2 = *this->object2->getV();
    f1.resize(p1.size());
    f2.resize(p2.size());

    LMVecCoord& lambda = *this->lambda->getX();
    LMVecDeriv& flambda = *this->lambda->getF();
    flambda.resize(lambda.size());

    // Create list of active contact
    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        c.pen0 = (p2[c.m2]-p1[c.m1])*c.norm - c.dist;//c.dist - (p1[c.m1]-p2[c.m2])*c.norm;
    }

    // flamdba += C X

    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        if (c.pen0 > 0) continue;
        flambda[i] += p2[c.m2]*c.norm - p1[c.m1]*c.norm - c.dist;
    }

    // f += Ct lambda

    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        if (c.pen0 > 0) continue;
        Real v = lambda[i];
        f1[c.m1] += c.norm * v;
        f2[c.m2] -= c.norm * v;
    }
}

template<class DataTypes>
void LagrangianMultiplierContactConstraint<DataTypes>::addDForce()
{
    VecDeriv& f1  = *this->object1->getF();
    VecCoord& p1 = *this->object1->getX();
    VecDeriv& dx1 = *this->object1->getDx();
    VecDeriv& f2  = *this->object2->getF();
    VecCoord& p2 = *this->object2->getX();
    VecDeriv& dx2 = *this->object2->getDx();
    f1.resize(dx1.size());
    f2.resize(dx2.size());

    LMVecCoord& lambda = *this->lambda->getX();
    LMVecCoord& dlambda = *this->lambda->getDx();
    LMVecDeriv& flambda = *this->lambda->getF();
    flambda.resize(dlambda.size());

    // dflamdba += C dX

    // Create list of active contact
    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        c.pen = c.pen0; // + dx2[c.m2]*c.norm - dx1[c.m1]*c.norm;
    }

    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        if (c.pen > 0) continue;
        flambda[i] += dx2[c.m2]*c.norm - dx1[c.m1]*c.norm;
    }

    // df += Ct dlambda

    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        if (c.pen > 0) continue;
        Real v = dlambda[i];
        f1[c.m1] += c.norm * v;
        f2[c.m2] -= c.norm * v;
    }
}

template<class DataTypes>
void LagrangianMultiplierContactConstraint<DataTypes>::draw()
{
    if (!getContext()->getShowForceFields()) return;
    VecCoord& p1 = *this->object1->getX();
    VecCoord& p2 = *this->object2->getX();
    LMVecCoord& lambda = *this->lambda->getX();
    glDisable(GL_LIGHTING);

    glBegin(GL_LINES);
    for (unsigned int i=0; i<contacts.size(); i++)
    {
        const Contact& c = contacts[i];
        Real d = c.dist - (p2[c.m2]-p1[c.m1])*c.norm;
        if (d > 0)
            glColor4f(1,0,0,1);
        else
            glColor4f(0,1,0,1);
        GL::glVertexT(p1[c.m1]);
        GL::glVertexT(p2[c.m2]);
    }
    glEnd();

    //if (getContext()->getShowNormals())
    {
        glColor4f(1,1,0,1);
        glBegin(GL_LINES);
        for (unsigned int i=0; i<contacts.size(); i++)
        {
            const Contact& c = contacts[i];
            if (c.pen > 0) continue;
            Coord p = p1[c.m1] - c.norm * lambda[i];
            GL::glVertexT(p1[c.m1]);
            GL::glVertexT(p);
            p = p2[c.m2] + c.norm * lambda[i];
            GL::glVertexT(p2[c.m2]);
            GL::glVertexT(p);
        }
        glEnd();
    }
}

} // namespace Components

} // namespace Sofa

#endif
<commit_msg>r309/sofa-dev : Correction of the unilateral Lagrange multipliers response method: - test based on the Lagrange multipliers values to know if a contact should be considered or not ! - This test should be relative to 0...but for numerical issue, it is relative to the value -0.05<commit_after>#ifndef SOFA_COMPONENTS_LagrangianMultiplierContactConstraint_INL
#define SOFA_COMPONENTS_LagrangianMultiplierContactConstraint_INL

#include "LagrangianMultiplierContactConstraint.h"
#include "Sofa/Core/Constraint.inl"
#include "Sofa/Core/MechanicalObject.inl"
#include "Common/config.h"
#include <assert.h>
#include <GL/gl.h>
#include "GL/template.h"

namespace Sofa
{

namespace Components
{

template<class DataTypes>
void LagrangianMultiplierContactConstraint<DataTypes>::addContact(int m1, int m2, const Deriv& norm, Real dist, Real ks, Real mu_s, Real mu_v)
{
    int i = contacts.size();
    contacts.resize(i+1);
    lambda->resize(i+1);
    (*lambda->getX())[i] = 0;
    Contact& c = contacts[i];
    c.m1 = m1;
    c.m2 = m2;
    c.norm = norm;
    c.dist = dist;
    c.ks = ks;
    c.mu_s = mu_s;
    c.mu_v = mu_v;
    c.pen = 0;
}

template<class DataTypes>
void LagrangianMultiplierContactConstraint<DataTypes>::addForce()
{
    assert(this->object1);
    assert(this->object2);
    VecDeriv& f1 = *this->object1->getF();
    VecCoord& p1 = *this->object1->getX();
    VecDeriv& v1 = *this->object1->getV();
    VecDeriv& f2 = *this->object2->getF();
    VecCoord& p2 = *this->object2->getX();
    VecDeriv& v2 = *this->object2->getV();
    f1.resize(p1.size());
    f2.resize(p2.size());

    LMVecCoord& lambda = *this->lambda->getX();
    LMVecDeriv& flambda = *this->lambda->getF();
    flambda.resize(lambda.size());

    // Create list of active contact
    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        c.pen = c.pen0 = (p2[c.m2]-p1[c.m1])*c.norm - c.dist;//c.dist - (p1[c.m1]-p2[c.m2])*c.norm;
        lambda[i] = c.pen0;
    }

    // flamdba += C . DOF
    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        if (lambda[i] > -0.05) continue;
        flambda[i] += p2[c.m2]*c.norm - p1[c.m1]*c.norm - c.dist;
    }

    // f += Ct . lambda
    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        Real v = lambda[i];
        if (lambda[i] > -0.05) continue;
        f1[c.m1] += c.norm * v;
        f2[c.m2] -= c.norm * v;
    }
}

template<class DataTypes>
void LagrangianMultiplierContactConstraint<DataTypes>::addDForce()
{
    VecDeriv& f1  = *this->object1->getF();
    VecCoord& p1 = *this->object1->getX();
    VecDeriv& dx1 = *this->object1->getDx();
    VecDeriv& f2  = *this->object2->getF();
    VecCoord& p2 = *this->object2->getX();
    VecDeriv& dx2 = *this->object2->getDx();
    f1.resize(dx1.size());
    f2.resize(dx2.size());

    LMVecCoord& lambda = *this->lambda->getX();
    LMVecCoord& dlambda = *this->lambda->getDx();
    LMVecDeriv& flambda = *this->lambda->getF();
    flambda.resize(dlambda.size());

    // dflamdba += C . dX
    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        if (lambda[i] > -0.05) continue;
        flambda[i] += dx2[c.m2]*c.norm - dx1[c.m1]*c.norm;
    }

    // df += Ct . dlambda
    for (unsigned int i=0; i<contacts.size(); i++)
    {
        Contact& c = contacts[i];
        if (lambda[i] > -0.05) continue;
        Real v = dlambda[i];
        f1[c.m1] += c.norm * v;
        f2[c.m2] -= c.norm * v;
    }
}

template<class DataTypes>
void LagrangianMultiplierContactConstraint<DataTypes>::draw()
{
    if (!getContext()->getShowForceFields()) return;
    VecCoord& p1 = *this->object1->getX();
    VecCoord& p2 = *this->object2->getX();
    LMVecCoord& lambda = *this->lambda->getX();
    glDisable(GL_LIGHTING);

    glBegin(GL_LINES);
    for (unsigned int i=0; i<contacts.size(); i++)
    {
        const Contact& c = contacts[i];
        Real d = c.dist - (p2[c.m2]-p1[c.m1])*c.norm;
        if (d > 0)
            glColor4f(1,0,0,1);
        else
            glColor4f(0,1,0,1);
        GL::glVertexT(p1[c.m1]);
        GL::glVertexT(p2[c.m2]);
    }
    glEnd();

    //if (getContext()->getShowNormals())
    {
        glColor4f(1,1,0,1);
        glBegin(GL_LINES);
        for (unsigned int i=0; i<contacts.size(); i++)
        {
            const Contact& c = contacts[i];
            if (c.pen > 0) continue;
            Coord p = p1[c.m1] - c.norm * lambda[i];
            GL::glVertexT(p1[c.m1]);
            GL::glVertexT(p);
            p = p2[c.m2] + c.norm * lambda[i];
            GL::glVertexT(p2[c.m2]);
            GL::glVertexT(p);
        }
        glEnd();
    }
}

} // namespace Components

} // namespace Sofa

#endif
<|endoftext|>


<commit_before>#include "rubykokuban/BindImage.hpp"

#include "mruby.h"
#include "mruby/class.h"
#include "mruby/data.h"
#include "mruby/string.h"
#include "ofMain.h"

namespace rubykokuban {

namespace {
void free(mrb_state *mrb, void *p)
{
    if (p) {
        delete ((ofImage*)p);
    }
}

struct mrb_data_type data_type = { "rubykokuban_image", free };

mrb_value load(mrb_state *mrb, mrb_value self)
{
    ofImage* obj = new ofImage();

    mrb_value str;
    mrb_get_args(mrb, "S", &str);

    string filename(mrb_string_value_ptr(mrb, str));
    obj->loadImage(filename);

    struct RData *data = mrb_data_object_alloc(mrb, mrb_class_ptr(self), obj, &data_type);
    return mrb_obj_value(data);
}

mrb_value grab_screen(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value save(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value color(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value set_color(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value resize(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value set_image_type(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value crop_bang(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value crop(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value rotate90(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value mirror(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value update(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value set_anchor_percent(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value set_anchor_point(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value reset_anchor(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value draw(mrb_state *mrb, mrb_value self)
{
    mrb_int x, y;
    mrb_get_args(mrb, "ii", &x, &y);

    ofImage* obj = (ofImage*)DATA_PTR(self);
    obj->draw(x, y);

    return mrb_nil_value();
}

mrb_value draw_sub(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value height(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value width(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

}

//--------------------------------------------------------------------------------
void BindImage::Bind(mrb_state* mrb)
{
    struct RClass *cc = mrb_define_class(mrb, "Image", mrb->object_class);

    mrb_define_class_method(mrb , cc, "load",               load,               MRB_ARGS_REQ(1));
    mrb_define_class_method(mrb , cc, "grab_screen",        grab_screen,        MRB_ARGS_REQ(4));

    mrb_define_method(mrb, cc,        "save",               save,               MRB_ARGS_ARG(2, 1));
    mrb_define_method(mrb, cc,        "color",              color,              MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "set_color",          set_color,          MRB_ARGS_REQ(3));
    mrb_define_method(mrb, cc,        "resize",             resize,             MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "set_image_type",     set_image_type,     MRB_ARGS_REQ(1));
    mrb_define_method(mrb, cc,        "crop",               crop,               MRB_ARGS_REQ(5));
    mrb_define_method(mrb, cc,        "crop!",              crop_bang,          MRB_ARGS_REQ(4));
    mrb_define_method(mrb, cc,        "rotate90",           rotate90,           MRB_ARGS_REQ(1));
    mrb_define_method(mrb, cc,        "mirror",             mirror,             MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "update",             update,             MRB_ARGS_NONE());
    mrb_define_method(mrb, cc,        "set_anchor_percent", set_anchor_percent, MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "set_anchor_point",   set_anchor_point,   MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "reset_anchor",       reset_anchor,       MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "draw",               draw,               MRB_ARGS_ARG(2, 3));
    mrb_define_method(mrb, cc,        "draw_sub",           draw_sub,           MRB_ARGS_ARG(6, 2));
    mrb_define_method(mrb, cc,        "height",             height,             MRB_ARGS_NONE());
    mrb_define_method(mrb, cc,        "width",              width,              MRB_ARGS_NONE());
}

}
<commit_msg>Add obj(mrb_value self)<commit_after>#include "rubykokuban/BindImage.hpp"

#include "mruby.h"
#include "mruby/class.h"
#include "mruby/data.h"
#include "mruby/string.h"
#include "ofMain.h"

namespace rubykokuban {

namespace {
ofImage& obj(mrb_value self)
{
    return *((ofImage*)DATA_PTR(self));
}

void free(mrb_state *mrb, void *p)
{
    if (p) {
        delete ((ofImage*)p);
    }
}

struct mrb_data_type data_type = { "rubykokuban_image", free };

mrb_value load(mrb_state *mrb, mrb_value self)
{
    ofImage* obj = new ofImage();

    mrb_value str;
    mrb_get_args(mrb, "S", &str);

    string filename(mrb_string_value_ptr(mrb, str));
    obj->loadImage(filename);

    struct RData *data = mrb_data_object_alloc(mrb, mrb_class_ptr(self), obj, &data_type);
    return mrb_obj_value(data);
}

mrb_value grab_screen(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value save(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value color(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value set_color(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value resize(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value set_image_type(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value crop_bang(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value crop(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value rotate90(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value mirror(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value update(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value set_anchor_percent(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value set_anchor_point(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value reset_anchor(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value draw(mrb_state *mrb, mrb_value self)
{
    mrb_int x, y;
    mrb_get_args(mrb, "ii", &x, &y);

    obj(self).draw(x, y);

    return mrb_nil_value();
}

mrb_value draw_sub(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value height(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

mrb_value width(mrb_state *mrb, mrb_value self)
{
    return mrb_nil_value();
}

}

//--------------------------------------------------------------------------------
void BindImage::Bind(mrb_state* mrb)
{
    struct RClass *cc = mrb_define_class(mrb, "Image", mrb->object_class);

    mrb_define_class_method(mrb , cc, "load",               load,               MRB_ARGS_REQ(1));
    mrb_define_class_method(mrb , cc, "grab_screen",        grab_screen,        MRB_ARGS_REQ(4));

    mrb_define_method(mrb, cc,        "save",               save,               MRB_ARGS_ARG(2, 1));
    mrb_define_method(mrb, cc,        "color",              color,              MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "set_color",          set_color,          MRB_ARGS_REQ(3));
    mrb_define_method(mrb, cc,        "resize",             resize,             MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "set_image_type",     set_image_type,     MRB_ARGS_REQ(1));
    mrb_define_method(mrb, cc,        "crop",               crop,               MRB_ARGS_REQ(5));
    mrb_define_method(mrb, cc,        "crop!",              crop_bang,          MRB_ARGS_REQ(4));
    mrb_define_method(mrb, cc,        "rotate90",           rotate90,           MRB_ARGS_REQ(1));
    mrb_define_method(mrb, cc,        "mirror",             mirror,             MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "update",             update,             MRB_ARGS_NONE());
    mrb_define_method(mrb, cc,        "set_anchor_percent", set_anchor_percent, MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "set_anchor_point",   set_anchor_point,   MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "reset_anchor",       reset_anchor,       MRB_ARGS_REQ(2));
    mrb_define_method(mrb, cc,        "draw",               draw,               MRB_ARGS_ARG(2, 3));
    mrb_define_method(mrb, cc,        "draw_sub",           draw_sub,           MRB_ARGS_ARG(6, 2));
    mrb_define_method(mrb, cc,        "height",             height,             MRB_ARGS_NONE());
    mrb_define_method(mrb, cc,        "width",              width,              MRB_ARGS_NONE());
}

}
<|endoftext|>


<commit_before>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *
*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_INTERACTIONFORCEFIELD_PLANEFORCEFIELD_INL
#define SOFA_COMPONENT_INTERACTIONFORCEFIELD_PLANEFORCEFIELD_INL

#include <sofa/core/behavior/ForceField.inl>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/simulation/common/Simulation.h>
#include <sofa/component/forcefield/PlaneForceField.h>
#include <sofa/helper/system/config.h>
#include <sofa/helper/accessor.h>
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/helper/gl/template.h>
#include <assert.h>
#include <iostream>
#include <sofa/defaulttype/BoundingBox.h>
#include <limits>

namespace sofa
{

namespace component
{

namespace forcefield
{

template<class DataTypes>
void PlaneForceField<DataTypes>::setPlane(const Deriv& normal, Real d)
{
	DPos tmpN = DataTypes::getDPos(normal);
	Real n = tmpN.norm();
	planeNormal.setValue( tmpN / n);
	planeD.setValue( d / n );
}

template<class DataTypes>
void PlaneForceField<DataTypes>::addForce(const core::MechanicalParams* /* mparams */ /* PARAMS FIRST */, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v)
{
    sofa::helper::WriteAccessor< core::objectmodel::Data< VecDeriv > > f1 = f;
    sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > p1 = x;
    sofa::helper::ReadAccessor< core::objectmodel::Data< VecDeriv > > v1 = v;

    //this->dfdd.resize(p1.size());
    this->contacts.clear();
    f1.resize(p1.size());

    unsigned int ibegin = 0;
    unsigned int iend = p1.size();

    if (localRange.getValue()[0] >= 0)
        ibegin = localRange.getValue()[0];

    if (localRange.getValue()[1] >= 0 && (unsigned int)localRange.getValue()[1]+1 < iend)
        iend = localRange.getValue()[1]+1;

	Real limit = this->maxForce.getValue();
	limit *= limit; // squared

	Real stiff = this->stiffness.getValue();
	Real damp = this->damping.getValue();
	DPos planeN = planeNormal.getValue();

    for (unsigned int i=ibegin; i<iend; i++)
    {
        Real d = DataTypes::getCPos(p1[i])*planeN-planeD.getValue();
        if (bilateral.getValue() || d<0 )
        {
            //serr<<"PlaneForceField<DataTypes>::addForce, d = "<<d<<sendl;
            Real forceIntensity = -stiff*d;
            //serr<<"PlaneForceField<DataTypes>::addForce, stiffness = "<<stiffness.getValue()<<sendl;
            Real dampingIntensity = -damp*d;
            //serr<<"PlaneForceField<DataTypes>::addForce, dampingIntensity = "<<dampingIntensity<<sendl;
            DPos force = planeN*forceIntensity - DataTypes::getDPos(v1[i])*dampingIntensity;

			Real amplitude = force.norm2();
			if(limit && amplitude > limit)
				force *= sqrt(limit / amplitude);
            //serr<<"PlaneForceField<DataTypes>::addForce, force = "<<force<<sendl;
			Deriv tmpF;
			DataTypes::setDPos(tmpF, force);
            f1[i] += tmpF;
            //this->dfdd[i] = -this->stiffness;
            this->contacts.push_back(i);
        }
    }
}

template<class DataTypes>
void PlaneForceField<DataTypes>::addDForce(const core::MechanicalParams* mparams /* PARAMS FIRST */, DataVecDeriv& df, const DataVecDeriv& dx)
{
    sofa::helper::WriteAccessor< core::objectmodel::Data< VecDeriv > > df1 = df;
    sofa::helper::ReadAccessor< core::objectmodel::Data< VecDeriv > > dx1 = dx;

    df1.resize(dx1.size());
    const Real fact = (Real)(-this->stiffness.getValue() * mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()));
	DPos planeN = planeNormal.getValue();

    for (unsigned int i=0; i<this->contacts.size(); i++)
    {
        unsigned int p = this->contacts[i];
        assert(p<dx1.size());
        DataTypes::setDPos(df1[p], DataTypes::getDPos(df1[p]) + planeN * (fact * (DataTypes::getDPos(dx1[p]) * planeN)));
    }
}

template<class DataTypes>
void PlaneForceField<DataTypes>::addKToMatrix(const core::MechanicalParams* mparams /* PARAMS FIRST */, const sofa::core::behavior::MultiMatrixAccessor* matrix )
{
    const Real fact = (Real)(-this->stiffness.getValue()*mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()));
    Deriv normal;
	DataTypes::setDPos(normal, planeNormal.getValue());
    sofa::core::behavior::MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate);
    sofa::defaulttype::BaseMatrix* mat = mref.matrix;
    unsigned int offset = mref.offset;

    for (unsigned int i=0; i<this->contacts.size(); i++)
    {
        unsigned int p = this->contacts[i];
        for (int l=0; l<Deriv::total_size; ++l)
            for (int c=0; c<Deriv::total_size; ++c)
            {
                SReal coef = normal[l] * fact * normal[c];
                mat->add(offset + p*Deriv::total_size + l, offset + p*Deriv::total_size + c, coef);
            }
    }
}

template<class DataTypes>
void PlaneForceField<DataTypes>::updateStiffness( const VecCoord& vx )
{
    helper::ReadAccessor<VecCoord> x = vx;

    this->contacts.clear();

    unsigned int ibegin = 0;
    unsigned int iend = x.size();

    if (localRange.getValue()[0] >= 0)
        ibegin = localRange.getValue()[0];

    if (localRange.getValue()[1] >= 0 && (unsigned int)localRange.getValue()[1]+1 < iend)
        iend = localRange.getValue()[1]+1;

    for (unsigned int i=ibegin; i<iend; i++)
    {
        Real d = DataTypes::getCPos(x[i])*planeNormal.getValue()-planeD.getValue();
        if (d<0)
            this->contacts.push_back(i);
    }
}

// Rotate the plane. Note that the rotation is only applied on the 3 first coordinates
template<class DataTypes>
void PlaneForceField<DataTypes>::rotate( Deriv axe, Real angle )
{
    defaulttype::Vec3d axe3d(1,1,1); axe3d = DataTypes::getDPos(axe);
    defaulttype::Vec3d normal3d; normal3d = planeNormal.getValue();
    defaulttype::Vec3d v = normal3d.cross(axe3d);
    if (v.norm2() < 1.0e-10) return;
    v.normalize();
    v = normal3d * cos ( angle ) + v * sin ( angle );
    *planeNormal.beginEdit() = v;
    planeNormal.endEdit();
}

template<class DataTypes>
void PlaneForceField<DataTypes>::draw(const core::visual::VisualParams* vparams)
{
    if (!vparams->displayFlags().getShowForceFields()) return;
	//if (!vparams->isSupported(core::visual::API_OpenGL)) return;
    drawPlane(vparams);
}

template<class DataTypes>
void PlaneForceField<DataTypes>::drawPlane(const core::visual::VisualParams* vparams,float size)
{
    if (size == 0.0f) size = (float)drawSize.getValue();

    helper::ReadAccessor<VecCoord> p1 = *this->mstate->getX();

    defaulttype::Vec3d normal; normal = planeNormal.getValue();

    // find a first vector inside the plane
    defaulttype::Vec3d v1;
    if( 0.0 != normal[0] ) v1 = defaulttype::Vec3d(-normal[1]/normal[0], 1.0, 0.0);
    else if ( 0.0 != normal[1] ) v1 = defaulttype::Vec3d(1.0, -normal[0]/normal[1],0.0);
    else if ( 0.0 != normal[2] ) v1 = defaulttype::Vec3d(1.0, 0.0, -normal[0]/normal[2]);
    v1.normalize();
    // find a second vector inside the plane and orthogonal to the first
    defaulttype::Vec3d v2;
    v2 = v1.cross(normal);
    v2.normalize();

    defaulttype::Vec3d center = normal*planeD.getValue();
    defaulttype::Vec3d corners[4];
    corners[0] = center-v1*size-v2*size;
    corners[1] = center+v1*size-v2*size;
    corners[2] = center+v1*size+v2*size;
    corners[3] = center-v1*size+v2*size;

    std::vector< defaulttype::Vector3 > points;

    points.push_back(corners[0]);
    points.push_back(corners[1]);
    points.push_back(corners[2]);

    points.push_back(corners[0]);
    points.push_back(corners[2]);
    points.push_back(corners[3]);

    vparams->drawTool()->setPolygonMode(2,false); //Cull Front face

    vparams->drawTool()->drawTriangles(points, defaulttype::Vec<4,float>(color.getValue()[0],color.getValue()[1],color.getValue()[2],0.5));
    vparams->drawTool()->setPolygonMode(0,false); //No Culling

    std::vector< defaulttype::Vector3 > pointsLine;
    // lines for points penetrating the plane

    unsigned int ibegin = 0;
    unsigned int iend = p1.size();

    if (localRange.getValue()[0] >= 0)
        ibegin = localRange.getValue()[0];

    if (localRange.getValue()[1] >= 0 && (unsigned int)localRange.getValue()[1]+1 < iend)
        iend = localRange.getValue()[1]+1;

	defaulttype::Vector3 point1,point2;
    for (unsigned int i=ibegin; i<iend; i++)
    {
        Real d = DataTypes::getCPos(p1[i])*planeNormal.getValue()-planeD.getValue();
        CPos p2 = DataTypes::getCPos(p1[i]);
        p2 += planeNormal.getValue()*(-d);
        if (d<0)
        {
            point1 = DataTypes::getCPos(p1[i]);
            point2 = p2;
			pointsLine.push_back(point1);
			pointsLine.push_back(point2);
        }
    }
    vparams->drawTool()->drawLines(pointsLine, 1, defaulttype::Vec<4,float>(1,0,0,1));
}

template <class DataTypes>
void PlaneForceField<DataTypes>::computeBBox(const core::ExecParams * params)
{
    if (!bDraw.getValue()) return;

    const Real max_real = std::numeric_limits<Real>::max();
    const Real min_real = std::numeric_limits<Real>::min();
    Real maxBBox[3] = {min_real,min_real,min_real};
    Real minBBox[3] = {max_real,max_real,max_real};

    defaulttype::Vec3d normal; normal = planeNormal.getValue(params);
    double size=10.0;

    // find a first vector inside the plane
    defaulttype::Vec3d v1;
    if( 0.0 != normal[0] ) v1 = defaulttype::Vec3d(-normal[1]/normal[0], 1.0, 0.0);
    else if ( 0.0 != normal[1] ) v1 = defaulttype::Vec3d(1.0, -normal[0]/normal[1],0.0);
    else if ( 0.0 != normal[2] ) v1 = defaulttype::Vec3d(1.0, 0.0, -normal[0]/normal[2]);
    v1.normalize();
    // find a second vector inside the plane and orthogonal to the first
    defaulttype::Vec3d v2;
    v2 = v1.cross(normal);
    v2.normalize();

    defaulttype::Vec3d center = normal*planeD.getValue();
    defaulttype::Vec3d corners[4];
    corners[0] = center-v1*size-v2*size;
    corners[1] = center+v1*size-v2*size;
    corners[2] = center+v1*size+v2*size;
    corners[3] = center-v1*size+v2*size;

    for (unsigned int i=0; i<4; i++)
    {
        for (int c=0; c<3; c++)
        {
            if (corners[i][c] > maxBBox[c]) maxBBox[c] = (Real)corners[i][c];
            if (corners[i][c] < minBBox[c]) minBBox[c] = (Real)corners[i][c];
        }
    }
    this->f_bbox.setValue(params,sofa::defaulttype::TBoundingBox<Real>(minBBox,maxBBox));
}

} // namespace forcefield

} // namespace component

} // namespace sofa

#endif
<commit_msg>r11306/sofa : fix PlaneForceField visu: use draw boolean to draw or not<commit_after>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *
*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_INTERACTIONFORCEFIELD_PLANEFORCEFIELD_INL
#define SOFA_COMPONENT_INTERACTIONFORCEFIELD_PLANEFORCEFIELD_INL

#include <sofa/core/behavior/ForceField.inl>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/simulation/common/Simulation.h>
#include <sofa/component/forcefield/PlaneForceField.h>
#include <sofa/helper/system/config.h>
#include <sofa/helper/accessor.h>
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/helper/gl/template.h>
#include <assert.h>
#include <iostream>
#include <sofa/defaulttype/BoundingBox.h>
#include <limits>

namespace sofa
{

namespace component
{

namespace forcefield
{

template<class DataTypes>
void PlaneForceField<DataTypes>::setPlane(const Deriv& normal, Real d)
{
	DPos tmpN = DataTypes::getDPos(normal);
	Real n = tmpN.norm();
	planeNormal.setValue( tmpN / n);
	planeD.setValue( d / n );
}

template<class DataTypes>
void PlaneForceField<DataTypes>::addForce(const core::MechanicalParams* /* mparams */ /* PARAMS FIRST */, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& v)
{
    sofa::helper::WriteAccessor< core::objectmodel::Data< VecDeriv > > f1 = f;
    sofa::helper::ReadAccessor< core::objectmodel::Data< VecCoord > > p1 = x;
    sofa::helper::ReadAccessor< core::objectmodel::Data< VecDeriv > > v1 = v;

    //this->dfdd.resize(p1.size());
    this->contacts.clear();
    f1.resize(p1.size());

    unsigned int ibegin = 0;
    unsigned int iend = p1.size();

    if (localRange.getValue()[0] >= 0)
        ibegin = localRange.getValue()[0];

    if (localRange.getValue()[1] >= 0 && (unsigned int)localRange.getValue()[1]+1 < iend)
        iend = localRange.getValue()[1]+1;

	Real limit = this->maxForce.getValue();
	limit *= limit; // squared

	Real stiff = this->stiffness.getValue();
	Real damp = this->damping.getValue();
	DPos planeN = planeNormal.getValue();

    for (unsigned int i=ibegin; i<iend; i++)
    {
        Real d = DataTypes::getCPos(p1[i])*planeN-planeD.getValue();
        if (bilateral.getValue() || d<0 )
        {
            //serr<<"PlaneForceField<DataTypes>::addForce, d = "<<d<<sendl;
            Real forceIntensity = -stiff*d;
            //serr<<"PlaneForceField<DataTypes>::addForce, stiffness = "<<stiffness.getValue()<<sendl;
            Real dampingIntensity = -damp*d;
            //serr<<"PlaneForceField<DataTypes>::addForce, dampingIntensity = "<<dampingIntensity<<sendl;
            DPos force = planeN*forceIntensity - DataTypes::getDPos(v1[i])*dampingIntensity;

			Real amplitude = force.norm2();
			if(limit && amplitude > limit)
				force *= sqrt(limit / amplitude);
            //serr<<"PlaneForceField<DataTypes>::addForce, force = "<<force<<sendl;
			Deriv tmpF;
			DataTypes::setDPos(tmpF, force);
            f1[i] += tmpF;
            //this->dfdd[i] = -this->stiffness;
            this->contacts.push_back(i);
        }
    }
}

template<class DataTypes>
void PlaneForceField<DataTypes>::addDForce(const core::MechanicalParams* mparams /* PARAMS FIRST */, DataVecDeriv& df, const DataVecDeriv& dx)
{
    sofa::helper::WriteAccessor< core::objectmodel::Data< VecDeriv > > df1 = df;
    sofa::helper::ReadAccessor< core::objectmodel::Data< VecDeriv > > dx1 = dx;

    df1.resize(dx1.size());
    const Real fact = (Real)(-this->stiffness.getValue() * mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()));
	DPos planeN = planeNormal.getValue();

    for (unsigned int i=0; i<this->contacts.size(); i++)
    {
        unsigned int p = this->contacts[i];
        assert(p<dx1.size());
        DataTypes::setDPos(df1[p], DataTypes::getDPos(df1[p]) + planeN * (fact * (DataTypes::getDPos(dx1[p]) * planeN)));
    }
}

template<class DataTypes>
void PlaneForceField<DataTypes>::addKToMatrix(const core::MechanicalParams* mparams /* PARAMS FIRST */, const sofa::core::behavior::MultiMatrixAccessor* matrix )
{
    const Real fact = (Real)(-this->stiffness.getValue()*mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()));
    Deriv normal;
	DataTypes::setDPos(normal, planeNormal.getValue());
    sofa::core::behavior::MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate);
    sofa::defaulttype::BaseMatrix* mat = mref.matrix;
    unsigned int offset = mref.offset;

    for (unsigned int i=0; i<this->contacts.size(); i++)
    {
        unsigned int p = this->contacts[i];
        for (int l=0; l<Deriv::total_size; ++l)
            for (int c=0; c<Deriv::total_size; ++c)
            {
                SReal coef = normal[l] * fact * normal[c];
                mat->add(offset + p*Deriv::total_size + l, offset + p*Deriv::total_size + c, coef);
            }
    }
}

template<class DataTypes>
void PlaneForceField<DataTypes>::updateStiffness( const VecCoord& vx )
{
    helper::ReadAccessor<VecCoord> x = vx;

    this->contacts.clear();

    unsigned int ibegin = 0;
    unsigned int iend = x.size();

    if (localRange.getValue()[0] >= 0)
        ibegin = localRange.getValue()[0];

    if (localRange.getValue()[1] >= 0 && (unsigned int)localRange.getValue()[1]+1 < iend)
        iend = localRange.getValue()[1]+1;

    for (unsigned int i=ibegin; i<iend; i++)
    {
        Real d = DataTypes::getCPos(x[i])*planeNormal.getValue()-planeD.getValue();
        if (d<0)
            this->contacts.push_back(i);
    }
}

// Rotate the plane. Note that the rotation is only applied on the 3 first coordinates
template<class DataTypes>
void PlaneForceField<DataTypes>::rotate( Deriv axe, Real angle )
{
    defaulttype::Vec3d axe3d(1,1,1); axe3d = DataTypes::getDPos(axe);
    defaulttype::Vec3d normal3d; normal3d = planeNormal.getValue();
    defaulttype::Vec3d v = normal3d.cross(axe3d);
    if (v.norm2() < 1.0e-10) return;
    v.normalize();
    v = normal3d * cos ( angle ) + v * sin ( angle );
    *planeNormal.beginEdit() = v;
    planeNormal.endEdit();
}

template<class DataTypes>
void PlaneForceField<DataTypes>::draw(const core::visual::VisualParams* vparams)
{
    if (!vparams->displayFlags().getShowForceFields()) return;
	//if (!vparams->isSupported(core::visual::API_OpenGL)) return;
    if (bDraw.getValue()) drawPlane(vparams);
}

template<class DataTypes>
void PlaneForceField<DataTypes>::drawPlane(const core::visual::VisualParams* vparams,float size)
{
    if (size == 0.0f) size = (float)drawSize.getValue();

    helper::ReadAccessor<VecCoord> p1 = *this->mstate->getX();

    defaulttype::Vec3d normal; normal = planeNormal.getValue();

    // find a first vector inside the plane
    defaulttype::Vec3d v1;
    if( 0.0 != normal[0] ) v1 = defaulttype::Vec3d(-normal[1]/normal[0], 1.0, 0.0);
    else if ( 0.0 != normal[1] ) v1 = defaulttype::Vec3d(1.0, -normal[0]/normal[1],0.0);
    else if ( 0.0 != normal[2] ) v1 = defaulttype::Vec3d(1.0, 0.0, -normal[0]/normal[2]);
    v1.normalize();
    // find a second vector inside the plane and orthogonal to the first
    defaulttype::Vec3d v2;
    v2 = v1.cross(normal);
    v2.normalize();

    defaulttype::Vec3d center = normal*planeD.getValue();
    defaulttype::Vec3d corners[4];
    corners[0] = center-v1*size-v2*size;
    corners[1] = center+v1*size-v2*size;
    corners[2] = center+v1*size+v2*size;
    corners[3] = center-v1*size+v2*size;

    std::vector< defaulttype::Vector3 > points;

    points.push_back(corners[0]);
    points.push_back(corners[1]);
    points.push_back(corners[2]);

    points.push_back(corners[0]);
    points.push_back(corners[2]);
    points.push_back(corners[3]);

    vparams->drawTool()->setPolygonMode(2,false); //Cull Front face

    vparams->drawTool()->drawTriangles(points, defaulttype::Vec<4,float>(color.getValue()[0],color.getValue()[1],color.getValue()[2],0.5));
    vparams->drawTool()->setPolygonMode(0,false); //No Culling

    std::vector< defaulttype::Vector3 > pointsLine;
    // lines for points penetrating the plane

    unsigned int ibegin = 0;
    unsigned int iend = p1.size();

    if (localRange.getValue()[0] >= 0)
        ibegin = localRange.getValue()[0];

    if (localRange.getValue()[1] >= 0 && (unsigned int)localRange.getValue()[1]+1 < iend)
        iend = localRange.getValue()[1]+1;

	defaulttype::Vector3 point1,point2;
    for (unsigned int i=ibegin; i<iend; i++)
    {
        Real d = DataTypes::getCPos(p1[i])*planeNormal.getValue()-planeD.getValue();
        CPos p2 = DataTypes::getCPos(p1[i]);
        p2 += planeNormal.getValue()*(-d);
        if (d<0)
        {
            point1 = DataTypes::getCPos(p1[i]);
            point2 = p2;
			pointsLine.push_back(point1);
			pointsLine.push_back(point2);
        }
    }
    vparams->drawTool()->drawLines(pointsLine, 1, defaulttype::Vec<4,float>(1,0,0,1));
}

template <class DataTypes>
void PlaneForceField<DataTypes>::computeBBox(const core::ExecParams * params)
{
    if (!bDraw.getValue()) return;

    const Real max_real = std::numeric_limits<Real>::max();
    const Real min_real = std::numeric_limits<Real>::min();
    Real maxBBox[3] = {min_real,min_real,min_real};
    Real minBBox[3] = {max_real,max_real,max_real};

    defaulttype::Vec3d normal; normal = planeNormal.getValue(params);
    double size=10.0;

    // find a first vector inside the plane
    defaulttype::Vec3d v1;
    if( 0.0 != normal[0] ) v1 = defaulttype::Vec3d(-normal[1]/normal[0], 1.0, 0.0);
    else if ( 0.0 != normal[1] ) v1 = defaulttype::Vec3d(1.0, -normal[0]/normal[1],0.0);
    else if ( 0.0 != normal[2] ) v1 = defaulttype::Vec3d(1.0, 0.0, -normal[0]/normal[2]);
    v1.normalize();
    // find a second vector inside the plane and orthogonal to the first
    defaulttype::Vec3d v2;
    v2 = v1.cross(normal);
    v2.normalize();

    defaulttype::Vec3d center = normal*planeD.getValue();
    defaulttype::Vec3d corners[4];
    corners[0] = center-v1*size-v2*size;
    corners[1] = center+v1*size-v2*size;
    corners[2] = center+v1*size+v2*size;
    corners[3] = center-v1*size+v2*size;

    for (unsigned int i=0; i<4; i++)
    {
        for (int c=0; c<3; c++)
        {
            if (corners[i][c] > maxBBox[c]) maxBBox[c] = (Real)corners[i][c];
            if (corners[i][c] < minBBox[c]) minBBox[c] = (Real)corners[i][c];
        }
    }
    this->f_bbox.setValue(params,sofa::defaulttype::TBoundingBox<Real>(minBBox,maxBBox));
}

} // namespace forcefield

} // namespace component

} // namespace sofa

#endif
<|endoftext|>