@genbox: Angle Joint Code

Coordinator
Aug 16, 2010 at 12:51 PM

Hey Ian,

Meant to get this to you earlier.  Here is the Angle joint code I was using in one of my projects. This was adapted to work with the older FP3 version of the engine but should be about the same.

Will probably need to clean it up a bit to fit into current version.

using System;
using System.Collections.Generic;
using System.Text;

using Microsoft.Xna.Framework;

using FarseerPhysics.Common;


namespace FarseerPhysics.Dynamics.Joints
{  
    public class AngleJoint : Joint
    {
        public float TargetAngle
        {
            get { return _targetAngle; }
            set
            {
                if (value != _targetAngle)
                {
                    _targetAngle = value;
                    WakeBodies();
                }
            }
        }
        public float BiasFactor;
        public float Softness;
        public float MaxImpulse;

        public AngleJoint(Body bodyA, Body bodyB)
            : base(bodyA, bodyB)
        {
            JointType = Joints.JointType.Angle;
            TargetAngle = 0;
            BiasFactor = .2f;
            Softness = 0f;
            MaxImpulse = float.MaxValue;            
        }

        public override Vector2 WorldAnchorA
        {
            get { return BodyA.Position; }
        }

        public override Vector2 WorldAnchorB
        {
            get { return BodyB.Position; }
        }

        public override Vector2 GetReactionForce(float inv_dt)
        {
            //TODO
            //return _inv_dt * _impulse;
            return new Vector2(0, 0);
        }

        public override float GetReactionTorque(float inv_dt)
        {
            return 0;
        }

        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            _jointError = (BodyB._sweep.Angle - BodyA._sweep.Angle - TargetAngle);
            //_jointError = (BodyB.GetAngle() - BodyA.GetAngle() - _targetAngle);

            _bias = -BiasFactor * step.Inv_DeltaTime * _jointError;

            _massFactor = (1 - Softness) / (BodyA._invI + BodyB._invI);
        }

        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            float p = (_bias - BodyB._angularVelocity + BodyA._angularVelocity) * _massFactor;
            BodyA._angularVelocity -= BodyA._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
            BodyB._angularVelocity += BodyB._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
        }

        internal override bool SolvePositionConstraints()
        {
            //no position solving for this joint
            return true;
        }

        private float _targetAngle;
        private float _bias;
        private float _jointError;
        private float _massFactor;
    }
}

 

Coordinator
Aug 16, 2010 at 12:55 PM
Nice work Jeff. I will add it to the engine for now. If you have any changes to the code, be sure to let me know.
Coordinator
Aug 17, 2010 at 12:23 AM

Should you by any chance have converted the springs to be FPE 3.0 compatible?

Aug 17, 2010 at 7:57 AM

Excellent, thanks a lot! :D

Aug 17, 2010 at 10:02 AM

Thanks for adding this. Any chance the Fixed Angle Joint will be in it soon? I need to change the rotation of a body and I read (in the F2.1 manual) this is the way to go .

Coordinator
Aug 17, 2010 at 4:07 PM
It can be done using the revolute joint in FPE 3.0, but it is way easier to use a fixed angle joint. I will take a look at it.
Coordinator
Aug 17, 2010 at 4:42 PM
Fixed Angle joint should be pretty similar to Angle Joint. Another option is to make one of the bodies angle joint is attached to static. Would essentially give you the same effect. -Jeff
Coordinator
Aug 17, 2010 at 5:52 PM
I've added the FixedAngleJoint to FPE 3.0.
Aug 18, 2010 at 8:23 AM

Jeff, that's what I have now, but I thought the fixed angle joint is a bit more elegant.

Thanks alot genbox!