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!"); } }