diff --git a/src/ConstraintGeometry/constraint/ConstraintUnilateral.h b/src/ConstraintGeometry/constraint/ConstraintUnilateral.h index def887c..5554099 100644 --- a/src/ConstraintGeometry/constraint/ConstraintUnilateral.h +++ b/src/ConstraintGeometry/constraint/ConstraintUnilateral.h @@ -18,6 +18,7 @@ class SOFA_CONSTRAINTGEOMETRY_API ConstraintUnilateral : public TBaseConstraint< SOFA_CLASS(ConstraintUnilateral , SOFA_TEMPLATE2(TBaseConstraint,collisionalgorithm::BaseProximity,collisionalgorithm::BaseProximity) ); Data d_friction; + Data d_contactDistance; Data d_maxforce0; Data d_maxforce1; Data d_maxforce2; @@ -26,11 +27,23 @@ class SOFA_CONSTRAINTGEOMETRY_API ConstraintUnilateral : public TBaseConstraint< ConstraintUnilateral() : d_friction(initData(&d_friction, 0.0, "mu", "Friction")) + , d_contactDistance(initData(&d_contactDistance, 0.0, "contactDistance", "Standoff distance kept between the proximities in contact")) , d_maxforce0(initData(&d_maxforce0, std::numeric_limits::max(), "maxForce0", "Max force applied on the first axis")) , d_maxforce1(initData(&d_maxforce1, std::numeric_limits::max(), "maxForce1", "Max force applied on the second axis")) , d_maxforce2(initData(&d_maxforce2, std::numeric_limits::max(), "maxForce2", "Max force applied on the third axis")) , l_directions(initLink("directions", "link to the default direction")) {} + void getConstraintViolation(const core::ConstraintParams* p, + sofa::linearalgebra::BaseVector* v, unsigned cid) override + { + TBaseConstraint::getConstraintViolation(p, v, cid); + // Shift violation by -contactDistance so that the constraint is maintained at a distance. + // ic->id() is the normal row. + const double cd = d_contactDistance.getValue(); + for (const auto& ic : d_container.getValue()) + v->add(ic->id(), -cd); + } + ConstraintNormal createConstraintNormal(const BaseProximity::SPtr & first, const BaseProximity::SPtr & second) const override { if (l_directions==NULL) return ConstraintNormal();