Exemplo n.º 1
0
        public override void SolveConstraint(float timeStep)
        {
            float tau     = 0.1f;
            float damping = 1.0f;

            Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
            Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);

            Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
            Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;

            Vector3 localNormalInA = new Vector3();

            // linear
            for (int i = 0; i < 3; i++)
            {
                if (IsLimited(i))
                {
                    Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
                    Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));

                    if (i == 0)
                    {
                        localNormalInA = new Vector3(1, 0, 0);
                    }
                    else if (i == 1)
                    {
                        localNormalInA = new Vector3(0, 1, 0);
                    }
                    else
                    {
                        localNormalInA = new Vector3(0, 0, 1);
                    }

                    Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);

                    float jacDiagABInv = 1f / _jacLinear[i].Diagonal;

                    //velocity error (first order error)
                    float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
                                                                      RigidBodyB.LinearVelocity, angvelB);

                    //positional error (zeroth order error)
                    float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld);
                    float lo    = -1e30f;
                    float hi    = 1e30f;

                    //handle the limits
                    if (_lowerLimit[i] < _upperLimit[i])
                    {
                        if (depth > _upperLimit[i])
                        {
                            depth -= _upperLimit[i];
                            lo     = 0f;
                        }
                        else
                        {
                            if (depth < _lowerLimit[i])
                            {
                                depth -= _lowerLimit[i];
                                hi     = 0f;
                            }
                            else
                            {
                                continue;
                            }
                        }
                    }

                    float normalImpulse    = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv;
                    float oldNormalImpulse = _accumulatedImpulse[i];
                    float sum = oldNormalImpulse + normalImpulse;
                    _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum;
                    normalImpulse          = _accumulatedImpulse[i] - oldNormalImpulse;

                    Vector3 impulse_vector = normalWorld * normalImpulse;
                    RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
                    RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
                }
            }

            Vector3 axis;
            float   angle;
            Matrix  frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA;
            Matrix  frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB;

            TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle);
            Quaternion diff    = new Quaternion(axis, angle);
            Matrix     diffMat = Matrix.CreateFromQuaternion(diff);
            Vector3    xyz;

            // this is not perfect, we can first check which axis are limited, and choose a more appropriate order
            MatrixToEulerXYZ(diffMat, out xyz);

            // angular
            for (int i = 0; i < 3; i++)
            {
                if (IsLimited(i + 3))
                {
                    Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
                    Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));

                    float jacDiagABInv = 1f / _jacAng[i].Diagonal;

                    //velocity error (first order error)
                    float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
                                                                   RigidBodyB.LinearVelocity, angvelB);

                    //positional error (zeroth order error)
                    Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
                    Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);

                    float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB);

                    float lo = -1e30f;
                    float hi = 1e30f;

                    //handle the twist limit
                    if (_lowerLimit[i + 3] < _upperLimit[i + 3])
                    {
                        //clamp the values
                        float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f;
                        float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f;

                        float projAngle;

                        if (i == 0)
                        {
                            projAngle = -2f * xyz.Z;
                        }
                        else if (i == 1)
                        {
                            projAngle = -2f * xyz.Y;
                        }
                        else
                        {
                            projAngle = -2f * xyz.Z;
                        }

                        if (projAngle < loLimit)
                        {
                            hi      = 0f;
                            rel_pos = loLimit - projAngle;
                        }
                        else
                        {
                            if (projAngle > hiLimit)
                            {
                                lo      = 0f;
                                rel_pos = (hiLimit - projAngle);
                            }
                            else
                            {
                                continue;
                            }
                        }
                    }

                    //impulse

                    float normalImpulse    = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv;
                    float oldNormalImpulse = _accumulatedImpulse[i + 3];
                    float sum = oldNormalImpulse + normalImpulse;
                    _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum;
                    normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse;

                    Vector3 axis2          = _sign[i] * Vector3.Cross(axisA, axisB);
                    Vector3 impulse_vector = axis2 * normalImpulse;

                    RigidBodyA.ApplyTorqueImpulse(impulse_vector);
                    RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
                }
            }
        }
