Esempio n. 1
0
        /// <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;
        }
Esempio n. 2
0
 /// <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;
 }
Esempio n. 3
0
 /// <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;
 }
Esempio n. 4
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;
        }
Esempio n. 5
0
        /// <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;
        }
Esempio n. 6
0
        /// <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;
        }
Esempio n. 7
0
        /// <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;
        }
Esempio n. 8
0
        /// <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;
        }
Esempio n. 9
0
        /// <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;
        }
Esempio n. 10
0
        /// <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;
        }
Esempio n. 11
0
        /// <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;
        }
Esempio n. 12
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);


        }
Esempio n. 14
0
        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;
        }
Esempio n. 15
0
        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;
        }