[Solved] Creating a PrismaticJoint in FP2.1 similar to Box2Dx

Topics: Developer Forum
Aug 22, 2009 at 9:53 PM

Hello,

 

Could I enlist some help in porting the PrismaticJoint from Box2Dx to FP2.1?  I've taken a stab at it myself but I've failed hopelessly.  I suspect I'm missing the _accumulatedImpulse and _velocityBias components that I see in the other FP2.1 Joints, but frankly I just don't understand what I'm doing (nevermind understand what I'm doing wrong) .. the Joint promptly explodes and throws a NaN error on the AngularVelocity of my body.  I've excluded the Motor and Limit calculations to keep things as simple as possible, and I didn't port SolvePositionConstraints as it doesn't seem to affect the state.  If anyone could lend a hand, I'd be grateful!  Here is what I've tried ..

PrismaticJoint.cs

using System;
using System.Diagnostics;
using FarseerGames.FarseerPhysics.Mathematics;

#if (XNA)
using Microsoft.Xna.Framework;
#else
using FarseerGames.FarseerPhysics.Mathematics;
#endif

namespace FarseerGames.FarseerPhysics.Dynamics.Joints
{
    /// <summary>
    /// A 3-by-3 matrix. Stored in column-major order.
    /// </summary>
    public struct Mat33
    {
        /// <summary>
        /// Construct this matrix using columns.
        /// </summary>
        public Mat33(Vector3 c1, Vector3 c2, Vector3 c3)
        {
            Col1 = c1;
            Col2 = c2;
            Col3 = c3;
        }

        /// <summary>
        /// Set this matrix to all zeros.
        /// </summary>
        public void SetZero()
        {
            Col1 = Vector3.Zero;
            Col2 = Vector3.Zero;
            Col3 = Vector3.Zero;
        }

        /// <summary>
        /// Solve A * x = b, where b is a column vector. This is more efficient
        /// than computing the inverse in one-shot cases. Solve only the upper
        /// 2-by-2 matrix equation.
        /// </summary>
        public Vector2 Solve22(Vector2 b)
        {
            float a11 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.Y;
            float det = a11 * a22 - a12 * a21;
            det = 1.0f / det;
            Vector2 x = new Vector2();
            x.X = det * (a22 * b.X - a12 * b.Y);
            x.Y = det * (a11 * b.Y - a21 * b.X);
            return x;
        }

        public Vector3 Col1, Col2, Col3;
    }

    class PrismaticJoint : Joint
    {
        private Body _body1;
        private Body _body2;
        public Vector2 _localAnchor1;
        public Vector2 _localAnchor2;

        public PrismaticJoint()
        {
        }

        public PrismaticJoint(Body body1, Body body2, Vector2 anchor, Vector2 direction)
        {
            _body1 = body1;
            _body2 = body2;

            body1.GetLocalPosition(ref anchor, out _localAnchor1);
            body2.GetLocalPosition(ref anchor, out _localAnchor2);
            _localXAxis1 = direction;
            _localYAxis1 = Cross(1.0f, _localXAxis1);
        }

        /// <summary>
        /// Gets or sets the first body.
        /// </summary>
        /// <Value>The body1.</Value>
        public Body Body1
        {
            get { return _body1; }
            set { _body1 = value; }
        }

        /// <summary>
        /// Gets or sets the second body.
        /// </summary>
        /// <Value>The body2.</Value>
        public Body Body2
        {
            get { return _body2; }
            set { _body2 = value; }
        }


        /// <summary>
        /// Validates this instance.
        /// </summary>
        public override void Validate()
        {
            if (_body1.IsDisposed || _body2.IsDisposed)
            {
                Dispose();
            }
        }

        Vector2 _localCenter1, _localCenter2, _localXAxis1, _localYAxis1, _axis, _perp;
        float _a1, _a2, _s1, _s2;
        float _invMass1, _invI1;
        float _invMass2, _invI2;
        Mat33 _K = new Mat33();
        Vector2 _impulse;

        public static Vector2 Cross(float s, Vector2 a)
        {
            return new Vector2(-s * a.Y, s * a.X);
        }

        public static float Cross(Vector2 a, Vector2 b)
        {
            return a.X * b.Y - a.Y * b.X;
        }

