Skip to content

[SofaConstraint] Fix UncoupledConstraintCorrection topology change handling #1115

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
hugtalbot merged 4 commits into from
Aug 12, 2019
Merged

[SofaConstraint] Fix UncoupledConstraintCorrection topology change handling #1115

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
41c6c27
[SofaConstraint] Remove duplicate constructor used to only change the…
epernod Jul 27, 2019
ab959c7
[SofaConstraint] Fix UncoupledConstraintCorrection topology change ha…
epernod Jul 27, 2019
b595b56
[SofaConstraint] Clean UncoupledConstraintCorrection, remove deprecat…
epernod Jul 27, 2019
7816132
[SofaConstraint] remove commented method in UncoupledConstraintCorrec…
epernod Jul 31, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 1 addition & 16 deletions modules/SofaConstraint/UncoupledConstraintCorrection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,20 +42,6 @@ namespace constraintset

using namespace sofa::defaulttype;

template<>
SOFA_CONSTRAINT_API UncoupledConstraintCorrection< sofa::defaulttype::Rigid3Types >::UncoupledConstraintCorrection(sofa::core::behavior::MechanicalState<sofa::defaulttype::Rigid3Types> *mm)
: Inherit(mm)
, compliance(initData(&compliance, "compliance", "Rigid compliance value: 1st value for translations, 6 others for upper-triangular part of symmetric 3x3 rotation compliance matrix"))
, defaultCompliance(initData(&defaultCompliance, (Real)0.00001, "defaultCompliance", "Default compliance value for new dof or if all should have the same (in which case compliance vector should be empty)"))
, f_verbose( initData(&f_verbose,false,"verbose","Dump the constraint matrix at each iteration") )
, d_handleTopologyChange(initData(&d_handleTopologyChange, false, "handleTopologyChange", "Enable support of topological changes for compliance vector (should be disabled for rigids)"))
, d_correctionVelocityFactor(initData(&d_correctionVelocityFactor, (Real)1.0, "correctionVelocityFactor", "Factor applied to the constraint forces when correcting the velocities"))
, d_correctionPositionFactor(initData(&d_correctionPositionFactor, (Real)1.0, "correctionPositionFactor", "Factor applied to the constraint forces when correcting the positions"))
, d_useOdeSolverIntegrationFactors(initData(&d_useOdeSolverIntegrationFactors, false, "useOdeSolverIntegrationFactors", "Use odeSolver integration factors instead of correctionVelocityFactor and correctionPositionFactor"))
, m_pOdeSolver(NULL)
{
}

