3 row, 3 column matrix.
        ///<summary>
        /// Constructs a new transformable shape.
        ///</summary>
        ///<param name="shape">Base shape to transform.</param>
        ///<param name="transform">Transform to use.</param>
        public TransformableShape(ConvexShape shape, Matrix3x3 transform)
        {
            this.shape = shape;
            this.transform = transform;

            UpdateConvexShapeInfo();
        }
        ///<summary>
        /// Constructs a new transformable shape.
        ///</summary>
        /// <param name="shape">Base shape to transform.</param>
        /// <param name="transform">Transform to use.</param>
        /// <param name="description">Cached information about the shape. Assumed to be correct; no extra processing or validation is performed.</param>
        public TransformableShape(ConvexShape shape, Matrix3x3 transform, ConvexShapeDescription description)
        {
            this.shape = shape;
            this.transform = transform;

            UpdateConvexShapeInfo(description);
        }
        protected internal override void UpdateJacobiansAndVelocityBias()
        {
 

            linearJacobian = new Matrix3x3();

            Vector3 boneAxis;
            Quaternion.Transform(ref BoneLocalAxis, ref TargetBone.Orientation, out boneAxis);

            Vector3 jacobian;
            Vector3.Cross(ref boneAxis, ref PlaneNormal, out jacobian);

            angularJacobian = new Matrix3x3
            {
                M11 = jacobian.X,
                M12 = jacobian.Y,
                M13 = jacobian.Z,
            };


            Vector3.Dot(ref boneAxis, ref PlaneNormal, out velocityBias.X);
            velocityBias.X = -errorCorrectionFactor * velocityBias.X;


        }
        protected internal override void UpdateJacobiansAndVelocityBias()
        {
            linearJacobianA = linearJacobianB = new Matrix3x3();
            angularJacobianA = new Matrix3x3 { M11 = 1, M22 = 1, M33 = 1 };
            angularJacobianB = new Matrix3x3 { M11 = -1, M22 = -1, M33 = -1 };

            //The error is computed using this equation:
            //GoalRelativeOrientation * ConnectionA.Orientation * Error = ConnectionB.Orientation
            //GoalRelativeOrientation is the original rotation from A to B in A's local space.
            //Multiplying by A's orientation gives us where B *should* be.
            //Of course, B won't be exactly where it should be after initialization.
            //The Error component holds the difference between what is and what should be.
            //Error = (GoalRelativeOrientation * ConnectionA.Orientation)^-1 * ConnectionB.Orientation
            Quaternion bTarget;
            Quaternion.Concatenate(ref GoalRelativeOrientation, ref ConnectionA.Orientation, out bTarget);
            Quaternion bTargetConjugate;
            Quaternion.Conjugate(ref bTarget, out bTargetConjugate);

            Quaternion error;
            Quaternion.Concatenate(ref bTargetConjugate, ref ConnectionB.Orientation, out error);

            //Convert the error into an axis-angle vector usable for bias velocity.
            float angle;
            Vector3 axis;
            Quaternion.GetAxisAngleFromQuaternion(ref error, out axis, out angle);

            velocityBias.X = errorCorrectionFactor * axis.X * angle;
            velocityBias.Y = errorCorrectionFactor * axis.Y * angle;
            velocityBias.Z = errorCorrectionFactor * axis.Z * angle;
        }
Esempio n. 5
0
        protected internal override void UpdateJacobiansAndVelocityBias()
        {
            linearJacobianA = Matrix3x3.Identity;
            //The jacobian entries are is [ La, Aa, -Lb, -Ab ] because the relative velocity is computed using A-B. So, negate B's jacobians!
            linearJacobianB = new Matrix3x3 { M11 = -1, M22 = -1, M33 = -1 };
            System.Numerics.Vector3 rA;
            QuaternionEx.Transform(ref LocalOffsetA, ref ConnectionA.Orientation, out rA);
            Matrix3x3.CreateCrossProduct(ref rA, out angularJacobianA);
            //Transposing a skew-symmetric matrix is equivalent to negating it.
            Matrix3x3.Transpose(ref angularJacobianA, out angularJacobianA);

            System.Numerics.Vector3 worldPositionA;
            Vector3Ex.Add(ref ConnectionA.Position, ref rA, out worldPositionA);

            System.Numerics.Vector3 rB;
            QuaternionEx.Transform(ref LocalOffsetB, ref ConnectionB.Orientation, out rB);
            Matrix3x3.CreateCrossProduct(ref rB, out angularJacobianB);

            System.Numerics.Vector3 worldPositionB;
            Vector3Ex.Add(ref ConnectionB.Position, ref rB, out worldPositionB);

            System.Numerics.Vector3 linearError;
            Vector3Ex.Subtract(ref worldPositionB, ref worldPositionA, out linearError);
            Vector3Ex.Multiply(ref linearError, errorCorrectionFactor, out velocityBias);
        }
        protected internal override void UpdateJacobiansAndVelocityBias()
        {
            linearJacobian = new Matrix3x3();

            Vector3 boneAxis;
            Quaternion.Transform(ref BoneLocalFreeAxis, ref TargetBone.Orientation, out boneAxis);

            angularJacobian = new Matrix3x3
            {
                M11 = constrainedAxis1.X,
                M12 = constrainedAxis1.Y,
                M13 = constrainedAxis1.Z,
                M21 = constrainedAxis2.X,
                M22 = constrainedAxis2.Y,
                M23 = constrainedAxis2.Z
            };

            Vector3 error;
            Vector3.Cross(ref boneAxis, ref freeAxis, out error);
            Vector2 constraintSpaceError;
            Vector3.Dot(ref error, ref constrainedAxis1, out constraintSpaceError.X);
            Vector3.Dot(ref error, ref constrainedAxis2, out constraintSpaceError.Y);
            velocityBias.X = errorCorrectionFactor * constraintSpaceError.X;
            velocityBias.Y = errorCorrectionFactor * constraintSpaceError.Y;
        }
        protected internal override void UpdateJacobiansAndVelocityBias()
        {
            linearJacobian = new Matrix3x3();
            angularJacobian = Matrix3x3.Identity;

            //Error is in world space. It gets projected onto the jacobians later.
            System.Numerics.Quaternion errorQuaternion;
            QuaternionEx.Conjugate(ref TargetBone.Orientation, out errorQuaternion);
            QuaternionEx.Multiply(ref TargetOrientation, ref errorQuaternion, out errorQuaternion);
            float angle;
            System.Numerics.Vector3 angularError;
            QuaternionEx.GetAxisAngleFromQuaternion(ref errorQuaternion, out angularError, out angle);
            Vector3Ex.Multiply(ref angularError, angle, out angularError);

            //This is equivalent to projecting the error onto the angular jacobian. The angular jacobian just happens to be the identity matrix!
            Vector3Ex.Multiply(ref angularError, errorCorrectionFactor, out velocityBias);
        }
