bool DistanceConstraint::solvePositionConstraint(SimulationModel &model)
{
ParticleData &pd = model.getParticles();
const unsigned i1 = m_bodies[0];
const unsigned i2 = m_bodies[1];
Eigen::Vector3f &x1 = pd.getPosition(i1);
Eigen::Vector3f &x2 = pd.getPosition(i2);
const float invMass1 = pd.getInvMass(i1);
const float invMass2 = pd.getInvMass(i2);
Eigen::Vector3f corr1, corr2;
const bool res = PositionBasedDynamics::solve_DistanceConstraint(
x1, invMass1, x2, invMass2,
m_restLength, model.getClothStiffness(), model.getClothStiffness(), corr1, corr2);
if (res)
{
if (invMass1 != 0.0f)
x1 += corr1;
if (invMass2 != 0.0f)
x2 += corr2;
}
return res;
}
bool GenericDistanceConstraint::solvePositionConstraint(SimulationModel &model)
{
ParticleData &pd = model.getParticles();
const unsigned i1 = m_bodies[0];
const unsigned i2 = m_bodies[1];
Eigen::Vector3f &x1 = pd.getPosition(i1);
Eigen::Vector3f &x2 = pd.getPosition(i2);
const float invMass1 = pd.getInvMass(i1);
const float invMass2 = pd.getInvMass(i2);
const float invMass[2] = { invMass1, invMass2 };
const Eigen::Vector3f x[2] = { x1, x2 };
Eigen::Vector3f corr[2];
const bool res = PositionBasedGenericConstraints::solve_GenericConstraint<2, 1>(
invMass, x, &m_restLength,
GenericDistanceConstraint::constraintFct,
GenericDistanceConstraint::gradientFct,
corr);
if (res)
{
const float stiffness = model.getClothStiffness();
if (invMass1 != 0.0f)
x1 += stiffness * corr[0];
if (invMass2 != 0.0f)
x2 += stiffness * corr[1];
}
return res;
}
