Example #1
0
    // 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;
        }
    }