bool FEMTetConstraint::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);
float currentVolume = -(1.0f / 6.0f) * (x4 - x1).dot((x3 - x1).cross(x2 - x1));
bool handleInversion = false;
if (currentVolume / m_volume < 0.2) // Only 20% of initial volume left
handleInversion = true;
Eigen::Vector3f corr1, corr2, corr3, corr4;
const bool res = PositionBasedDynamics::solve_FEMTetraConstraint(
x1, invMass1,
x2, invMass2,
x3, invMass3,
x4, invMass4,
m_volume,
m_invRestMat,
model.getSolidStiffness(),
model.getSolidPoissonRatio(), handleInversion,
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;
}