Esempio n. 8
0
 public MobileChunkShape(Vector3i csize, BlockInternal[] blocks, out Vector3 center)
 {
     Matrix3x3 boxMat = new BoxShape(csize.X, csize.Y, csize.Z).VolumeDistribution;
     ChunkSize = csize;
     Blocks = blocks;
     double weightInv = 1f / blocks.Length;
     center = new Vector3(csize.X / 2f, csize.Y / 2f, csize.Z / 2f);
     // TODO: More accurately get center of weight based on which blocks are solid or not!?
     Matrix3x3 volumeDistribution = new Matrix3x3();
     RigidTransform transform = new RigidTransform(center);
     Matrix3x3 contribution;
     CompoundShape.TransformContribution(ref transform, ref center, ref boxMat, blocks.Length, out contribution);
     Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution);
     Matrix3x3.Multiply(ref volumeDistribution, weightInv, out volumeDistribution);
     UpdateEntityShapeVolume(new EntityShapeVolumeDescription() { Volume = csize.X * csize.Y * csize.Z, VolumeDistribution = volumeDistribution });
     Center = center;
 }
        protected internal override void UpdateJacobiansAndVelocityBias()
        {
            linearJacobianA = linearJacobianB = new Matrix3x3();


            //There are two free axes and one restricted axis.
            //The constraint attempts to keep the hinge axis attached to connection A and the twist axis attached to connection B perpendicular to each other.
            //The restricted axis is the cross product between the twist and hinge axes.

            Vector3 worldTwistAxis, worldHingeAxis;
            Quaternion.Transform(ref LocalHingeAxis, ref ConnectionA.Orientation, out worldHingeAxis);
            Quaternion.Transform(ref LocalTwistAxis, ref ConnectionB.Orientation, out worldTwistAxis);

            Vector3 restrictedAxis;
            Vector3.Cross(ref worldHingeAxis, ref worldTwistAxis, out restrictedAxis);
            //Attempt to normalize the restricted axis.
            float lengthSquared = restrictedAxis.LengthSquared();
            if (lengthSquared > Toolbox.Epsilon)
            {
                Vector3.Divide(ref restrictedAxis, (float)Math.Sqrt(lengthSquared), out restrictedAxis);
            }
            else
            {
                restrictedAxis = new Vector3();
            }


            angularJacobianA = new Matrix3x3
              {
                  M11 = restrictedAxis.X,
                  M12 = restrictedAxis.Y,
                  M13 = restrictedAxis.Z,
              };
            Matrix3x3.Negate(ref angularJacobianA, out angularJacobianB);

            float error;
            Vector3.Dot(ref worldHingeAxis, ref worldTwistAxis, out error);
            error = (float)Math.Acos(MathHelper.Clamp(error, -1, 1)) - MathHelper.PiOver2;

            velocityBias = new Vector3(errorCorrectionFactor * error, 0, 0);


        }
Esempio n. 10
0
        protected internal override void UpdateJacobiansAndVelocityBias()
        {

            //This constraint doesn't consider linear motion.
            linearJacobianA = linearJacobianB = new Matrix3x3();

            //Compute the world axes.
            Vector3 axisA, axisB;
            Quaternion.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA);
            Quaternion.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB);

            float dot;
            Vector3.Dot(ref axisA, ref axisB, out dot);

            //Yes, we could avoid this acos here. Performance is not the highest goal of this system; the less tricks used, the easier it is to understand.
            float angle = (float)Math.Acos(MathHelper.Clamp(dot, -1, 1));

            //One angular DOF is constrained by this limit.
            Vector3 hingeAxis;
            Vector3.Cross(ref axisA, ref axisB, out hingeAxis);

            angularJacobianA.M1 = hingeAxis;
            hingeAxis.Invert( out angularJacobianB.M1 );

            //Note how we've computed the jacobians despite the limit being potentially inactive.
            //This is to enable 'speculative' limits.
            if (angle >= maximumAngle)
            {
                velocityBias = new Vector3(errorCorrectionFactor * (angle - maximumAngle), 0, 0);
            }
            else
            {
                //The constraint is not yet violated. But, it may be- allow only as much motion as could occur without violating the constraint.
                //Limits can't 'pull,' so this will not result in erroneous sticking.
                velocityBias = new Vector3(angle - maximumAngle, 0, 0);
            }


        }
