protected static void JacobiRotate(ref Matrix2x2 A, ref Matrix2x2 R)
{
// rotates A through phi in 01-plane to set A(0,1) = 0
// rotation stored in R whose columns are eigenvectors of A
double d = (A[0, 0] - A[1, 1]) / (2.0f * A[0, 1]);
double t = 1.0f / (Math.Abs(d) + Math.Sqrt(d * d + 1.0f));
if (d < 0.0f)
{
t = -t;
}
double c = 1.0f / Math.Sqrt(t * t + 1);
double s = t * c;
A[0, 0] += t * A[0, 1];
A[1, 1] -= t * A[0, 1];
A[0, 1] = A[1, 0] = 0.0f;
// store rotation in R
for (int k = 0; k < 2; k++)
{
double Rkp = c * R[k, 0] + s * R[k, 1];
double Rkq = -s * R[k, 0] + c * R[k, 1];
R[k, 0] = Rkp;
R[k, 1] = Rkq;
}
}
protected static void EigenDecomposition(ref Matrix2x2 A, ref Matrix2x2 R)
{
// only for symmetric matrices!
// A = R A' R^T, where A' is diagonal and R orthonormal
R = Matrix2x2.IDENTITY; // unit matrix
JacobiRotate(ref A, ref R);
}
static public Matrix2x2 operator *(Matrix2x2 a, Matrix2x2 b)
{
Matrix2x2 res = new Matrix2x2();
res[0, 0] = a[0, 0] * b[0, 0] + a[0, 1] * b[1, 0];
res[0, 1] = a[0, 0] * b[0, 1] + a[0, 1] * b[1, 1];
res[1, 0] = a[1, 0] * b[0, 0] + a[1, 1] * b[1, 0];
res[1, 1] = a[1, 0] * b[0, 1] + a[1, 1] * b[1, 1];
return(res);
}
public static Matrix2x2 MultiplyTransposedLeft(Matrix2x2 left, Matrix2x2 right)
{
Matrix2x2 res = new Matrix2x2();
res[0, 0] = left[0, 0] * right[0, 0] + left[1, 0] * right[1, 0];
res[0, 1] = left[0, 0] * right[0, 1] + left[1, 0] * right[1, 1];
res[1, 0] = left[0, 1] * right[0, 0] + left[1, 1] * right[1, 0];
res[1, 1] = left[0, 1] * right[0, 1] + left[1, 1] * right[1, 1];
return(res);
}
public override bool Equals(object obj)
{
if (obj is Matrix2x2)
{
Matrix2x2 v = (Matrix2x2)obj;
return(obj != null && this == v);
}
else
{
return(base.Equals(obj));
}
}
public Matrix2x2 ExtractRotation()
{
// A = RS, where S is symmetric and R is orthonormal
// -> S = (A^T A)^(1/2)
Matrix2x2 A = this;
Matrix2x2 S = new Matrix2x2();
Matrix2x2 R = Matrix2x2.IDENTITY; // default answer
// Here we use a closed-form evaluation of the decomposition for the 2D case
// In 3D, you'd have to use jacobi iteration to diagonalize a matrix
// See http://www.mpi-hd.mpg.de/personalhomes/jkopp/3x3/ for implementations of the 3D case
Matrix2x2 ATA;
ATA = Matrix2x2.MultiplyTransposedLeft(A, A);
Matrix2x2 U = new Matrix2x2();
R = Matrix2x2.IDENTITY;
EigenDecomposition(ref ATA, ref U);
double l0 = ATA[0, 0]; if (l0 <= 0.0f)
{
l0 = 0.0f;
}
else
{
l0 = 1.0f / Math.Sqrt(l0);
}
double l1 = ATA[1, 1]; if (l1 <= 0.0f)
{
l1 = 0.0f;
}
else
{
l1 = 1.0f / Math.Sqrt(l1);
}
Matrix2x2 S1 = new Matrix2x2();
S1[0, 0] = l0 * U[0, 0] * U[0, 0] + l1 * U[0, 1] * U[0, 1];
S1[0, 1] = l0 * U[0, 0] * U[1, 0] + l1 * U[0, 1] * U[1, 1];
S1[1, 0] = S1[0, 1];
S1[1, 1] = l0 * U[1, 0] * U[1, 0] + l1 * U[1, 1] * U[1, 1];
R = A * S1;
S = Matrix2x2.MultiplyTransposedLeft(R, A);
return(R);
}
public void ShapeMatch()
{
if (M == 0)
return;
// Calculate center of mass
Vector2 c = new Vector2();
foreach (Particle p in particles)
{
c += p.mass * p.x;
}
c /= M;
// Calculate A = Sum( m~ (xi - cr)(xi0 - cr0)^T ) - Eqn. 10
Matrix2x2 A = Matrix2x2.ZERO;
foreach (Particle p in particles)
{
A += p.mass * Matrix2x2.MultiplyWithTranspose(p.x - c, p.x0 - c0);
}
// Polar decompose
Matrix2x2 S = new Matrix2x2();
R = A.ExtractRotation();
if (double.IsNaN(R[0, 0]))
R = Matrix2x2.IDENTITY;
// Check for and fix inverted shape matching
if (R.Determinant() < 0)
R = R * -1;
// Calculate o, the remaining part of Tr
o = c + R * (-c0);
// Add our influence to the particles' goal positions
Vector2 sumAppliedForces = Vector2.ZERO;
foreach (Particle p in particles)
{
// Figure out the goal position according to this region, Tr * p.x0
Vector2 particleGoalPosition = o + R * p.x0;
p.goal = (1 - LsmBody.kChunkSmoothing) * p.goal + LsmBody.kChunkSmoothing * particleGoalPosition; // Blend
// For checking only
sumAppliedForces += p.mass * (particleGoalPosition - p.x);
}
// Error check
if (sumAppliedForces.Length() > 0.001)
{
Testbed.PostMessage(Color.Red, "Chunk's forces did not sum to zero!");
}
}
public static Matrix2x2 operator *(Matrix2x2 a, Matrix2x2 b)
{
Matrix2x2 res = new Matrix2x2();
res[0,0] = a[0,0]*b[0,0] + a[0,1]*b[1,0];
res[0,1] = a[0,0]*b[0,1] + a[0,1]*b[1,1];
res[1,0] = a[1,0]*b[0,0] + a[1,1]*b[1,0];
res[1,1] = a[1,0]*b[0,1] + a[1,1]*b[1,1];
return res;
}
protected static void JacobiRotate(ref Matrix2x2 A, ref Matrix2x2 R)
{
// rotates A through phi in 01-plane to set A(0,1) = 0
// rotation stored in R whose columns are eigenvectors of A
double d = (A[0,0] - A[1,1])/(2.0f*A[0,1]);
double t = 1.0f / (Math.Abs(d) + Math.Sqrt(d*d + 1.0f));
if (d < 0.0f) t = -t;
double c = 1.0f/Math.Sqrt(t*t + 1);
double s = t*c;
A[0,0] += t*A[0,1];
A[1,1] -= t*A[0,1];
A[0,1] = A[1,0] = 0.0f;
// store rotation in R
for (int k = 0; k < 2; k++) {
double Rkp = c*R[k,0] + s*R[k,1];
double Rkq =-s*R[k,0] + c*R[k,1];
R[k,0] = Rkp;
R[k,1] = Rkq;
}
}
protected static void EigenDecomposition(ref Matrix2x2 A, ref Matrix2x2 R)
{
// only for symmetric matrices!
// A = R A' R^T, where A' is diagonal and R orthonormal
R = Matrix2x2.IDENTITY; // unit matrix
JacobiRotate(ref A, ref R);
}
public Matrix2x2 ExtractRotation()
{
// A = RS, where S is symmetric and R is orthonormal
// -> S = (A^T A)^(1/2)
Matrix2x2 A = this;
Matrix2x2 S = new Matrix2x2();
Matrix2x2 R = Matrix2x2.IDENTITY; // default answer
// Here we use a closed-form evaluation of the decomposition for the 2D case
// In 3D, you'd have to use jacobi iteration to diagonalize a matrix
// See http://www.mpi-hd.mpg.de/personalhomes/jkopp/3x3/ for implementations of the 3D case
Matrix2x2 ATA;
ATA = Matrix2x2.MultiplyTransposedLeft(A, A);
Matrix2x2 U = new Matrix2x2();
R = Matrix2x2.IDENTITY;
EigenDecomposition(ref ATA, ref U);
double l0 = ATA[0,0]; if (l0 <= 0.0f) l0 = 0.0f; else l0 = 1.0f / Math.Sqrt(l0);
double l1 = ATA[1,1]; if (l1 <= 0.0f) l1 = 0.0f; else l1 = 1.0f / Math.Sqrt(l1);
Matrix2x2 S1 = new Matrix2x2();
S1[0,0] = l0*U[0,0]*U[0,0] + l1*U[0,1]*U[0,1];
S1[0,1] = l0*U[0,0]*U[1,0] + l1*U[0,1]*U[1,1];
S1[1,0] = S1[0,1];
S1[1,1] = l0*U[1,0]*U[1,0] + l1*U[1,1]*U[1,1];
R = A * S1;
S = Matrix2x2.MultiplyTransposedLeft(R, A);
return R;
}
public static Matrix2x2 MultiplyTransposedLeft(Matrix2x2 left, Matrix2x2 right)
{
Matrix2x2 res = new Matrix2x2();
res[0,0] = left[0,0]*right[0,0] + left[1,0]*right[1,0];
res[0,1] = left[0,0]*right[0,1] + left[1,0]*right[1,1];
res[1,0] = left[0,1]*right[0,0] + left[1,1]*right[1,0];
res[1,1] = left[0,1]*right[0,1] + left[1,1]*right[1,1];
return res;
}
// NOTE: This does NOT reuse sub-summations, and therefore is O(w^3) rather than O(1) as can be attained (see Readme.txt)
public void ShapeMatch()
{
if (M == 0)
{
return;
}
// Calculate center of mass
Vector2 c = new Vector2();
foreach (Particle p in particles)
{
c += p.PerRegionMass * p.x;
}
c /= M;
// Calculate A = Sum( m~ (xi - cr)(xi0 - cr0)^T ) - Eqn. 10
Matrix2x2 A = Matrix2x2.ZERO;
foreach (Particle p in particles)
{
A += p.PerRegionMass * Matrix2x2.MultiplyWithTranspose(p.x - c, p.x0 - c0);
}
// Polar decompose
Matrix2x2 S = new Matrix2x2();
R = A.ExtractRotation();
if (double.IsNaN(R[0, 0]))
{
R = Matrix2x2.IDENTITY;
}
// Check for and fix inverted shape matching
if (R.Determinant() < 0)
{
R = R * -1;
}
// Calculate o, the remaining part of Tr
o = c + R * (-c0);
// Add our influence to the particles' goal positions
Vector2 sumAppliedForces = Vector2.ZERO;
foreach (Particle p in particles)
{
// Figure out the goal position according to this region, Tr * p.x0
Vector2 particleGoalPosition = o + R * p.x0;
p.goal += p.PerRegionMass * particleGoalPosition;
p.R += p.PerRegionMass * R;
// For checking only
sumAppliedForces += p.PerRegionMass * (particleGoalPosition - p.x);
}
// Error check
if (sumAppliedForces.Length() > 0.001)
{
Testbed.PostMessage(Color.Red, "Shape matching region's forces did not sum to zero!");
}
}