template<>
SOFA_CONSTRAINT_API void UncoupledConstraintCorrection< defaulttype::Rigid3Types >::init()
{
Expand Down Expand Up @@ -183,8 +169,7 @@ int UncoupledConstraintCorrectionClass = core::RegisterObject("Component computi
.add< UncoupledConstraintCorrection< Vec2Types > >()
.add< UncoupledConstraintCorrection< Vec3Types > >()
.add< UncoupledConstraintCorrection< Rigid3Types > >()

;
;

template class SOFA_CONSTRAINT_API UncoupledConstraintCorrection< Vec1Types >;
template class SOFA_CONSTRAINT_API UncoupledConstraintCorrection< Vec2Types >;
Expand Down
11 changes: 2 additions & 9 deletions modules/SofaConstraint/UncoupledConstraintCorrection.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@

#include <sofa/core/behavior/ConstraintCorrection.h>
#include <sofa/core/behavior/OdeSolver.h>

#include <SofaBaseTopology/TopologyData.h>

namespace sofa
{
Expand Down Expand Up @@ -69,9 +69,6 @@ class UncoupledConstraintCorrection : public sofa::core::behavior::ConstraintCor

void reinit() override;

/// Handle Topological Changes.
void handleTopologyChange() override;

void addComplianceInConstraintSpace(const sofa::core::ConstraintParams *cparams, sofa::defaulttype::BaseMatrix *W) override;

void getComplianceMatrix(defaulttype::BaseMatrix* ) const override;
Expand Down Expand Up @@ -119,14 +116,12 @@ class UncoupledConstraintCorrection : public sofa::core::behavior::ConstraintCor

/// @}

Data< VecReal > compliance; ///< Rigid compliance value: 1st value for translations, 6 others for upper-triangular part of symmetric 3x3 rotation compliance matrix
topology::PointData< VecReal > compliance; ///< Rigid compliance value: 1st value for translations, 6 others for upper-triangular part of symmetric 3x3 rotation compliance matrix

Data< Real > defaultCompliance; ///< Default compliance value for new dof or if all should have the same (in which case compliance vector should be empty)

Data<bool> f_verbose; ///< Dump the constraint matrix at each iteration

Data < bool > d_handleTopologyChange; ///< Enable support of topological changes for compliance vector (disable if another component takes care of this)

Data< Real > d_correctionVelocityFactor; ///< Factor applied to the constraint forces when correcting the velocities
Data< Real > d_correctionPositionFactor; ///< Factor applied to the constraint forces when correcting the positions

Expand All @@ -147,8 +142,6 @@ class UncoupledConstraintCorrection : public sofa::core::behavior::ConstraintCor
void computeDx(const Data< VecDeriv > &f, VecDeriv& x);
};

template<>
UncoupledConstraintCorrection< sofa::defaulttype::Rigid3Types >::UncoupledConstraintCorrection(sofa::core::behavior::MechanicalState<sofa::defaulttype::Rigid3Types> *mm);

template<>
void UncoupledConstraintCorrection< sofa::defaulttype::Rigid3Types >::init();
Expand Down
98 changes: 18 additions & 80 deletions modules/SofaConstraint/UncoupledConstraintCorrection.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@

#include <sofa/core/topology/TopologyChange.h>
#include <SofaBaseTopology/PointSetTopologyContainer.h>

#include <SofaBaseTopology/TopologyData.inl>

namespace sofa
{
Expand Down Expand Up @@ -109,11 +109,10 @@ inline sofa::defaulttype::RigidDeriv<3, Real> UncoupledConstraintCorrection_comp

template<class DataTypes>
UncoupledConstraintCorrection<DataTypes>::UncoupledConstraintCorrection(sofa::core::behavior::MechanicalState<DataTypes> *mm)
: Inherit(mm)
, compliance(initData(&compliance, "compliance", "compliance value on each dof"))
: Inherit(mm)
, compliance(initData(&compliance, "compliance", "compliance value on each dof. If Rigid compliance (7 values): 1st value for translations, 6 others for upper-triangular part of symmetric 3x3 rotation compliance matrix"))
, defaultCompliance(initData(&defaultCompliance, (Real)0.00001, "defaultCompliance", "Default compliance value for new dof or if all should have the same (in which case compliance vector should be empty)"))
, f_verbose( initData(&f_verbose,false,"verbose","Dump the constraint matrix at each iteration") )
, d_handleTopologyChange(initData(&d_handleTopologyChange, true, "handleTopologyChange", "Enable support of topological changes for compliance vector (disable if another component takes care of this)"))
, d_correctionVelocityFactor(initData(&d_correctionVelocityFactor, (Real)1.0, "correctionVelocityFactor", "Factor applied to the constraint forces when correcting the velocities"))
, d_correctionPositionFactor(initData(&d_correctionPositionFactor, (Real)1.0, "correctionPositionFactor", "Factor applied to the constraint forces when correcting the positions"))
, d_useOdeSolverIntegrationFactors(initData(&d_useOdeSolverIntegrationFactors, false, "useOdeSolverIntegrationFactors", "Use odeSolver integration factors instead of correctionVelocityFactor and correctionPositionFactor"))
Expand Down Expand Up @@ -146,9 +145,10 @@ void UncoupledConstraintCorrection<DataTypes>::init()
if (!defaultCompliance.isSet() && !comp.empty())
defaultCompliance.setValue(comp[0]); // set default compliance to first one in case it was given in the compliance vector
if (comp.size() > 1)
serr << "Warning compliance size ( " << comp.size() << " is not equal to the size of the mstate (" << x.size() << ")" << sendl;
msg_warning() << "Compliance size ( " << comp.size() << " is not equal to the size of the mstate (" << x.size() << ")";
Real comp0 = (!comp.empty()) ? comp[0] : defaultCompliance.getValue();
sout << "Using " << comp0 << " as initial compliance" << sendl;
msg_warning() << "Using " << comp0 << " as initial compliance";

VecReal UsedComp;
for (unsigned int i=0; i<x.size(); i++)
{
Expand All @@ -158,15 +158,23 @@ void UncoupledConstraintCorrection<DataTypes>::init()
if (!UsedComp.empty())
{
compliance.setValue(UsedComp);

// If compliance is a vector of value per dof, need to register it as a PointData to the current topology
sofa::core::topology::BaseMeshTopology* _topology = this->getContext()->getMeshTopology();
if (_topology != nullptr)
{
compliance.createTopologicalEngine(_topology);
compliance.registerTopologicalData();
}
}
}

this->getContext()->get(m_pOdeSolver);
if (!m_pOdeSolver)
{
if (d_useOdeSolverIntegrationFactors.getValue() == true)
{
serr << "Can't find any odeSolver" << sendl;
msg_error() << "Can't find any odeSolver";
d_useOdeSolverIntegrationFactors.setValue(false);
}
d_useOdeSolverIntegrationFactors.setReadOnly(true);
Expand All @@ -179,76 +187,6 @@ void UncoupledConstraintCorrection<DataTypes>::reinit()
Inherit::reinit();
}

template< class DataTypes >
void UncoupledConstraintCorrection< DataTypes >::handleTopologyChange()
{
using sofa::core::topology::TopologyChange;
using sofa::core::topology::TopologyChangeType;
using sofa::core::topology::BaseMeshTopology;

if (!d_handleTopologyChange.getValue())
return; // another component takes care of updating compliance vector

BaseMeshTopology *topology = this->getContext()->getMeshTopology();
if (!topology)
return;
if (defaultCompliance.isSet() && compliance.getValue().empty())
return; // uniform compliance, no need to update it

std::list< const TopologyChange * >::const_iterator itBegin = topology->beginChange();
std::list< const TopologyChange * >::const_iterator itEnd = topology->endChange();

VecReal& comp = *(compliance.beginEdit());
const Real comp0 = defaultCompliance.getValue();

for (std::list< const TopologyChange * >::const_iterator changeIt = itBegin; changeIt != itEnd; ++changeIt)
{
const TopologyChangeType changeType = (*changeIt)->getChangeType();

switch ( changeType )
{
case core::topology::POINTSADDED :
{
unsigned int nbPoints = (static_cast< const sofa::core::topology::PointsAdded *> (*changeIt))->getNbAddedVertices();

VecReal addedCompliance;

for (unsigned int i = 0; i < nbPoints; i++)
{
addedCompliance.push_back(comp0);
}

comp.insert(comp.end(), addedCompliance.begin(), addedCompliance.end());

break;
}
case core::topology::POINTSREMOVED :
{
using sofa::helper::vector;

const vector< unsigned int > &pts = (static_cast< const sofa::core::topology::PointsRemoved * >(*changeIt))->getArray();

unsigned int lastIndexVec = comp.size() - 1;

for (unsigned int i = 0; i < pts.size(); i++)
{
comp[pts[i]] = comp[lastIndexVec];
lastIndexVec--;
}

comp.resize(comp.size() - pts.size());

break;
}
default:
break;
}
}

compliance.endEdit();
}


template<class DataTypes>
void UncoupledConstraintCorrection<DataTypes>::getComplianceWithConstraintMerge(defaulttype::BaseMatrix* Wmerged, std::vector<int> &constraint_merge)
{
Expand Down Expand Up @@ -359,7 +297,7 @@ void UncoupledConstraintCorrection<DataTypes>::addComplianceInConstraintSpace(co

if (verbose)
{
sout << "C[" << indexCurRowConst << "]";
dmsg_info() << "C[" << indexCurRowConst << "]";
}

const MatrixDerivColConstIterator colItBegin = rowIt.begin();
Expand All @@ -376,7 +314,7 @@ void UncoupledConstraintCorrection<DataTypes>::addComplianceInConstraintSpace(co

if (verbose)
{
sout << " dof[" << dof << "]=" << n;
dmsg_info() << " dof[" << dof << "]=" << n;
}

//w += (n * n) * (dof < comp.size() ? comp[dof] : comp0);
Expand Down