Esempio n. 11
0
        /// <summary>
        /// Creates a 4x4 matrix from a 3x3 matrix.
        /// </summary>
        /// <param name="a">3x3 matrix.</param>
        /// <returns>Created 4x4 matrix.</returns>
        public static Matrix ToMatrix4X4(Matrix3x3 a)
        {
#if !WINDOWS
            Matrix b = new Matrix();
#else
            Matrix b;
#endif
            b.M11 = a.M11;
            b.M12 = a.M12;
            b.M13 = a.M13;

            b.M21 = a.M21;
            b.M22 = a.M22;
            b.M23 = a.M23;

            b.M31 = a.M31;
            b.M32 = a.M32;
            b.M33 = a.M33;

            b.M44 = 1;
            b.M14 = 0;
            b.M24 = 0;
            b.M34 = 0;
            b.M41 = 0;
            b.M42 = 0;
            b.M43 = 0;
            return b;
        }
Esempio n. 12
0
        /// <summary>
        /// Creates a 4x4 matrix from a 3x3 matrix.
        /// </summary>
        /// <param name="a">3x3 matrix.</param>
        /// <param name="b">Created 4x4 matrix.</param>
        public static void ToMatrix4X4(ref Matrix3x3 a, out Matrix b)
        {
#if !WINDOWS
            b = new Matrix();
#endif
            b.M11 = a.M11;
            b.M12 = a.M12;
            b.M13 = a.M13;

            b.M21 = a.M21;
            b.M22 = a.M22;
            b.M23 = a.M23;

            b.M31 = a.M31;
            b.M32 = a.M32;
            b.M33 = a.M33;

            b.M44 = 1;
            b.M14 = 0;
            b.M24 = 0;
            b.M34 = 0;
            b.M41 = 0;
            b.M42 = 0;
            b.M43 = 0;
        }
Esempio n. 13
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 Matrix3x3 a, ref Matrix3x2 b, out Matrix3x2 result)
        {
            float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31;
            float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32;

            float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31;
            float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32;

            float resultM31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31;
            float resultM32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32;

            result.M11 = resultM11;
            result.M12 = resultM12;

            result.M21 = resultM21;
            result.M22 = resultM22;

            result.M31 = resultM31;
            result.M32 = resultM32;
        }
Esempio n. 14
0
 ///<summary>
 /// Computes the volume contribution of a point.
 ///</summary>
 ///<param name="pointWeight">Weight of the point.</param>
 ///<param name="center">Location to use as the center for the purposes of computing the contribution.</param>
 ///<param name="p">Point to compute the contribution of.</param>
 ///<param name="contribution">Contribution of the point.</param>
 public static void GetPointContribution(float pointWeight, ref Vector3 center, Vector3 p, out Matrix3x3 contribution)
 {
     Vector3.Subtract(ref p, ref center, out p);
     float xx = pointWeight * p.X * p.X;
     float yy = pointWeight * p.Y * p.Y;
     float zz = pointWeight * p.Z * p.Z;
     contribution.M11 = yy + zz;
     contribution.M22 = xx + zz;
     contribution.M33 = xx + yy;
     contribution.M12 = -pointWeight * p.X * p.Y;
     contribution.M13 = -pointWeight * p.X * p.Z;
     contribution.M23 = -pointWeight * p.Y * p.Z;
     contribution.M21 = contribution.M12;
     contribution.M31 = contribution.M13;
     contribution.M32 = contribution.M23;
 }
Esempio n. 15
0
        /// <summary>
        /// Updates the quaternion using RK4 integration.
        /// </summary>
        /// <param name="q">Quaternion to update.</param>
        /// <param name="localInertiaTensorInverse">Local-space inertia tensor of the object being updated.</param>
        /// <param name="angularMomentum">Angular momentum of the object.</param>
        /// <param name="dt">Time since last frame, in seconds.</param>
        /// <param name="newOrientation">New orientation quaternion.</param>
        public static void UpdateOrientationRK4(ref Quaternion q, ref Matrix3x3 localInertiaTensorInverse, ref Vector3 angularMomentum, float dt, out Quaternion newOrientation)
        {
            //TODO: This is a little goofy
            //Quaternion diff = differentiateQuaternion(ref q, ref localInertiaTensorInverse, ref angularMomentum);
            Quaternion d1;
            DifferentiateQuaternion(ref q, ref localInertiaTensorInverse, ref angularMomentum, out d1);
            Quaternion s2;
            Quaternion.Multiply(ref d1, dt * .5f, out s2);
            Quaternion.Add(ref q, ref s2, out s2);

            Quaternion d2;
            DifferentiateQuaternion(ref s2, ref localInertiaTensorInverse, ref angularMomentum, out d2);
            Quaternion s3;
            Quaternion.Multiply(ref d2, dt * .5f, out s3);
            Quaternion.Add(ref q, ref s3, out s3);

            Quaternion d3;
            DifferentiateQuaternion(ref s3, ref localInertiaTensorInverse, ref angularMomentum, out d3);
            Quaternion s4;
            Quaternion.Multiply(ref d3, dt, out s4);
            Quaternion.Add(ref q, ref s4, out s4);

            Quaternion d4;
            DifferentiateQuaternion(ref s4, ref localInertiaTensorInverse, ref angularMomentum, out d4);

            Quaternion.Multiply(ref d1, dt / 6, out d1);
            Quaternion.Multiply(ref d2, dt / 3, out d2);
            Quaternion.Multiply(ref d3, dt / 3, out d3);
            Quaternion.Multiply(ref d4, dt / 6, out d4);
            Quaternion added;
            Quaternion.Add(ref q, ref d1, out added);
            Quaternion.Add(ref added, ref d2, out added);
            Quaternion.Add(ref added, ref d3, out added);
            Quaternion.Add(ref added, ref d4, out added);
            Quaternion.Normalize(ref added, out newOrientation);
        }
