BEPUphysics.Constraints.TwoEntity.JointLimits.RevoluteLimit.Update C# (CSharp) 메소드

Update() 공개 메소드

Performs the frame's configuration step.
public Update ( float dt ) : void
dt float Timestep duration.
리턴 void
        public override void Update(float dt)
        {
            //Transform the axes into world space.
            basis.rotationMatrix = connectionA.orientationMatrix;
            basis.ComputeWorldSpaceAxes();
            Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis);

            //Compute the plane normals.
            Vector3 minPlaneNormal, maxPlaneNormal;
            //Rotate basisA y axis around the basisA primary axis.
            Matrix rotation;
            Matrix.CreateFromAxisAngle(ref basis.primaryAxis, minimumAngle + MathHelper.PiOver2, out rotation);
            Vector3.TransformNormal(ref basis.xAxis, ref rotation, out minPlaneNormal);
            Matrix.CreateFromAxisAngle(ref basis.primaryAxis, maximumAngle - MathHelper.PiOver2, out rotation);
            Vector3.TransformNormal(ref basis.xAxis, ref rotation, out maxPlaneNormal);

            //Compute the errors along the two normals.
            float planePositionMin, planePositionMax;
            Vector3.Dot(ref minPlaneNormal, ref worldTestAxis, out planePositionMin);
            Vector3.Dot(ref maxPlaneNormal, ref worldTestAxis, out planePositionMax);


            float span = GetDistanceFromMinimum(maximumAngle);

            //Early out and compute the determine the plane normal.
            if (span >= MathHelper.Pi)
            {
                if (planePositionMax > 0 || planePositionMin > 0)
                {
                    //It's in a perfectly valid configuration, so skip.
                    isActiveInSolver = false;
                    minIsActive = false;
                    maxIsActive = false;
                    error = Vector2.Zero;
                    accumulatedImpulse = Vector2.Zero;
                    isLimitActive = false;
                    return;
                }

                if (planePositionMax > planePositionMin)
                {
                    //It's quicker to escape out to the max plane than the min plane.
                    error.X = 0;
                    error.Y = -planePositionMax;
                    accumulatedImpulse.X = 0;
                    minIsActive = false;
                    maxIsActive = true;
                }
                else
                {
                    //It's quicker to escape out to the min plane than the max plane.
                    error.X = -planePositionMin;
                    error.Y = 0;
                    accumulatedImpulse.Y = 0;
                    minIsActive = true;
                    maxIsActive = false;
                }
                //There's never a non-degenerate situation where having both planes active with a span 
                //greater than pi is useful.
            }
            else
            {
                if (planePositionMax > 0 && planePositionMin > 0)
                {
                    //It's in a perfectly valid configuration, so skip.
                    isActiveInSolver = false;
                    minIsActive = false;
                    maxIsActive = false;
                    error = Vector2.Zero;
                    accumulatedImpulse = Vector2.Zero;
                    isLimitActive = false;
                    return;
                }

                if (planePositionMin <= 0 && planePositionMax <= 0)
                {
                    //Escape upward.
                    //Activate both planes.
                    error.X = -planePositionMin;
                    error.Y = -planePositionMax;
                    minIsActive = true;
                    maxIsActive = true;
                }
                else if (planePositionMin <= 0)
                {
                    //It's quicker to escape out to the min plane than the max plane.
                    error.X = -planePositionMin;
                    error.Y = 0;
                    accumulatedImpulse.Y = 0;
                    minIsActive = true;
                    maxIsActive = false;
                }
                else
                {
                    //It's quicker to escape out to the max plane than the min plane.
                    error.X = 0;
                    error.Y = -planePositionMax;
                    accumulatedImpulse.X = 0;
                    minIsActive = false;
                    maxIsActive = true;
                }
            }
            isLimitActive = true;


            //****** VELOCITY BIAS ******//
            //Compute the correction velocity
            float errorReduction;
            springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);

            //Compute the jacobians
            if (minIsActive)
            {
                Vector3.Cross(ref minPlaneNormal, ref worldTestAxis, out jacobianMinA);
                if (jacobianMinA.LengthSquared() < Toolbox.Epsilon)
                {
                    //The plane normal is aligned with the test axis.
                    //Use the basis's free axis.
                    jacobianMinA = basis.primaryAxis;
                }
                jacobianMinA.Normalize();
                jacobianMinB.X = -jacobianMinA.X;
                jacobianMinB.Y = -jacobianMinA.Y;
                jacobianMinB.Z = -jacobianMinA.Z;
            }
            if (maxIsActive)
            {
                Vector3.Cross(ref maxPlaneNormal, ref worldTestAxis, out jacobianMaxA);
                if (jacobianMaxA.LengthSquared() < Toolbox.Epsilon)
                {
                    //The plane normal is aligned with the test axis.
                    //Use the basis's free axis.
                    jacobianMaxA = basis.primaryAxis;
                }
                jacobianMaxA.Normalize();
                jacobianMaxB.X = -jacobianMaxA.X;
                jacobianMaxB.Y = -jacobianMaxA.Y;
                jacobianMaxB.Z = -jacobianMaxA.Z;
            }

            //Error is always positive
            if (minIsActive)
            {
                biasVelocity.X = MathHelper.Min(MathHelper.Max(0, error.X - margin) * errorReduction, maxCorrectiveVelocity);
                if (bounciness > 0)
                {
                    float relativeVelocity;
                    float dot;
                    //Find the velocity contribution from each connection
                    Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out relativeVelocity);
                    Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out dot);
                    relativeVelocity += dot;
                    if (-relativeVelocity > bounceVelocityThreshold)
                        biasVelocity.X = MathHelper.Max(biasVelocity.X, -bounciness * relativeVelocity);
                }
            }
            if (maxIsActive)
            {
                biasVelocity.Y = MathHelper.Min(MathHelper.Max(0, error.Y - margin) * errorReduction, maxCorrectiveVelocity);
                if (bounciness > 0)
                {
                    //Find the velocity contribution from each connection
                    if (maxIsActive)
                    {
                        float relativeVelocity;
                        Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out relativeVelocity);
                        float dot;
                        Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out dot);
                        relativeVelocity += dot;
                        if (-relativeVelocity > bounceVelocityThreshold)
                            biasVelocity.Y = MathHelper.Max(biasVelocity.Y, -bounciness * relativeVelocity);
                    }
                }
            }


            //****** EFFECTIVE MASS MATRIX ******//
            //Connection A's contribution to the mass matrix
            float minEntryA, minEntryB;
            float maxEntryA, maxEntryB;
            Vector3 transformedAxis;
            if (connectionA.isDynamic)
            {
                if (minIsActive)
                {
                    Matrix3x3.Transform(ref jacobianMinA, ref connectionA.inertiaTensorInverse, out transformedAxis);
                    Vector3.Dot(ref transformedAxis, ref jacobianMinA, out minEntryA);
                }
                else
                    minEntryA = 0;
                if (maxIsActive)
                {
                    Matrix3x3.Transform(ref jacobianMaxA, ref connectionA.inertiaTensorInverse, out transformedAxis);
                    Vector3.Dot(ref transformedAxis, ref jacobianMaxA, out maxEntryA);
                }
                else
                    maxEntryA = 0;
            }
            else
            {
                minEntryA = 0;
                maxEntryA = 0;
            }
            //Connection B's contribution to the mass matrix
            if (connectionB.isDynamic)
            {
                if (minIsActive)
                {
                    Matrix3x3.Transform(ref jacobianMinB, ref connectionB.inertiaTensorInverse, out transformedAxis);
                    Vector3.Dot(ref transformedAxis, ref jacobianMinB, out minEntryB);
                }
                else
                    minEntryB = 0;
                if (maxIsActive)
                {
                    Matrix3x3.Transform(ref jacobianMaxB, ref connectionB.inertiaTensorInverse, out transformedAxis);
                    Vector3.Dot(ref transformedAxis, ref jacobianMaxB, out maxEntryB);
                }
                else
                    maxEntryB = 0;
            }
            else
            {
                minEntryB = 0;
                maxEntryB = 0;
            }
            //Compute the inverse mass matrix
            //Notice that the mass matrix isn't linked, it's two separate ones.
            velocityToImpulse.X = 1 / (softness + minEntryA + minEntryB);
            velocityToImpulse.Y = 1 / (softness + maxEntryA + maxEntryB);

            
        }