public override void CalculateMassInertia() { base.inertia = Matrix3.Zero; base.mass = 0.0f; for (int i = 0; i < Shapes.Length; i++) { Matrix3 currentInertia = Shapes[i].InverseOrientation * Shapes[i].Shape.Inertia * Shapes[i].Orientation; Vector3 p = Shapes[i].Position * -1.0f; float m = Shapes[i].Shape.Mass; currentInertia.M11 += m * (p.Y * p.Y + p.Z * p.Z); currentInertia.M22 += m * (p.X * p.X + p.Z * p.Z); currentInertia.M33 += m * (p.X * p.X + p.Y * p.Y); currentInertia.M12 += -p.X * p.Y * m; currentInertia.M21 += -p.X * p.Y * m; currentInertia.M31 += -p.X * p.Z * m; currentInertia.M13 += -p.X * p.Z * m; currentInertia.M32 += -p.Y * p.Z * m; currentInertia.M23 += -p.Y * p.Z * m; //base.inertia += currentInertia; Matrix3.Add(ref base.inertia, ref currentInertia, out base.inertia); base.mass += m; } }
public static void add() { var a = new Matrix3(_a); var b = new Matrix3(_b); var sum = a.Add(b); Assert.Equal(_sum, sum); Assert.Equal(_a, a); Assert.Equal(_b, b); }
/// <summary> /// Called once before iteration starts. /// </summary> /// <param name="timestep">The 5simulation timestep</param> public override void PrepareForIteration(float timestep) { effectiveMass = Matrix3.Add(body1.invInertiaWorld, body2.invInertiaWorld); softnessOverDt = softness / timestep; effectiveMass.M11 += softnessOverDt; effectiveMass.M22 += softnessOverDt; effectiveMass.M33 += softnessOverDt; Matrix3.Invert(ref effectiveMass, out effectiveMass); Matrix3 orientationDifference; Matrix3.Mult(ref initialOrientation1, ref initialOrientation2, out orientationDifference); Matrix3.Transpose(ref orientationDifference, out orientationDifference); Matrix3 q = orientationDifference * body2.invOrientation * body1.orientation; Vector3 axis; float x = q.M32 - q.M23; float y = q.M13 - q.M31; float z = q.M21 - q.M12; float r = JMath.Sqrt(x * x + y * y + z * z); float t = q.M11 + q.M22 + q.M33; float angle = (float)Math.Atan2(r, t - 1); axis = new Vector3(x, y, z) * angle; if (r != 0.0f) { axis = axis * (1.0f / r); } bias = axis * biasFactor * (-1.0f / timestep); // Apply previous frame solution as initial guess for satisfying the constraint. if (!body1.IsStatic) { body1.angularVelocity += Vector3.Transform(accumulatedImpulse, body1.invInertiaWorld); } if (!body2.IsStatic) { body2.angularVelocity += Vector3.Transform(-1.0f * accumulatedImpulse, body2.invInertiaWorld); } }
public void Random_NByN_Symetric(int n) { Rectangular rand = new Rectangular(); Matrix3 A = rand.Randomfloat(n, n); A = A.Add(Matrix3.Transpose(A)); /// Any matrix added to its transpose will be symetric Assert.That(StandardMatrixTests.IsSymetric(A), Is.True); EigenvalueDecomposition EofA = new EigenvalueDecomposition(A); Matrix3 V = EofA.V; // V is orthogonal V times V transpose is the identity Assert.That(Matrix3.Mult(V, Matrix3.Transpose(V)).ToFloatArray(), Is.EqualTo(Matrix3.Identity.ToFloatArray()).Within(.0000001)); Matrix3 D = EofA.getD(); Matrix3 test = Matrix3.Mult(D, Matrix3.Transpose(V)); test = Matrix3.Mult(V, test); Assert.That(test.ToFloatArray(), Is.EqualTo(A.ToFloatArray()).Within(.0000001).Percent); }
/// <summary> /// Calculates the inertia of the shape relative to the center of mass. /// </summary> /// <param name="shape"></param> /// <param name="centerOfMass"></param> /// <param name="inertia">Returns the inertia relative to the center of mass, not to the origin</param> /// <returns></returns> #region public static float CalculateMassInertia(Shape shape, out JVector centerOfMass, out JMatrix inertia) public static float CalculateMassInertia(Shape shape, out Vector3 centerOfMass, out Matrix3 inertia) { float mass = 0.0f; centerOfMass = Vector3.Zero; inertia = Matrix3.Zero; if (shape is Multishape) { throw new ArgumentException("Can't calculate inertia of multishapes.", "shape"); } // build a triangle hull around the shape List <Vector3> hullTriangles = new List <Vector3>(); shape.MakeHull(ref hullTriangles, 3); // create inertia of tetrahedron with vertices at // (0,0,0) (1,0,0) (0,1,0) (0,0,1) float a = 1.0f / 60.0f, b = 1.0f / 120.0f; Matrix3 C = new Matrix3(a, b, b, b, a, b, b, b, a); for (int i = 0; i < hullTriangles.Count; i += 3) { Vector3 column0 = hullTriangles[i + 0]; Vector3 column1 = hullTriangles[i + 1]; Vector3 column2 = hullTriangles[i + 2]; Matrix3 A = new Matrix3(column0.X, column1.X, column2.X, column0.Y, column1.Y, column2.Y, column0.Z, column1.Z, column2.Z); float detA = A.Determinant; // now transform this canonical tetrahedron to the target tetrahedron // inertia by a linear transformation A var acta = A * C * Matrix3.Transpose(A); Matrix3 tetrahedronInertia; Matrix3.Mult(ref acta, detA, out tetrahedronInertia); Vector3 tetrahedronCOM = (1.0f / 4.0f) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]); float tetrahedronMass = (1.0f / 6.0f) * detA; inertia = Matrix3.Add(inertia, tetrahedronInertia); centerOfMass += tetrahedronMass * tetrahedronCOM; mass += tetrahedronMass; } // Matrix3 tmpIn; Matrix3.Mult(ref Matrix3.Identity, inertia.Trace, out tmpIn); inertia = Matrix3.Sub(tmpIn, inertia); //inertia = Matrix3.Multiply(Matrix3.Identity, inertia.Trace) - inertia; centerOfMass = centerOfMass * (1.0f / mass); float x = centerOfMass.X; float y = centerOfMass.Y; float z = centerOfMass.Z; // now translate the inertia by the center of mass Matrix3 t = new Matrix3( -mass * (y * y + z * z), mass * x * y, mass * x * z, mass * y * x, -mass * (z * z + x * x), mass * y * z, mass * z * x, mass * z * y, -mass * (x * x + y * y)); Matrix3.Add(ref inertia, ref t, out inertia); return(mass); }