Esempio n. 16
0
 /// <summary>
 /// Constructs a quaternion from a rotation matrix.
 /// </summary>
 /// <param name="r">Rotation matrix to create the quaternion from.</param>
 /// <param name="q">System.Numerics.Quaternion based on the rotation matrix.</param>
 public static void CreateFromRotationMatrix(ref Matrix3x3 r, out System.Numerics.Quaternion q)
 {
     float trace = r.M11 + r.M22 + r.M33;
     #if !WINDOWS
     q = new System.Numerics.Quaternion();
     #endif
     if (trace >= 0)
     {
         var S = (float)Math.Sqrt(trace + 1.0) * 2; // S=4*qw
         var inverseS = 1 / S;
         q.W = 0.25f * S;
         q.X = (r.M23 - r.M32) * inverseS;
         q.Y = (r.M31 - r.M13) * inverseS;
         q.Z = (r.M12 - r.M21) * inverseS;
     }
     else if ((r.M11 > r.M22) & (r.M11 > r.M33))
     {
         var S = (float)Math.Sqrt(1.0 + r.M11 - r.M22 - r.M33) * 2; // S=4*qx
         var inverseS = 1 / S;
         q.W = (r.M23 - r.M32) * inverseS;
         q.X = 0.25f * S;
         q.Y = (r.M21 + r.M12) * inverseS;
         q.Z = (r.M31 + r.M13) * inverseS;
     }
     else if (r.M22 > r.M33)
     {
         var S = (float)Math.Sqrt(1.0 + r.M22 - r.M11 - r.M33) * 2; // S=4*qy
         var inverseS = 1 / S;
         q.W = (r.M31 - r.M13) * inverseS;
         q.X = (r.M21 + r.M12) * inverseS;
         q.Y = 0.25f * S;
         q.Z = (r.M32 + r.M23) * inverseS;
     }
     else
     {
         var S = (float)Math.Sqrt(1.0 + r.M33 - r.M11 - r.M22) * 2; // S=4*qz
         var inverseS = 1 / S;
         q.W = (r.M12 - r.M21) * inverseS;
         q.X = (r.M31 + r.M13) * inverseS;
         q.Y = (r.M32 + r.M23) * inverseS;
         q.Z = 0.25f * S;
     }
 }
Esempio n. 17
0
        protected internal override void ComputeEffectiveMass()
        {
            //For all constraints, the effective mass matrix is 1 / (J * M^-1 * JT).
            //For two bone constraints, J has 4 3x3 matrices. M^-1 (W below) is a 12x12 matrix with 4 3x3 block diagonal matrices.
            //To compute the whole denominator,
            Matrix3x3 linearW;
            Matrix3x3 linearA, angularA, linearB, angularB;

            if (!ConnectionA.Pinned)
            {
                Matrix3x3.CreateScale(ConnectionA.inverseMass, out linearW);
                Matrix3x3.Multiply(ref linearJacobianA, ref linearW, out linearA); //Compute J * M^-1 for linear component
                Matrix3x3.MultiplyByTransposed(ref linearA, ref linearJacobianA, out linearA); //Compute (J * M^-1) * JT for linear component

                Matrix3x3.Multiply(ref angularJacobianA, ref ConnectionA.inertiaTensorInverse, out angularA); //Compute J * M^-1 for angular component
                Matrix3x3.MultiplyByTransposed(ref angularA, ref angularJacobianA, out angularA); //Compute (J * M^-1) * JT for angular component
            }
            else
            {
                //Treat pinned bones as if they have infinite inertia.
                linearA = new Matrix3x3();
                angularA = new Matrix3x3();
            }

            if (!ConnectionB.Pinned)
            {
                Matrix3x3.CreateScale(ConnectionB.inverseMass, out linearW);
                Matrix3x3.Multiply(ref linearJacobianB, ref linearW, out linearB); //Compute J * M^-1 for linear component
                Matrix3x3.MultiplyByTransposed(ref linearB, ref linearJacobianB, out linearB); //Compute (J * M^-1) * JT for linear component

                Matrix3x3.Multiply(ref angularJacobianB, ref ConnectionB.inertiaTensorInverse, out angularB); //Compute J * M^-1 for angular component
                Matrix3x3.MultiplyByTransposed(ref angularB, ref angularJacobianB, out angularB); //Compute (J * M^-1) * JT for angular component
            }
            else
            {
                //Treat pinned bones as if they have infinite inertia.
                linearB = new Matrix3x3();
                angularB = new Matrix3x3();
            }

            //A nice side effect of the block diagonal nature of M^-1 is that the above separated components are now combined into the complete denominator matrix by addition!
            Matrix3x3.Add(ref linearA, ref angularA, out effectiveMass);
            Matrix3x3.Add(ref effectiveMass, ref linearB, out effectiveMass);
            Matrix3x3.Add(ref effectiveMass, ref angularB, out effectiveMass);

            //Incorporate the constraint softness into the effective mass denominator. This pushes the matrix away from singularity.
            //Softness will also be incorporated into the velocity solve iterations to complete the implementation.
            if (effectiveMass.M1.X != 0)
                effectiveMass.M1.X += softness;
            if (effectiveMass.M2.Y != 0)
                effectiveMass.M2.Y += softness;
            if (effectiveMass.M3.Z != 0)
                effectiveMass.M3.Z += softness;

            //Invert! Takes us from J * M^-1 * JT to 1 / (J * M^-1 * JT).
            Matrix3x3.AdaptiveInvert(ref effectiveMass, out effectiveMass);

        }
