// Simulation
public void ShapeMatch()
{
if (IsDirty)
{
CalculateInvariants();
IsDirty = false;
}
for (int i = 0; i != mParticles.Count; ++i)
{
SmParticle particle = mParticles[i];
particle.mSumData.mV = particle.PerRegionMass * particle.mX;
particle.mSumData.mM = Matrix3x3.MultiplyWithTranspose(particle.PerRegionMass * particle.mX, particle.mX0);
}
SumParticlesToRegions();
for (int i = 0; i != mRegions.Count; ++i)
{
SmRegion r = mRegions[i];
Vector3 Fmixi = r.mSumData.mV;
Matrix3x3 Fmixi0T = r.mSumData.mM;
r.mC = (1 / r.mM) * Fmixi; //9
r.mA = Fmixi0T - Matrix3x3.MultiplyWithTranspose(r.mM * r.mC, r.mC0);
Matrix3x3 S = r.mA.transpose * r.mA;
S = r.mEigenVectors.transpose * S * r.mEigenVectors;
float[] eigenValues = new float[3];
Jacobi.jacobi(3, ref S, ref eigenValues, ref r.mEigenVectors);
for (int j = 0; j != 3; ++j)
{
if (eigenValues[j] <= 0.0f)
{
eigenValues[j] = 0.05f;
}
eigenValues[j] = 1.0f / Mathf.Sqrt(eigenValues[j]);
}
Matrix3x3 DPrime = new Matrix3x3(eigenValues[0], 0, 0, 0, eigenValues[1], 0, 0, 0, eigenValues[2]);
S = r.mEigenVectors * DPrime * r.mEigenVectors.transpose;
r.mR = r.mA * S;
if (r.mR.determinant < 0)
{
r.mR *= (-1.0f);
}
r.mT = r.mC - r.mR * r.mC0;
r.mSumData.mM = r.mR;
r.mSumData.mV = r.mT;
}
SumRegionsToParticles();
for (int i = 0; i != mParticles.Count; ++i)
{
SmParticle particle = mParticles[i];
float invNumParentRegions = 1.0f / particle.mParentRegions.Count;
particle.mG = (invNumParentRegions * particle.mSumData.mM) * particle.mX0 + invNumParentRegions * particle.mSumData.mV;
particle.mR = invNumParentRegions * particle.mSumData.mM;
}
}