        /// <summary>
        /// Calculates all the work needed before updating the joint.
        /// </summary>
        /// <param name="inverseDt">The inverse dt.</param>
        public override void PreStep(float inverseDt)
        {
            if (IsDisposed)
                return;

            _localCenter1 = new Vector2(0, 0);
            _localCenter2 = new Vector2(0, 0);

            // Compute the effective masses.
            float R1x = (_localAnchor1.X * _body1.GetBodyMatrix().M11) + (_localAnchor1.Y * _body1.GetBodyMatrix().M21);
            float R1y = (_localAnchor1.X * _body1.GetBodyMatrix().M12) + (_localAnchor1.Y * _body1.GetBodyMatrix().M22);

            float R2x = (_localAnchor2.X * _body2.GetBodyMatrix().M11) + (_localAnchor2.Y * _body2.GetBodyMatrix().M21);
            float R2y = (_localAnchor2.X * _body2.GetBodyMatrix().M12) + (_localAnchor2.Y * _body2.GetBodyMatrix().M22);

            Vector2 r1 = new Vector2(R1x, R1y), r2 = new Vector2(R2x, R2y);
            Vector2 d = r2 - r1;

            _invMass1 = Body1.inverseMass;
            _invI1 = Body1.inverseMomentOfInertia;
            _invMass2 = Body2.inverseMass;
            _invI2 = Body2.inverseMomentOfInertia;

            // Compute motor Jacobian and effective mass.
            {
                _axis.X = (_localXAxis1.X * _body1.GetBodyMatrix().M11) + (_localXAxis1.Y * _body1.GetBodyMatrix().M21);
                _axis.Y = (_localXAxis1.X * _body1.GetBodyMatrix().M12) + (_localXAxis1.Y * _body1.GetBodyMatrix().M22);

                _a1 = Cross(d + r1, _axis);
                _a2 = Cross(r2, _axis);
            }

            // Prismatic constraint.
            {
                _perp.X = (_localYAxis1.X * _body1.GetBodyMatrix().M11) + (_localYAxis1.Y * _body1.GetBodyMatrix().M21);
                _perp.Y = (_localYAxis1.X * _body1.GetBodyMatrix().M12) + (_localYAxis1.Y * _body1.GetBodyMatrix().M22);

                _s1 = Cross(d + r1, _perp);
                _s2 = Cross(r2, _perp);

                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 + i2 * _s2;
                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = i1 + i2;
                float k23 = i1 * _a1 + i2 * _a2;
                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.Col1 = new Vector3(k11, k12, k13);
                _K.Col2 = new Vector3(k12, k22, k23);
                _K.Col3 = new Vector3(k13, k23, k33);
            }

            _impulse.X = 0;
            _impulse.Y = 0;
        }

        /// <summary>
        /// Updates this instance.
        /// </summary>
        public override void Update()
        {
            base.Update();

            if (IsDisposed)
                return;

            Body b1 = Body1;
            Body b2 = Body2;

            Vector2 v1 = b1.LinearVelocity;
            float w1 = b1.AngularVelocity;
            Vector2 v2 = b2.LinearVelocity;
            float w2 = b2.AngularVelocity;

            Vector2 Cdot1 = new Vector2();
            Cdot1.X = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
            Cdot1.Y = w2 - w1;

            // Limit is inactive, just solve the prismatic constraint in block form.
            Vector2 df = _K.Solve22(-Cdot1);

            _impulse.X += df.X;
            _impulse.Y += df.Y;

            Vector2 P = df.X * _perp;
            float L1 = df.X * _s1 + df.Y;
            float L2 = df.X * _s2 + df.Y;

            v1 -= _invMass1 * P;
            w1 -= _invI1 * L1;

            v2 += _invMass2 * P;
            w2 += _invI2 * L2;

            b1.LinearVelocity = v1;
            b1.AngularVelocity = w1;
            b2.LinearVelocity = v2;
            b2.AngularVelocity = w2;
        }
    }
}

 

Coordinator
Aug 22, 2009 at 10:04 PM

Can't you just use the sliderjoint? they should be identical in functionality (when you exclude the motor and added stability)

Aug 22, 2009 at 10:08 PM

Not really - the SliderJoint allows rotation between the two bodies.  The PrismaticJoint keeps the rotation of the two bodies constant and only allows translation along the axis given.  Even an AngleJoint and SliderJoint together won't accomplish what I want - it allows translation in two dimensions (provided the two bodies satisfy the min/max constraint).  I think an AngleJoint and PinJoint would get close, but doesn't allow any translation at all (would effectively weld the two bodies together, I suppose).

 

Aug 22, 2009 at 10:26 PM

Here's a video of the type of Joint I want to build: http://tinypic.com/player.php?v=5yaihx&s=3

 

 

Coordinator
Aug 22, 2009 at 11:19 PM

