axiosengine/axios/Dynamics/Joints/AngleJoint.cs

93 lines
2.6 KiB
C#
Raw Normal View History

2012-03-19 23:57:59 +00:00
using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;
namespace FarseerPhysics.Dynamics.Joints
{
/// <summary>
/// Maintains a fixed angle between two bodies
/// </summary>
public class AngleJoint : Joint
{
public float BiasFactor;
public float MaxImpulse;
public float Softness;
private float _bias;
private float _jointError;
private float _massFactor;
private float _targetAngle;
internal AngleJoint()
{
JointType = JointType.Angle;
}
public AngleJoint(Body bodyA, Body bodyB)
: base(bodyA, bodyB)
{
JointType = JointType.Angle;
TargetAngle = 0;
BiasFactor = .2f;
Softness = 0f;
MaxImpulse = float.MaxValue;
}
public float TargetAngle
{
get { return _targetAngle; }
set
{
if (value != _targetAngle)
{
_targetAngle = value;
WakeBodies();
}
}
}
public override Vector2 WorldAnchorA
{
get { return BodyA.Position; }
}
public override Vector2 WorldAnchorB
{
get { return BodyB.Position; }
set { Debug.Assert(false, "You can't set the world anchor on this joint type."); }
}
public override Vector2 GetReactionForce(float inv_dt)
{
//TODO
//return _inv_dt * _impulse;
return Vector2.Zero;
}
public override float GetReactionTorque(float inv_dt)
{
return 0;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
_jointError = (BodyB.Sweep.A - BodyA.Sweep.A - TargetAngle);
_bias = -BiasFactor * step.inv_dt * _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;
}
}
}