Exemplo n.º 2
0
        public override void SolveConstraint(float timeStep)
        {
            Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
            Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);

            Vector3 normal  = new Vector3(0, 0, 0);
            float   tau     = 0.3f;
            float   damping = 1f;

            //linear part
            if (!_angularOnly)
            {
                for (int i = 0; i < 3; i++)
                {
                    if (i == 0)
                    {
                        normal = new Vector3(1, 0, 0);
                    }
                    else if (i == 1)
                    {
                        normal = new Vector3(0, 1, 0);
                    }
                    else
                    {
                        normal = new Vector3(0, 0, 1);
                    }

                    float jacDiagABInv = 1f / _jac[i].Diagonal;

                    Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
                    Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;

                    Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
                    Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
                    Vector3 vel  = vel1 - vel2;
                    float   rel_vel;
                    rel_vel = Vector3.Dot(normal, vel);
                    //positional error (zeroth order error)
                    float depth   = -Vector3.Dot(pivotAInW - pivotBInW, normal);                   //this is the error projected on the normal
                    float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
                    AppliedImpulse += impulse;
                    Vector3 impulse_vector = normal * impulse;
                    RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition);
                    RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition);
                }
            }
            //solve angular part
            // get axes in world space
            Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
            Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform);

            Vector3 angVelA = RigidBodyA.AngularVelocity;
            Vector3 angVelB = RigidBodyB.AngularVelocity;
            Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA);
            Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB);

            Vector3 angAOrthog   = angVelA - angVelAroundHingeAxisA;
            Vector3 angBOrthog   = angVelB - angVelAroundHingeAxisB;
            Vector3 velrelOrthog = angAOrthog - angBOrthog;

            //solve angular velocity correction
            float relaxation = 1f;
            float len        = velrelOrthog.Length();

            if (len > 0.00001f)
            {
                Vector3 normal2 = Vector3.Normalize(velrelOrthog);
                float   denom   = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
                                  RigidBodyB.ComputeAngularImpulseDenominator(normal2);
                // scale for mass and relaxation
                velrelOrthog *= (1f / denom) * 0.9f;
            }

            //solve angular positional correction
            Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep);
            float   len2         = angularError.Length();

            if (len2 > 0.00001f)
            {
                Vector3 normal2 = Vector3.Normalize(angularError);
                float   denom2  = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
                                  RigidBodyB.ComputeAngularImpulseDenominator(normal2);
                angularError *= (1f / denom2) * relaxation;
            }

            RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError);
            RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError);

            //apply motor
            if (_enableAngularMotor)
            {
                //todo: add limits too
                Vector3 angularLimit = Vector3.Zero;

                Vector3 velrel     = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
                float   projRelVel = Vector3.Dot(velrel, axisA);

                float desiredMotorVel = _motorTargetVelocity;
                float motorRelvel     = desiredMotorVel - projRelVel;

                float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) +
                               RigidBodyB.ComputeAngularImpulseDenominator(axisA);

                float unclippedMotorImpulse = (1f / denom3) * motorRelvel;
                //todo: should clip against accumulated impulse
                float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse;
                clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse;
                Vector3 motorImp = clippedMotorImpulse * axisA;

                RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit);
                RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit);
            }
        }
Exemplo n.º 3
0
        public override void BuildJacobian()
        {
            Vector3 localNormalInA = new Vector3(0, 0, 0);

            Vector3 pivotInA = _frameInA.Translation;
            Vector3 pivotInB = _frameInB.Translation;

            Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
            Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);

            Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
            Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;

            //linear part
            for (int i = 0; i < 3; i++)
            {
                if (IsLimited(i))
                {
                    if (i == 0)
                    {
                        localNormalInA = new Vector3(1, 0, 0);
                    }
                    else if (i == 1)
                    {
                        localNormalInA = new Vector3(0, 1, 0);
                    }
                    else
                    {
                        localNormalInA = new Vector3(0, 0, 1);
                    }

                    Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);

                    // Create linear atom
                    _jacLinear[i] = new JacobianEntry(
                        MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
                        MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
                        MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
                        MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
                        normalWorld,
                        RigidBodyA.InvInertiaDiagLocal,
                        RigidBodyA.InverseMass,
                        RigidBodyB.InvInertiaDiagLocal,
                        RigidBodyB.InverseMass);

                    //optionally disable warmstarting
                    _accumulatedImpulse[i] = 0f;

                    // Apply accumulated impulse
                    Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld;

                    RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
                    RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
                }
            }

            // angular part
            for (int i = 0; i < 3; i++)
            {
                if (IsLimited(i + 3))
                {
                    Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
                    Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);

                    Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB);

                    // Create angular atom
                    _jacAng[i] = new JacobianEntry(axis,
                                                   MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
                                                   MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
                                                   RigidBodyA.InvInertiaDiagLocal,
                                                   RigidBodyB.InvInertiaDiagLocal);

                    _accumulatedImpulse[i + 3] = 0f;

                    // Apply accumulated impulse
                    Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis;

                    RigidBodyA.ApplyTorqueImpulse(impulse_vector);
                    RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
                }
            }
        }