/// <summary> /// Adds the two matrices together on a per-element basis. /// </summary> /// <param name="a">First matrix to add.</param> /// <param name="b">Second matrix to add.</param> /// <param name="result">Sum of the two matrices.</param> public static void Add(ref Matrix a, ref Matrix2X2 b, out Matrix2X2 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; }
/// <summary> /// Gets the mass matrix of the constraint. /// </summary> /// <param name="massMatrix">Constraint's mass matrix.</param> public void GetMassMatrix(out Matrix2X2 massMatrix) { massMatrix = effectiveMassMatrix; }
/// <summary> /// Gets the mass matrix of the revolute limit. /// The revolute limit is special; in terms of solving, it is /// actually sometimes TWO constraints; a minimum plane, and a /// maximum plane. The M11 field represents the minimum plane mass matrix /// and the M22 field represents the maximum plane mass matrix. /// </summary> /// <param name="massMatrix">Mass matrix of the constraint.</param> public void GetMassMatrix(out Matrix2X2 massMatrix) { massMatrix.M11 = velocityToImpulse.X; massMatrix.M22 = velocityToImpulse.Y; massMatrix.M12 = 0; massMatrix.M21 = 0; }
/// <summary> /// Computes the transposed matrix of a matrix. /// </summary> /// <param name="matrix">Matrix to transpose.</param> /// <param name="result">Transposed matrix.</param> public static void Transpose(ref Matrix2X2 matrix, out Matrix2X2 result) { float m21 = matrix.M12; result.M11 = matrix.M11; result.M12 = matrix.M21; result.M21 = m21; result.M22 = matrix.M22; }
/// <summary> /// Subtracts the two matrices from each other on a per-element basis. /// </summary> /// <param name="a">First matrix to subtract.</param> /// <param name="b">Second matrix to subtract.</param> /// <param name="result">Difference of the two matrices.</param> public static void Subtract(ref Matrix2X2 a, ref Matrix2X2 b, out Matrix2X2 result) { float m11 = a.M11 - b.M11; float m12 = a.M12 - b.M12; float m21 = a.M21 - b.M21; float m22 = a.M22 - b.M22; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; }
/// <summary> /// Transforms the vector by the matrix. /// </summary> /// <param name="v">Vector2 to transform.</param> /// <param name="matrix">Matrix to use as the transformation.</param> /// <param name="result">Product of the transformation.</param> public static void Transform(ref Vector2 v, ref Matrix2X2 matrix, out Vector2 result) { float vX = v.X; float vY = v.Y; #if !WINDOWS result = new Vector2(); #endif result.X = vX * matrix.M11 + vY * matrix.M21; result.Y = vX * matrix.M12 + vY * matrix.M22; }
/// <summary> /// Multiplies the two matrices. /// </summary> /// <param name="a">First matrix to multiply.</param> /// <param name="b">Second matrix to multiply.</param> /// <param name="result">Product of the multiplication.</param> public static void Multiply(ref Matrix2X3 a, ref Matrix3X2 b, out Matrix2X2 result) { result.M11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31; result.M12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32; result.M21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31; result.M22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32; }
/// <summary> /// Negates every element in the matrix. /// </summary> /// <param name="matrix">Matrix to negate.</param> /// <param name="result">Negated matrix.</param> public static void Negate(ref Matrix2X2 matrix, out Matrix2X2 result) { float m11 = -matrix.M11; float m12 = -matrix.M12; float m21 = -matrix.M21; float m22 = -matrix.M22; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; }
/// <summary> /// Inverts the given matix. /// </summary> /// <param name="matrix">Matrix to be inverted.</param> /// <param name="result">Inverted matrix.</param> public static void Invert(ref Matrix2X2 matrix, out Matrix2X2 result) { float determinantInverse = 1 / (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21); float m11 = matrix.M22 * determinantInverse; float m12 = -matrix.M12 * determinantInverse; float m21 = -matrix.M21 * determinantInverse; float m22 = matrix.M11 * determinantInverse; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; }
/// <summary> /// Multiplies the two matrices. /// </summary> /// <param name="a">First matrix to multiply.</param> /// <param name="b">Second matrix to multiply.</param> /// <param name="result">Product of the multiplication.</param> public static void Multiply(ref Matrix a, ref Matrix2X2 b, out Matrix2X2 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22; result.M11 = resultM11; result.M12 = resultM12; result.M21 = resultM21; result.M22 = resultM22; }
/// <summary> /// Constructs a uniform scaling matrix. /// </summary> /// <param name="scale">Value to use in the diagonal.</param> /// <param name="matrix">Scaling matrix.</param> public static void CreateScale(float scale, out Matrix2X2 matrix) { matrix.M11 = scale; matrix.M22 = scale; matrix.M12 = 0; matrix.M21 = 0; }
/// <summary> /// Gets the mass matrix of the constraint. /// </summary> /// <param name="massMatrix">Constraint's mass matrix.</param> public void GetMassMatrix(out Matrix2X2 massMatrix) { Matrix2X2.Negate(ref negativeEffectiveMassMatrix, out massMatrix); }
///<summary> /// Performs the frame's configuration step. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { contactCount = contactManifoldConstraint.penetrationConstraints.count; switch (contactCount) { case 1: manifoldCenter = contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position; break; case 2: Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position, ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position, out manifoldCenter); manifoldCenter.X *= .5f; manifoldCenter.Y *= .5f; manifoldCenter.Z *= .5f; break; case 3: Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position, ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position, out manifoldCenter); Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position, ref manifoldCenter, out manifoldCenter); manifoldCenter.X *= .333333333f; manifoldCenter.Y *= .333333333f; manifoldCenter.Z *= .333333333f; break; case 4: //This isn't actually the center of the manifold. Is it good enough? Sure seems like it. Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position, ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position, out manifoldCenter); Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position, ref manifoldCenter, out manifoldCenter); Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[3].contact.Position, ref manifoldCenter, out manifoldCenter); manifoldCenter.X *= .25f; manifoldCenter.Y *= .25f; manifoldCenter.Z *= .25f; break; default: manifoldCenter = Toolbox.NoVector; break; } //Compute the three dimensional relative velocity at the point. Vector3 velocityA, velocityB; if (entityA != null) { Vector3.Subtract(ref manifoldCenter, ref entityA.position, out ra); Vector3.Cross(ref entityA.angularVelocity, ref ra, out velocityA); Vector3.Add(ref velocityA, ref entityA.linearVelocity, out velocityA); } else velocityA = new Vector3(); if (entityB != null) { Vector3.Subtract(ref manifoldCenter, ref entityB.position, out rb); Vector3.Cross(ref entityB.angularVelocity, ref rb, out velocityB); Vector3.Add(ref velocityB, ref entityB.linearVelocity, out velocityB); } else velocityB = new Vector3(); Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity); //Get rid of the normal velocity. Vector3 normal = contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Normal; float normalVelocityScalar = normal.X * relativeVelocity.X + normal.Y * relativeVelocity.Y + normal.Z * relativeVelocity.Z; relativeVelocity.X -= normalVelocityScalar * normal.X; relativeVelocity.Y -= normalVelocityScalar * normal.Y; relativeVelocity.Z -= normalVelocityScalar * normal.Z; //Create the jacobian entry and decide the friction coefficient. float length = relativeVelocity.LengthSquared(); if (length > Toolbox.Epsilon) { length = (float)Math.Sqrt(length); float inverseLength = 1 / length; linearA.M11 = relativeVelocity.X * inverseLength; linearA.M12 = relativeVelocity.Y * inverseLength; linearA.M13 = relativeVelocity.Z * inverseLength; friction = length > CollisionResponseSettings.StaticFrictionVelocityThreshold ? contactManifoldConstraint.materialInteraction.KineticFriction : contactManifoldConstraint.materialInteraction.StaticFriction; } else { friction = contactManifoldConstraint.materialInteraction.StaticFriction; //If there was no velocity, try using the previous frame's jacobian... if it exists. //Reusing an old one is okay since jacobians are cleared when a contact is initialized. if (!(linearA.M11 != 0 || linearA.M12 != 0 || linearA.M13 != 0)) { //Otherwise, just redo it all. //Create arbitrary axes. Vector3 axis1; Vector3.Cross(ref normal, ref Toolbox.RightVector, out axis1); length = axis1.LengthSquared(); if (length > Toolbox.Epsilon) { length = (float)Math.Sqrt(length); float inverseLength = 1 / length; linearA.M11 = axis1.X * inverseLength; linearA.M12 = axis1.Y * inverseLength; linearA.M13 = axis1.Z * inverseLength; } else { Vector3.Cross(ref normal, ref Toolbox.UpVector, out axis1); axis1.Normalize(); linearA.M11 = axis1.X; linearA.M12 = axis1.Y; linearA.M13 = axis1.Z; } } } //Second axis is first axis x normal linearA.M21 = (linearA.M12 * normal.Z) - (linearA.M13 * normal.Y); linearA.M22 = (linearA.M13 * normal.X) - (linearA.M11 * normal.Z); linearA.M23 = (linearA.M11 * normal.Y) - (linearA.M12 * normal.X); //Compute angular jacobians if (entityA != null) { //angularA 1 = ra x linear axis 1 angularA.M11 = (ra.Y * linearA.M13) - (ra.Z * linearA.M12); angularA.M12 = (ra.Z * linearA.M11) - (ra.X * linearA.M13); angularA.M13 = (ra.X * linearA.M12) - (ra.Y * linearA.M11); //angularA 2 = ra x linear axis 2 angularA.M21 = (ra.Y * linearA.M23) - (ra.Z * linearA.M22); angularA.M22 = (ra.Z * linearA.M21) - (ra.X * linearA.M23); angularA.M23 = (ra.X * linearA.M22) - (ra.Y * linearA.M21); } //angularB 1 = linear axis 1 x rb if (entityB != null) { angularB.M11 = (linearA.M12 * rb.Z) - (linearA.M13 * rb.Y); angularB.M12 = (linearA.M13 * rb.X) - (linearA.M11 * rb.Z); angularB.M13 = (linearA.M11 * rb.Y) - (linearA.M12 * rb.X); //angularB 2 = linear axis 2 x rb angularB.M21 = (linearA.M22 * rb.Z) - (linearA.M23 * rb.Y); angularB.M22 = (linearA.M23 * rb.X) - (linearA.M21 * rb.Z); angularB.M23 = (linearA.M21 * rb.Y) - (linearA.M22 * rb.X); } //Compute inverse effective mass matrix Matrix2X2 entryA, entryB; //these are the transformed coordinates Matrix2X3 transform; Matrix3X2 transpose; if (entityADynamic) { Matrix2X3.Multiply(ref angularA, ref entityA.inertiaTensorInverse, out transform); Matrix2X3.Transpose(ref angularA, out transpose); Matrix2X2.Multiply(ref transform, ref transpose, out entryA); entryA.M11 += entityA.inverseMass; entryA.M22 += entityA.inverseMass; } else { entryA = new Matrix2X2(); } if (entityBDynamic) { Matrix2X3.Multiply(ref angularB, ref entityB.inertiaTensorInverse, out transform); Matrix2X3.Transpose(ref angularB, out transpose); Matrix2X2.Multiply(ref transform, ref transpose, out entryB); entryB.M11 += entityB.inverseMass; entryB.M22 += entityB.inverseMass; } else { entryB = new Matrix2X2(); } velocityToImpulse.M11 = -entryA.M11 - entryB.M11; velocityToImpulse.M12 = -entryA.M12 - entryB.M12; velocityToImpulse.M21 = -entryA.M21 - entryB.M21; velocityToImpulse.M22 = -entryA.M22 - entryB.M22; Matrix2X2.Invert(ref velocityToImpulse, out velocityToImpulse); }
private static float ComputeNorm(ref Matrix2X2 m) { //Would a square-based norm be faster and sufficient ? //Huge number of branches in this float norm = MathHelper.Max(Math.Abs(m.M11), Math.Abs(m.M12)); norm = MathHelper.Max(norm, Math.Abs(m.M21)); norm = MathHelper.Max(norm, Math.Abs(m.M22)); return norm; }
private bool Get2X2InverseMassMatrix(int indexA, int indexB, out Matrix2X2 massMatrix) { massMatrix.M11 = GetMassMatrixEntry(indexA, indexA); massMatrix.M12 = GetMassMatrixEntry(indexA, indexB); massMatrix.M21 = massMatrix.M12; // getMassMatrixEntry(indexB, indexA); massMatrix.M22 = GetMassMatrixEntry(indexB, indexB); return ComputeNorm(ref massMatrix) < ConditionNumberLimit; }