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);
        }
Exemple #2
0
        protected internal override void UpdateJacobiansAndVelocityBias()
        {
            linearJacobian = Matrix3x3.Identity;
            System.Numerics.Vector3 r;
            QuaternionEx.Transform(ref LocalOffset, ref TargetBone.Orientation, out r);
            Matrix3x3.CreateCrossProduct(ref r, out angularJacobian);
            //Transposing a skew symmetric matrix is equivalent to negating it.
            Matrix3x3.Transpose(ref angularJacobian, out angularJacobian);

            System.Numerics.Vector3 worldPosition;
            Vector3Ex.Add(ref TargetBone.Position, ref r, out worldPosition);

            //Error is in world space.
            System.Numerics.Vector3 linearError;
            Vector3Ex.Subtract(ref TargetPosition, ref worldPosition, out linearError);
            //This is equivalent to projecting the error onto the linear jacobian. The linear jacobian just happens to be the identity matrix!
            Vector3Ex.Multiply(ref linearError, errorCorrectionFactor, out velocityBias);
        }
Exemple #3
0
 public void Do()
 {
     Matrix3x3.CreateCrossProduct(v, out var skew);
     Matrix3x3.MultiplyTransposed(skew, symmetric, out var intermediate);
     Matrix3x3.Multiply(intermediate, skew, out result);
 }
Exemple #4
0
        /// <summary>
        /// Calculates necessary information for velocity solving.
        /// Called by preStep(float dt)
        /// </summary>
        /// <param name="dt">Time in seconds since the last update.</param>
        public override void Update(float dt)
        {
            Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA);
            Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB);


            float errorReductionParameter;

            springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReductionParameter, out softness);

            //Mass System.Numerics.Matrix4x4
            Matrix3x3 k;
            Matrix3x3 linearComponent;

            Matrix3x3.CreateCrossProduct(ref worldOffsetA, out rACrossProduct);
            Matrix3x3.CreateCrossProduct(ref worldOffsetB, out rBCrossProduct);
            if (connectionA.isDynamic && connectionB.isDynamic)
            {
                Matrix3x3.CreateScale(connectionA.inverseMass + connectionB.inverseMass, out linearComponent);
                Matrix3x3 angularComponentA, angularComponentB;
                Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
                Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
                Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
                Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
                Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k);
                Matrix3x3.Subtract(ref k, ref angularComponentB, out k);
            }
            else if (connectionA.isDynamic && !connectionB.isDynamic)
            {
                Matrix3x3.CreateScale(connectionA.inverseMass, out linearComponent);
                Matrix3x3 angularComponentA;
                Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
                Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
                Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k);
            }
            else if (!connectionA.isDynamic && connectionB.isDynamic)
            {
                Matrix3x3.CreateScale(connectionB.inverseMass, out linearComponent);
                Matrix3x3 angularComponentB;
                Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
                Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
                Matrix3x3.Subtract(ref linearComponent, ref angularComponentB, out k);
            }
            else
            {
                throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
            }
            k.M11 += softness;
            k.M22 += softness;
            k.M33 += softness;
            Matrix3x3.Invert(ref k, out massMatrix);

            Vector3Ex.Add(ref connectionB.position, ref worldOffsetB, out error);
            Vector3Ex.Subtract(ref error, ref connectionA.position, out error);
            Vector3Ex.Subtract(ref error, ref worldOffsetA, out error);


            Vector3Ex.Multiply(ref error, -errorReductionParameter, out biasVelocity);

            //Ensure that the corrective velocity doesn't exceed the max.
            float length = biasVelocity.LengthSquared();

            if (length > maxCorrectiveVelocitySquared)
            {
                float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length);
                biasVelocity.X *= multiplier;
                biasVelocity.Y *= multiplier;
                biasVelocity.Z *= multiplier;
            }
        }
Exemple #5
0
        ///<summary>
        /// Performs the frame's configuration step.
        ///</summary>
        ///<param name="dt">Timestep duration.</param>
        public override void Update(float dt)
        {
            //Transform point into world space.
            Matrix3x3.Transform(ref localPoint, ref entity.orientationMatrix, out r);
            Vector3.Add(ref r, ref entity.position, out worldPoint);

            float updateRate = 1 / dt;

            if (settings.mode == MotorMode.Servomechanism)
            {
                Vector3.Subtract(ref settings.servo.goal, ref worldPoint, out error);
                float separationDistance = error.Length();
                if (separationDistance > Toolbox.BigEpsilon)
                {
                    float errorReduction;
                    settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, updateRate, out errorReduction, out usedSoftness);

                    //The rate of correction can be based on a constant correction velocity as well as a 'spring like' correction velocity.
                    //The constant correction velocity could overshoot the destination, so clamp it.
                    float correctionSpeed = MathHelper.Min(settings.servo.baseCorrectiveSpeed, separationDistance * updateRate) +
                                            separationDistance * errorReduction;

                    Vector3.Multiply(ref error, correctionSpeed / separationDistance, out biasVelocity);
                    //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 to use a bias from an earlier frame.
                    biasVelocity = new Vector3();
                }
            }
            else
            {
                usedSoftness = settings.velocityMotor.softness * updateRate;
                biasVelocity = settings.velocityMotor.goalVelocity;
                error        = Vector3.Zero;
            }

            //Compute the maximum force that can be applied this frame.
            ComputeMaxForces(settings.maximumForce, dt);

            //COMPUTE EFFECTIVE MASS MATRIX
            //Transforms a change in velocity to a change in momentum when multiplied.
            Matrix3x3 linearComponent;

            Matrix3x3.CreateScale(entity.inverseMass, out linearComponent);
            Matrix3x3 rACrossProduct;

            Matrix3x3.CreateCrossProduct(ref r, out rACrossProduct);
            Matrix3x3 angularComponentA;

            Matrix3x3.Multiply(ref rACrossProduct, ref entity.inertiaTensorInverse, out angularComponentA);
            Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
            Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out effectiveMassMatrix);

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

            Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
        }