Esempio n. 18
0
 ///<summary>
 /// Computes a volume distribution based on a bunch of point contributions.
 ///</summary>
 ///<param name="pointContributions">Point contributions to the volume distribution.</param>
 ///<param name="center">Location to use as the center for purposes of computing point contributions.</param>
 ///<returns>Volume distribution of the point contributions.</returns>
 public static Matrix3x3 ComputeVolumeDistribution(RawList<Vector3> pointContributions, ref Vector3 center)
 {
     var volumeDistribution = new Matrix3x3();
     float pointWeight = 1f / pointContributions.Count;
     for (int i = 0; i < pointContributions.Count; i++)
     {
         Matrix3x3 contribution;
         GetPointContribution(pointWeight, ref center, pointContributions[i], out contribution);
         Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution);
     }
     return volumeDistribution;
 }
Esempio n. 19
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 Matrix3x3 matrix, out Matrix3x3 result)
        {
            float determinantInverse = 1 / matrix.Determinant();
            float m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse;
            float m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse;
            float m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse;

            float m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse;
            float m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse;
            float m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse;

            float m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse;
            float m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse;
            float m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse;

            result.M11 = m11;
            result.M12 = m12;
            result.M13 = m13;

            result.M21 = m21;
            result.M22 = m22;
            result.M23 = m23;

            result.M31 = m31;
            result.M32 = m32;
            result.M33 = m33;
        }
        /// <summary>
        /// Initializes the constraint for the current frame.
        /// </summary>
        /// <param name="dt">Time between frames.</param>
        public override void Update(float dt)
        {
            basis.rotationMatrix = entity.orientationMatrix;
            basis.ComputeWorldSpaceAxes();

            float updateRate = 1 / dt;
            if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo.
            {
                Quaternion currentRelativeOrientation;
                var worldTransform = basis.WorldTransform;
                Quaternion.CreateFromRotationMatrix(ref worldTransform, out currentRelativeOrientation);

                //Compute the relative orientation R' between R and the target relative orientation.
                Quaternion errorOrientation;
                Quaternion.Conjugate(ref currentRelativeOrientation, out errorOrientation);
                Quaternion.Multiply(ref settings.servo.goal, ref errorOrientation, out errorOrientation);

                float errorReduction;
                settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);

                //Turn this into an axis-angle representation.
                Quaternion.GetAxisAngleFromQuaternion(ref errorOrientation, out axis, out angle);

                //Scale the axis by the desired velocity if the angle is sufficiently large (epsilon).
                if (angle > Toolbox.BigEpsilon)
                {
                    float velocity = MathHelper.Min(settings.servo.baseCorrectiveSpeed, angle * updateRate) + angle * errorReduction;

                    biasVelocity.X = axis.X * velocity;
                    biasVelocity.Y = axis.Y * velocity;
                    biasVelocity.Z = axis.Z * velocity;

                    //Ensure that the corrective velocity doesn't exceed the max.
                    float length = biasVelocity.LengthSquared();
                    if (length > settings.servo.maxCorrectiveVelocitySquared)
                    {
                        float multiplier = settings.servo.maxCorrectiveVelocity / (float) Math.Sqrt(length);
                        biasVelocity.X *= multiplier;
                        biasVelocity.Y *= multiplier;
                        biasVelocity.Z *= multiplier;
                    }
                }
                else
                {
                    //Wouldn't want an old frame's bias velocity to sneak in.
                    biasVelocity = new Vector3();
                }
            }
            else
            {
                usedSoftness = settings.velocityMotor.softness * updateRate;
                angle = 0; //Zero out the error;
                Matrix3x3 transform = basis.WorldTransform;
                Matrix3x3.Transform(ref settings.velocityMotor.goalVelocity, ref transform, out biasVelocity);
            }

            //Compute effective mass
            effectiveMassMatrix = entity.inertiaTensorInverse;
            effectiveMassMatrix.M11 += usedSoftness;
            effectiveMassMatrix.M22 += usedSoftness;
            effectiveMassMatrix.M33 += usedSoftness;
            Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);

            //Update the maximum force
            ComputeMaxForces(settings.maximumForce, dt);
        }
