Ejemplo n.º 1
0
        ///<summary>
        /// Performs the frame's configuration step.
        ///</summary>
        ///<param name="dt">Timestep duration.</param>
        public override void Update(float dt)
        {
            entityADynamic = entityA != null && entityA.isDynamic;
            entityBDynamic = entityB != null && entityB.isDynamic;

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