Ok, I thought you were just after the translation effect. I can't help you port over the joint to 2.1, I simply don't have the time as I'm working on 3.0 (that includes the prismatic joint).

Aug 24, 2009 at 12:14 AM

Understandable .. I would switch to 3.0 but I have too much invested in 2.1 (not to mention that 3.0 isn't done :).  Great job btw and thank you very much for all the hard work.  FWIW, I managed to hack a SliderJoint into a RevoluteJoint that's constrained along a given axis.  Two of them together (with different anchors for body2) work exactly like a PrismaticJoint.  Here's the code for anyone that wants to use it:

 

using System;
using System.Diagnostics;
using FarseerGames.FarseerPhysics.Mathematics;

#if (XNA)
using Microsoft.Xna.Framework;
#else
using FarseerGames.FarseerPhysics.Mathematics;
#endif

namespace FarseerGames.FarseerPhysics.Dynamics.Joints
{
    class PrismaticJoint : Joint
    {
        private float _accumulatedImpulse;
        private Vector2 _anchor;
        private Vector2 _anchor1;
        private Vector2 _anchor2;
        private Body _body1;
        private Body _body2;
        private float _effectiveMass;
        private Vector2 _r1;
        private Vector2 _r2;
        private float _slop = .01f;
        private float _velocityBias;
        private Vector2 _worldAnchor1;
        private Vector2 _worldAnchor2;
        private Vector2 _localDirection1;
        private Vector2 _worldAnchorDifferenceNormalized;

        public PrismaticJoint()
        {
        }

        public PrismaticJoint(Body body1, Body body2, Vector2 anchor1, Vector2 anchor2, Vector2 localDirection)
        {
            _body1 = body1;
            _body2 = body2;

            _anchor1 = anchor1;
            _anchor2 = anchor2;

            _localDirection1 = localDirection;
        }

        /// <summary>
        /// Gets or sets the first body.
        /// </summary>
        /// <Value>The body1.</Value>
        public Body Body1
        {
            get { return _body1; }
            set { _body1 = value; }
        }

        /// <summary>
        /// Gets or sets the second body.
        /// </summary>
        /// <Value>The body2.</Value>
        public Body Body2
        {
            get { return _body2; }
            set { _body2 = value; }
        }


        /// <summary>
        /// Validates this instance.
        /// </summary>
        public override void Validate()
        {
            if (_body1.IsDisposed || _body2.IsDisposed)
            {
                Dispose();
            }
        }

        ///********************************************************************
        //Desc: Taken from comp.graphics.algorithms FAQ
        //*********************************************************************/
        public static Vector2 ClosestPointOnLineSegment(
                     ref Vector2 lineA,
                     ref Vector2 lineB,
                     ref Vector2 point)
        {
            Vector2 v = lineB - lineA;
            v.Normalize();
            float t = Vector2.Dot(v, point - lineA);
            return lineA + v * t;
        }

        /// <summary>
        /// Calculates all the work needed before updating the joint.
        /// </summary>
        /// <param name="inverseDt">The inverse dt.</param>
        public override void PreStep(float inverseDt)
        {
            if (IsDisposed)
                return;

            //calc _r1 and _r2 from the anchors
            _body1.GetBodyMatrix(out _body1MatrixTemp);
            _body2.GetBodyMatrix(out _body2MatrixTemp);
            Vector2.TransformNormal(ref _anchor1, ref _body1MatrixTemp, out _r1);
            Vector2.TransformNormal(ref _anchor2, ref _body2MatrixTemp, out _r2);

            //calc the diff between _anchor positions
            Vector2.Add(ref _body1.position, ref _r1, out _worldAnchor1);
            Vector2.Add(ref _body2.position, ref _r2, out _worldAnchor2);

            //  Find the nearest point along the constraint axis
            Vector2 worldAnchor1Direction = _body1.GetWorldPosition(_anchor1 + _localDirection1);
            Vector2 closestPt = ClosestPointOnLineSegment(ref _worldAnchor1, ref worldAnchor1Direction, ref _worldAnchor2);
            _worldAnchor1 = closestPt;
            _r1 = _worldAnchor1 - Body1.position;

            Vector2.Subtract(ref _worldAnchor2, ref _worldAnchor1, out _worldAnchorDifference);

            _distance = _worldAnchorDifference.Length();
            JointError = 0;

            if (_distance < _slop)
            {
                JointError = 0; //allow some _slop 
                _accumulatedImpulse = 0;
            }
            else
            {
                JointError = _distance;
            }

            //normalize the difference vector
            Vector2.Multiply(ref _worldAnchorDifference, 1 / (_distance != 0 ? _distance : float.PositiveInfinity),
                             out _worldAnchorDifferenceNormalized); //_distance = 0 --> error (fix) 

            //calc velocity bias
            _velocityBias = BiasFactor * inverseDt * (JointError);

            //calc mass normal (effective mass in relation to constraint)
            Calculator.Cross(ref _r1, ref _worldAnchorDifferenceNormalized, out _r1cn);
            Calculator.Cross(ref _r2, ref _worldAnchorDifferenceNormalized, out _r2cn);
            _kNormal = _body1.inverseMass + _body2.inverseMass + _body1.inverseMomentOfInertia * _r1cn * _r1cn +
                       _body2.inverseMomentOfInertia * _r2cn * _r2cn;
            _effectiveMass = (1) / (_kNormal + Softness);

            //convert scalar accumulated _impulse to vector
            Vector2.Multiply(ref _worldAnchorDifferenceNormalized, _accumulatedImpulse, out _accumulatedImpulseVector);

            //apply accumulated impulses (warm starting)
            _body2.ApplyImmediateImpulse(ref _accumulatedImpulseVector);
            Calculator.Cross(ref _r2, ref _accumulatedImpulseVector, out _angularImpulse);
            _body2.ApplyAngularImpulse(_angularImpulse);

            Vector2.Multiply(ref _accumulatedImpulseVector, -1, out _accumulatedImpulseVector);
            _body1.ApplyImmediateImpulse(ref _accumulatedImpulseVector);
            Calculator.Cross(ref _r1, ref _accumulatedImpulseVector, out _angularImpulse);
            _body1.ApplyAngularImpulse(_angularImpulse);
        }

        /// <summary>
        /// Updates this instance.
        /// </summary>
        public override void Update()
        {
            base.Update();

            if (IsDisposed)
                return;

            //calc velocity _anchor points (angular component + linear)
            Calculator.Cross(ref _body1.AngularVelocity, ref _r1, out _angularVelocityComponent1);
            Vector2.Add(ref _body1.LinearVelocity, ref _angularVelocityComponent1, out _velocity1);

            Calculator.Cross(ref _body2.AngularVelocity, ref _r2, out _angularVelocityComponent2);
            Vector2.Add(ref _body2.LinearVelocity, ref _angularVelocityComponent2, out _velocity2);

            //calc velocity difference
            Vector2.Subtract(ref _velocity2, ref _velocity1, out _dv);

            //map the velocity difference into constraint space
            Vector2.Dot(ref _dv, ref _worldAnchorDifferenceNormalized, out _dvNormal);

            //calc the _impulse magnitude
            _impulseMagnitude = (-_velocityBias - _dvNormal - Softness * _accumulatedImpulse) * _effectiveMass;
            //_softness not implemented correctly yet

            float oldAccumulatedImpulse = _accumulatedImpulse;

            _accumulatedImpulse = Math.Min(oldAccumulatedImpulse + _impulseMagnitude, 0);
            _impulseMagnitude = _accumulatedImpulse - oldAccumulatedImpulse;

            //convert scalar _impulse to vector
            Vector2.Multiply(ref _worldAnchorDifferenceNormalized, _impulseMagnitude, out _impulse);

            //apply _impulse
            _body2.ApplyImmediateImpulse(ref _impulse);
            Calculator.Cross(ref _r2, ref _impulse, out _angularImpulse);
            _body2.ApplyAngularImpulse(_angularImpulse);

            Vector2.Multiply(ref _impulse, -1, out _impulse);
            _body1.ApplyImmediateImpulse(ref _impulse);
            Calculator.Cross(ref _r1, ref _impulse, out _angularImpulse);
            _body1.ApplyAngularImpulse(_angularImpulse);
        }

        #region Update variables

        private Vector2 _angularVelocityComponent1;
        private Vector2 _angularVelocityComponent2;
        private Vector2 _dv;
        private float _dvNormal;
        private Vector2 _impulse;
        private float _impulseMagnitude;
        private Vector2 _velocity1;
        private Vector2 _velocity2;

        #endregion

        #region PreStep variables

        private Vector2 _accumulatedImpulseVector;
        private float _angularImpulse;
        private Matrix _body1MatrixTemp;

        private Matrix _body2MatrixTemp;
        private float _distance;
        private float _kNormal;

        private float _r1cn;
        private float _r2cn;
        private Vector2 _worldAnchorDifference;

        #endregion
    }
}