Esempio n. 21
0
        /// <summary>
        /// Inverts the largest nonsingular submatrix in the matrix, excluding 2x2's that involve M13 or M31, and excluding 1x1's that include nondiagonal elements.
        /// </summary>
        /// <param name="matrix">Matrix to be inverted.</param>
        /// <param name="result">Inverted matrix.</param>
        public static void AdaptiveInvert(ref Matrix3x3 matrix, out Matrix3x3 result)
        {
            int submatrix;
            float determinantInverse = 1 / matrix.AdaptiveDeterminant(out submatrix);
            float m11, m12, m13, m21, m22, m23, m31, m32, m33;
            switch (submatrix)
            {
                case 0: //Full matrix.
                    m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse;
                    m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse;
                    m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse;

                    m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse;
                    m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse;
                    m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse;

                    m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse;
                    m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse;
                    m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse;
                    break;
                case 1: //Upper left matrix, m11, m12, m21, m22.
                    m11 = matrix.M22 * determinantInverse;
                    m12 = -matrix.M12 * determinantInverse;
                    m13 = 0;

                    m21 = -matrix.M21 * determinantInverse;
                    m22 = matrix.M11 * determinantInverse;
                    m23 = 0;

                    m31 = 0;
                    m32 = 0;
                    m33 = 0;
                    break;
                case 2: //Lower right matrix, m22, m23, m32, m33.
                    m11 = 0;
                    m12 = 0;
                    m13 = 0;

                    m21 = 0;
                    m22 = matrix.M33 * determinantInverse;
                    m23 = -matrix.M23 * determinantInverse;

                    m31 = 0;
                    m32 = -matrix.M32 * determinantInverse;
                    m33 = matrix.M22 * determinantInverse;
                    break;
                case 3: //Corners, m11, m31, m13, m33.
                    m11 = matrix.M33 * determinantInverse;
                    m12 = 0;
                    m13 = -matrix.M13 * determinantInverse;

                    m21 = 0;
                    m22 = 0;
                    m23 = 0;

                    m31 = -matrix.M31 * determinantInverse;
                    m32 = 0;
                    m33 = matrix.M11 * determinantInverse;
                    break;
                case 4: //M11
                    m11 = 1 / matrix.M11;
                    m12 = 0;
                    m13 = 0;

                    m21 = 0;
                    m22 = 0;
                    m23 = 0;

                    m31 = 0;
                    m32 = 0;
                    m33 = 0;
                    break;
                case 5: //M22
                    m11 = 0;
                    m12 = 0;
                    m13 = 0;

                    m21 = 0;
                    m22 = 1 / matrix.M22;
                    m23 = 0;

                    m31 = 0;
                    m32 = 0;
                    m33 = 0;
                    break;
                case 6: //M33
                    m11 = 0;
                    m12 = 0;
                    m13 = 0;

                    m21 = 0;
                    m22 = 0;
                    m23 = 0;

                    m31 = 0;
                    m32 = 0;
                    m33 = 1 / matrix.M33;
                    break;
                default: //Completely singular.
                    m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 0;
                    break;
            }

            result.M11 = m11;
            result.M12 = m12;
            result.M13 = m13;

            result.M21 = m21;
            result.M22 = m22;
            result.M23 = m23;

            result.M31 = m31;
            result.M32 = m32;
            result.M33 = m33;
        }
        /// <summary>
        /// Calculates necessary information for velocity solving.
        /// Called automatically by space.
        /// </summary>
        /// <param name="dt">Time in seconds since the last update.</param>
        public override void Update(float dt)
        {
            usedSoftness = softness / dt;

            effectiveMassMatrix = entity.inertiaTensorInverse;

            effectiveMassMatrix.M11 += usedSoftness;
            effectiveMassMatrix.M22 += usedSoftness;
            effectiveMassMatrix.M33 += usedSoftness;

            Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);

            //Determine maximum force
            if (maximumForce < float.MaxValue)
            {
                maxForceDt = maximumForce * dt;
                maxForceDtSquared = maxForceDt * maxForceDt;
            }
            else
            {
                maxForceDt = float.MaxValue;
                maxForceDtSquared = float.MaxValue;
            }
        }
Esempio n. 23
0
        /// <summary>
        /// Multiplies a transposed matrix with another matrix.
        /// </summary>
        /// <param name="matrix">Matrix to be multiplied.</param>
        /// <param name="transpose">Matrix to be transposed and multiplied.</param>
        /// <param name="result">Product of the multiplication.</param>
        public static void MultiplyTransposed(ref Matrix3x3 transpose, ref Matrix3x3 matrix, out Matrix3x3 result)
        {
            float resultM11 = transpose.M11 * matrix.M11 + transpose.M21 * matrix.M21 + transpose.M31 * matrix.M31;
            float resultM12 = transpose.M11 * matrix.M12 + transpose.M21 * matrix.M22 + transpose.M31 * matrix.M32;
            float resultM13 = transpose.M11 * matrix.M13 + transpose.M21 * matrix.M23 + transpose.M31 * matrix.M33;

            float resultM21 = transpose.M12 * matrix.M11 + transpose.M22 * matrix.M21 + transpose.M32 * matrix.M31;
            float resultM22 = transpose.M12 * matrix.M12 + transpose.M22 * matrix.M22 + transpose.M32 * matrix.M32;
            float resultM23 = transpose.M12 * matrix.M13 + transpose.M22 * matrix.M23 + transpose.M32 * matrix.M33;

            float resultM31 = transpose.M13 * matrix.M11 + transpose.M23 * matrix.M21 + transpose.M33 * matrix.M31;
            float resultM32 = transpose.M13 * matrix.M12 + transpose.M23 * matrix.M22 + transpose.M33 * matrix.M32;
            float resultM33 = transpose.M13 * matrix.M13 + transpose.M23 * matrix.M23 + transpose.M33 * matrix.M33;

            result.M11 = resultM11;
            result.M12 = resultM12;
            result.M13 = resultM13;

            result.M21 = resultM21;
            result.M22 = resultM22;
            result.M23 = resultM23;

            result.M31 = resultM31;
            result.M32 = resultM32;
            result.M33 = resultM33;
        }
