bool GenericIsometricBendingConstraint::solvePositionConstraint(SimulationModel &model)
{
ParticleData &pd = model.getParticles();
const unsigned i0 = m_bodies[0];
const unsigned i1 = m_bodies[1];
const unsigned i2 = m_bodies[2];
const unsigned i3 = m_bodies[3];
Eigen::Vector3f &x0 = pd.getPosition(i0);
Eigen::Vector3f &x1 = pd.getPosition(i1);
Eigen::Vector3f &x2 = pd.getPosition(i2);
Eigen::Vector3f &x3 = pd.getPosition(i3);
const float invMass0 = pd.getInvMass(i0);
const float invMass1 = pd.getInvMass(i1);
const float invMass2 = pd.getInvMass(i2);
const float invMass3 = pd.getInvMass(i3);
float invMass[4] = { invMass0, invMass1, invMass2, invMass3 };
const Eigen::Vector3f x[4] = { x0, x1, x2, x3 };
Eigen::Vector3f corr[4];
const bool res = PositionBasedGenericConstraints::solve_GenericConstraint<4, 1>(
invMass, x, &m_Q,
GenericIsometricBendingConstraint::constraintFct,
//GenericIsometricBendingConstraint::gradientFct,
corr);
if (res)
{
const float stiffness = model.getClothBendingStiffness();
if (invMass0 != 0.0f)
x0 += stiffness*corr[0];
if (invMass1 != 0.0f)
x1 += stiffness*corr[1];
if (invMass2 != 0.0f)
x2 += stiffness*corr[2];
if (invMass3 != 0.0f)
x3 += stiffness*corr[3];
}
return res;
}
bool DihedralConstraint::solvePositionConstraint(SimulationModel &model)
{
ParticleData &pd = model.getParticles();
const unsigned i1 = m_bodies[0];
const unsigned i2 = m_bodies[1];
const unsigned i3 = m_bodies[2];
const unsigned i4 = m_bodies[3];
Eigen::Vector3f &x1 = pd.getPosition(i1);
Eigen::Vector3f &x2 = pd.getPosition(i2);
Eigen::Vector3f &x3 = pd.getPosition(i3);
Eigen::Vector3f &x4 = pd.getPosition(i4);
const float invMass1 = pd.getInvMass(i1);
const float invMass2 = pd.getInvMass(i2);
const float invMass3 = pd.getInvMass(i3);
const float invMass4 = pd.getInvMass(i4);
Eigen::Vector3f corr1, corr2, corr3, corr4;
const bool res = PositionBasedDynamics::solve_DihedralConstraint(
x1, invMass1, x2, invMass2, x3, invMass3, x4, invMass4,
m_restAngle,
model.getClothBendingStiffness(),
corr1, corr2, corr3, corr4);
if (res)
{
if (invMass1 != 0.0f)
x1 += corr1;
if (invMass2 != 0.0f)
x2 += corr2;
if (invMass3 != 0.0f)
x3 += corr3;
if (invMass4 != 0.0f)
x4 += corr4;
}
return res;
}
bool IsometricBendingConstraint::solvePositionConstraint(SimulationModel &model)
{
ParticleData &pd = model.getParticles();
const unsigned i1 = m_bodies[0];
const unsigned i2 = m_bodies[1];
const unsigned i3 = m_bodies[2];
const unsigned i4 = m_bodies[3];
Vector3r &x1 = pd.getPosition(i1);
Vector3r &x2 = pd.getPosition(i2);
Vector3r &x3 = pd.getPosition(i3);
Vector3r &x4 = pd.getPosition(i4);
const Real invMass1 = pd.getInvMass(i1);
const Real invMass2 = pd.getInvMass(i2);
const Real invMass3 = pd.getInvMass(i3);
const Real invMass4 = pd.getInvMass(i4);
Vector3r corr1, corr2, corr3, corr4;
const bool res = PositionBasedDynamics::solve_IsometricBendingConstraint(
x1, invMass1, x2, invMass2, x3, invMass3, x4, invMass4,
m_Q,
model.getClothBendingStiffness(),
corr1, corr2, corr3, corr4);
if (res)
{
if (invMass1 != 0.0)
x1 += corr1;
if (invMass2 != 0.0)
x2 += corr2;
if (invMass3 != 0.0)
x3 += corr3;
if (invMass4 != 0.0)
x4 += corr4;
}
return res;
}
