bool ShapeMatchingConstraint::solvePositionConstraint(SimulationModel &model)
{
ParticleData &pd = model.getParticles();
for (unsigned int i = 0; i < m_numberOfBodies; i++)
{
m_x[i] = pd.getPosition(m_bodies[i]);
}
const bool res = PositionBasedDynamics::solve_ShapeMatchingConstraint(
m_x0, m_x, m_w, m_numberOfBodies,
m_restCm, m_invRestMat,
model.getSolidStiffness(), false,
m_corr);
if (res)
{
for (unsigned int i = 0; i < m_numberOfBodies; i++)
{
// Important: Divide position correction by the number of clusters
// which contain the vertex.
if (m_w[i] != 0.0f)
pd.getPosition(m_bodies[i]) += (1.0f / m_numClusters[i]) * m_corr[i];
}
}
return res;
}
bool StrainTetConstraint::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 stiffness(model.getSolidStiffness(), model.getSolidStiffness(), model.getSolidStiffness());
Eigen::Vector3f corr1, corr2, corr3, corr4;
const bool res = PositionBasedDynamics::solve_StrainTetraConstraint(
x1, invMass1,
x2, invMass2,
x3, invMass3,
x4, invMass4,
m_invRestMat,
stiffness,
stiffness,
model.getSolidNormalizeStretch(),
model.getSolidNormalizeShear(),
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 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;
}
bool VolumeConstraint::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_VolumeConstraint(x1, invMass1,
x2, invMass2,
x3, invMass3,
x4, invMass4,
m_restVolume,
model.getSolidStiffness(),
model.getSolidStiffness(),
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;
}