Esempio n. 24
0
 /// <summary>
 /// Creates a quaternion from a rotation matrix.
 /// </summary>
 /// <param name="r">Rotation matrix used to create a new quaternion.</param>
 /// <returns>System.Numerics.Quaternion representing the same rotation as the matrix.</returns>
 public static System.Numerics.Quaternion CreateFromRotationMatrix(Matrix3x3 r)
 {
     System.Numerics.Quaternion toReturn;
     CreateFromRotationMatrix(ref r, out toReturn);
     return toReturn;
 }
Esempio n. 25
0
        /// <summary>
        /// Multiplies a matrix with a transposed matrix.
        /// </summary>
        /// <param name="matrix">Matrix to be multiplied.</param>
        /// <param name="transpose">Matrix to be transposed and multiplied.</param>
        /// <param name="result">Product of the multiplication.</param>
        public static void MultiplyByTransposed(ref Matrix3x3 matrix, ref Matrix3x3 transpose, out Matrix3x3 result)
        {
            float resultM11 = matrix.M11 * transpose.M11 + matrix.M12 * transpose.M12 + matrix.M13 * transpose.M13;
            float resultM12 = matrix.M11 * transpose.M21 + matrix.M12 * transpose.M22 + matrix.M13 * transpose.M23;
            float resultM13 = matrix.M11 * transpose.M31 + matrix.M12 * transpose.M32 + matrix.M13 * transpose.M33;

            float resultM21 = matrix.M21 * transpose.M11 + matrix.M22 * transpose.M12 + matrix.M23 * transpose.M13;
            float resultM22 = matrix.M21 * transpose.M21 + matrix.M22 * transpose.M22 + matrix.M23 * transpose.M23;
            float resultM23 = matrix.M21 * transpose.M31 + matrix.M22 * transpose.M32 + matrix.M23 * transpose.M33;

            float resultM31 = matrix.M31 * transpose.M11 + matrix.M32 * transpose.M12 + matrix.M33 * transpose.M13;
            float resultM32 = matrix.M31 * transpose.M21 + matrix.M32 * transpose.M22 + matrix.M33 * transpose.M23;
            float resultM33 = matrix.M31 * transpose.M31 + matrix.M32 * transpose.M32 + matrix.M33 * transpose.M33;

            result.M11 = resultM11;
            result.M12 = resultM12;
            result.M13 = resultM13;

            result.M21 = resultM21;
            result.M22 = resultM22;
            result.M23 = resultM23;

            result.M31 = resultM31;
            result.M32 = resultM32;
            result.M33 = resultM33;
        }
Esempio n. 26
0
 /// <summary>
 /// Finds the change in the rotation state quaternion provided the local inertia tensor and angular velocity.
 /// </summary>
 /// <param name="orientation">Orienatation of the object.</param>
 /// <param name="localInertiaTensorInverse">Local-space inertia tensor of the object being updated.</param>
 /// <param name="angularMomentum">Angular momentum of the object.</param>
 ///  <param name="orientationChange">Change in quaternion.</param>
 public static void DifferentiateQuaternion(ref Quaternion orientation, ref Matrix3x3 localInertiaTensorInverse, ref Vector3 angularMomentum, out Quaternion orientationChange)
 {
     Quaternion normalizedOrientation;
     Quaternion.Normalize(ref orientation, out normalizedOrientation);
     Matrix3x3 tempRotMat;
     Matrix3x3.CreateFromQuaternion(ref normalizedOrientation, out tempRotMat);
     Matrix3x3 tempInertiaTensorInverse;
     Matrix3x3.MultiplyTransposed(ref tempRotMat, ref localInertiaTensorInverse, out tempInertiaTensorInverse);
     Matrix3x3.Multiply(ref tempInertiaTensorInverse, ref tempRotMat, out tempInertiaTensorInverse);
     Vector3 halfspin;
     Matrix3x3.Transform(ref angularMomentum, ref tempInertiaTensorInverse, out halfspin);
     Vector3.Multiply(ref halfspin, .5f, out halfspin);
     var halfspinQuaternion = new Quaternion(halfspin.X, halfspin.Y, halfspin.Z, 0);
     Quaternion.Multiply(ref halfspinQuaternion, ref normalizedOrientation, out orientationChange);
 }
Esempio n. 27
0
        /// <summary>
        /// Scales all components of the matrix.
        /// </summary>
        /// <param name="matrix">Matrix to scale.</param>
        /// <param name="scale">Amount to scale.</param>
        /// <param name="result">Scaled matrix.</param>
        public static void Multiply(ref Matrix3x3 matrix, float scale, out Matrix3x3 result)
        {
            result.M11 = matrix.M11 * scale;
            result.M12 = matrix.M12 * scale;
            result.M13 = matrix.M13 * scale;

            result.M21 = matrix.M21 * scale;
            result.M22 = matrix.M22 * scale;
            result.M23 = matrix.M23 * scale;

            result.M31 = matrix.M31 * scale;
            result.M32 = matrix.M32 * scale;
            result.M33 = matrix.M33 * scale;
        }
        protected internal override void UpdateJacobiansAndVelocityBias()
        {
            //Transform the anchors and offsets into world space.
            Vector3 offsetA, offsetB, lineDirection;
            Quaternion.Transform(ref LocalLineAnchor, ref ConnectionA.Orientation, out offsetA);
            Quaternion.Transform(ref LocalLineDirection, ref ConnectionA.Orientation, out lineDirection);
            Quaternion.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out offsetB);
            Vector3 anchorA, anchorB;
            Vector3.Add(ref ConnectionA.Position, ref offsetA, out anchorA);
            Vector3.Add(ref ConnectionB.Position, ref offsetB, out anchorB);

            //Compute the distance.
            Vector3 separation;
            Vector3.Subtract(ref anchorB, ref anchorA, out separation);
            //This entire constraint is very similar to the IKDistanceLimit, except the current distance is along an axis.
            float currentDistance;
            Vector3.Dot(ref separation, ref lineDirection, out currentDistance);

            //Compute jacobians
            if (currentDistance > maximumDistance)
            {
                //We are exceeding the maximum limit.
                velocityBias = new Vector3(errorCorrectionFactor * (currentDistance - maximumDistance), 0, 0);
            }
            else if (currentDistance < minimumDistance)
            {
                //We are exceeding the minimum limit.
                velocityBias = new Vector3(errorCorrectionFactor * (minimumDistance - currentDistance), 0, 0);
                //The limit can only push in one direction. Flip the jacobian!
                Vector3.Negate(ref lineDirection, out lineDirection);
            }
            else if (currentDistance - minimumDistance > (maximumDistance - minimumDistance) * 0.5f)
            {
                //The objects are closer to hitting the maximum limit.
                velocityBias = new Vector3(currentDistance - maximumDistance, 0, 0);
            }
            else
            {
                //The objects are closer to hitting the minimum limit.
                velocityBias = new Vector3(minimumDistance - currentDistance, 0, 0);
                //The limit can only push in one direction. Flip the jacobian!
                Vector3.Negate(ref lineDirection, out lineDirection);
            }

            Vector3 angularA, angularB;
            //We can't just use the offset to anchor for A's jacobian- the 'collision' location is way out there at anchorB!
            Vector3 rA;
            Vector3.Subtract(ref anchorB, ref ConnectionA.Position, out rA);
            Vector3.Cross(ref rA, ref lineDirection, out angularA);
            //linearB = -linearA, so just swap the cross product order.
            Vector3.Cross(ref lineDirection, ref offsetB, out angularB);

            //Put all the 1x3 jacobians into a 3x3 matrix representation.
            linearJacobianA = new Matrix3x3 { M11 = lineDirection.X, M12 = lineDirection.Y, M13 = lineDirection.Z };
            linearJacobianB = new Matrix3x3 { M11 = -lineDirection.X, M12 = -lineDirection.Y, M13 = -lineDirection.Z };
            angularJacobianA = new Matrix3x3 { M11 = angularA.X, M12 = angularA.Y, M13 = angularA.Z };
            angularJacobianB = new Matrix3x3 { M11 = angularB.X, M12 = angularB.Y, M13 = angularB.Z };

        }
Esempio n. 29
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 Matrix3x3 matrix, out Matrix3x3 result)
        {
            result.M11 = -matrix.M11;
            result.M12 = -matrix.M12;
            result.M13 = -matrix.M13;

            result.M21 = -matrix.M21;
            result.M22 = -matrix.M22;
            result.M23 = -matrix.M23;

            result.M31 = -matrix.M31;
            result.M32 = -matrix.M32;
            result.M33 = -matrix.M33;
        }
Esempio n. 30
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 Matrix3x3 a, ref Matrix3x3 b, out Matrix3x3 result)
        {
            float m11 = a.M11 - b.M11;
            float m12 = a.M12 - b.M12;
            float m13 = a.M13 - b.M13;

            float m21 = a.M21 - b.M21;
            float m22 = a.M22 - b.M22;
            float m23 = a.M23 - b.M23;

            float m31 = a.M31 - b.M31;
            float m32 = a.M32 - b.M32;
            float m33 = a.M33 - b.M33;

            result.M11 = m11;
            result.M12 = m12;
            result.M13 = m13;

            result.M21 = m21;
            result.M22 = m22;
            result.M23 = m23;

            result.M31 = m31;
            result.M32 = m32;
            result.M33 = m33;
        }
Esempio n. 31
0
 public static void Validate(this Matrix3x3 m)
 {
     m.Right.Validate();
     m.Up.Validate();
     m.Backward.Validate();
 }