Updated Hinge Controller
PGlynn 1255841694|%e %b %Y, %H:%M %Z|agohover
this comment was part of the HingeJoint controller
/// TODO at the moment the bodies should be unrotated when this is
/// called...
trying to setup a model bone heirarchy made this a problem because it wouldnt really be ideal to have to un orientate everything so i made this new controller that does account for rotation.
it has two initialize functions, one for relative and one for world based positions on the hinge and hingeDirection( note: i think hingeAxis is misleading because its not actually the rotation axis as you would think. Its actually the direction of the hinge and where the fwd and bck angles take place from )
i would like to set the up so you could enter the axis of rotation, but havnt been able to figure that out yet. I'm still not sure if the MAX_HINGE_ANGLE needs to be in place. anyways heres my updated class for anyone who is intrested.
#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using JigLibX.Math;
#endregion
namespace JigLibX.Physics
{
/// <summary>
/// Implements a simple hinge between two rigid bodies. The bodies
/// should be in a suitable configuration when this joint is created.
/// </summary>
public class WorldHingeJoint : Joint
{
private Vector3 hingeDirection;
private Vector3 hingePosRel0;
private Body body0;
private Body body1;
private bool usingLimit;
private bool hingeEnabled;
private bool broken;
private float damping;
private float extraTorque; // allow extra torque applied per update
/// PGlynn : from original JigLib hingejoint.hpp
/// TODO at the moment the bodies should be unrotated when this is
/// called... No angle limit if fwd > MAX_HINGE_ANGLE_LIMIT after
/// calling initialise the joint will be functional -
/// i.e. registered... so the bodies should be registered too! If
/// damping > 0 then the joint will be damped such that the torque
/// is equal to -damping * relative-rotation of the bodies
/// involved. The actual value is clamped to prevent instability.
const float MAX_HINGE_ANGLE_LIMIT = 150.0f;
private ConstraintPoint mMidPointConstraint;
private ConstraintMaxDistance[] mSidePointConstraints;
private ConstraintMaxDistance mMaxDistanceConstraint;
/// <summary>
/// default constructor so you can initialise this joint later
/// </summary>
public WorldHingeJoint()
{
}
static Matrix GetBodyTransformMatrix(Body body)
{
Matrix ret = body.Orientation;
ret.Translation = body.Position;
return ret;
}
// works like the standard jiglib Initialise
public void InitializeRelative(Body body0, Body body1, Vector3 hingeDirectionRel0, Vector3 hingePosRel0,
float hingeHalfWidth, float hingeFwdAngle, float hingeBckAngle, float sidewaysSlack, float damping)
{
InitializeWorld(body0, body1,
Vector3.TransformNormal(hingeDirectionRel0, body0.Orientation),
Vector3.Transform(hingePosRel0, GetBodyTransformMatrix(body0)),
hingeHalfWidth, hingeFwdAngle, hingeBckAngle, sidewaysSlack, damping);
}
// hingePos is a world coordinate no longer relative.
public void InitializeWorld(Body body0, Body body1, Vector3 hingeDirection, Vector3 hingePos,
float hingeHalfWidth, float hingeFwdAngle, float hingeBckAngle, float sidewaysSlack, float damping)
{
this.body0 = body0;
this.body1 = body1;
hingeDirection.Normalize();
this.hingeDirection = hingeDirection;
//this.hingePosRel0 = hingePosRel0;
this.usingLimit = false;
this.damping = damping;
// tScalar allowedDistance = 0.005f;
this.hingeDirection.Normalize();
Matrix body0InverseTransform = Matrix.Invert(GetBodyTransformMatrix(body0));//.TransformMatrix;
Matrix body1InverseTransform = Matrix.Invert(GetBodyTransformMatrix(body1));
Vector3.Transform(ref hingePos, ref body0InverseTransform, out this.hingePosRel0);
Vector3 hingePosRel1;// = body0.Position + hingePosRel0 - body1.Position;
Vector3.Transform(ref hingePos, ref body1InverseTransform, out hingePosRel1);
Vector3 hingeAxisRel0;
Vector3 hingeAxisRel1;
Vector3.TransformNormal(ref hingeDirection, ref body0InverseTransform, out hingeAxisRel0);
Vector3.TransformNormal(ref hingeDirection, ref body1InverseTransform, out hingeAxisRel1);
// generate the two positions relative to each body
Vector3 relPos0a = hingePosRel0 + hingeHalfWidth * hingeAxisRel0;
Vector3 relPos0b = hingePosRel0 - hingeHalfWidth * hingeAxisRel0;
Vector3 relPos1a = hingePosRel1 + hingeHalfWidth * hingeAxisRel1;
Vector3 relPos1b = hingePosRel1 - hingeHalfWidth * hingeAxisRel1;
float timescale = 1.0f / 20.0f;
float allowedDistanceMid = 0.005f;
float allowedDistanceSide = sidewaysSlack * hingeHalfWidth;
mSidePointConstraints = new ConstraintMaxDistance[2];
mSidePointConstraints[0] = new ConstraintMaxDistance();
mSidePointConstraints[1] = new ConstraintMaxDistance();
mSidePointConstraints[0].Initialise(body0, relPos0a, body1, relPos1a, allowedDistanceSide);
mSidePointConstraints[1].Initialise(body0, relPos0b, body1, relPos1b, allowedDistanceSide);
mMidPointConstraint = new ConstraintPoint();
mMidPointConstraint.Initialise(body0, hingePosRel0, body1, hingePosRel1, allowedDistanceMid, timescale);
if (hingeFwdAngle <= MAX_HINGE_ANGLE_LIMIT) // MAX_HINGE_ANGLE_LIMIT
{
// choose a direction that is perpendicular to the hinge
Vector3 perpDir = Vector3.Up;
if (Vector3.Dot(perpDir, hingeDirection) > 0.1f)
perpDir = Vector3.Right;
// now make it perpendicular to the hinge
Vector3 sideAxis = Vector3.Cross(hingeDirection, perpDir);
perpDir = Vector3.Cross(sideAxis, hingeDirection);
perpDir.Normalize();
// the length of the "arm" TODO take this as a parameter? what's
// the effect of changing it?
float len = 10.0f * hingeHalfWidth;
// Choose a position using that dir. this will be the anchor point
// for body 0. relative to hinge
Vector3 hingeRelAnchorPos0 = perpDir * len;
// anchor point for body 2 is chosen to be in the middle of the
// angle range. relative to hinge
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
Vector3 hingeRelAnchorPos1 = Vector3.Transform(hingeRelAnchorPos0, Matrix.CreateFromAxisAngle(hingeDirection, MathHelper.ToRadians(-angleToMiddle)));
// work out the "string" length
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * (float)System.Math.Sin(MathHelper.ToRadians(hingeHalfAngle * 0.5f));
//Vector3 hingePos = body1.Position + hingePosRel0;
Vector3 relPos0c = Vector3.Transform(hingePos + hingeRelAnchorPos0, body0InverseTransform);// -body0.Position;
Vector3 relPos1c = Vector3.Transform(hingePos + hingeRelAnchorPos1, body1InverseTransform);// -body1.Position;
mMaxDistanceConstraint = new ConstraintMaxDistance();
mMaxDistanceConstraint.Initialise(body0, relPos0c, body1, relPos1c, allowedDistance);
usingLimit = true;
}
if (this.damping <= 0.0f)
this.damping = -1.0f; // just make sure that a value of 0.0 doesn't mess up...
else
this.damping = MathHelper.Clamp(this.damping, 0, 1);
}
/// <summary>
/// Register the constraints
/// </summary>
public void EnableHinge()
{
if (hingeEnabled)
return;
if (body0 != null)
{
mMidPointConstraint.EnableConstraint();
mSidePointConstraints[0].EnableConstraint();
mSidePointConstraints[1].EnableConstraint();
if (usingLimit && !broken)
mMaxDistanceConstraint.EnableConstraint();
EnableController();
}
hingeEnabled = true;
}
/// <summary>
/// deregister the constraints
/// </summary>
public void DisableHinge()
{
if (!hingeEnabled)
return;
if (body0 != null)
{
mMidPointConstraint.DisableConstraint();
mSidePointConstraints[0].DisableConstraint();
mSidePointConstraints[1].DisableConstraint();
if (usingLimit && !broken)
mMaxDistanceConstraint.DisableConstraint();
DisableController();
}
hingeEnabled = false;
}
/// <summary>
/// Just remove the limit constraint
/// </summary>
public void Break()
{
if (broken)
return;
if (usingLimit)
mMaxDistanceConstraint.DisableConstraint();
broken = true;
}
/// <summary>
/// Just enable the limit constraint
/// </summary>
public void Mend()
{
if (!broken)
return;
if (usingLimit)
mMaxDistanceConstraint.EnableConstraint();
broken = false;
}
public override void UpdateController(float dt)
{
if (body0 == null || body1 == null)
return;
//Assert(0 != mBody0);
//Assert(0 != mBody1);
if (damping > 0.0f)
{
// Some hinges can bend in wonky ways. Derive the effective hinge axis
// using the relative rotation of the bodies.
Vector3 hingeAxis = body1.AngularVelocity - body0.AngularVelocity;
JiggleMath.NormalizeSafe(ref hingeAxis);
float angRot1;//
Vector3.Dot(ref body0.transformRate.AngularVelocity, ref hingeAxis, out angRot1);
float angRot2;
Vector3.Dot(ref body1.transformRate.AngularVelocity, ref hingeAxis, out angRot2);
float avAngRot = 0.5f * (angRot1 + angRot2);
float frac = 1.0f - damping;
float newAngRot1 = avAngRot + (angRot1 - avAngRot) * frac;
float newAngRot2 = avAngRot + (angRot2 - avAngRot) * frac;
Vector3 newAngVel1;// = body0.AngVel + (newAngRot1 - angRot1) * hingeAxis;
Vector3.Multiply(ref hingeAxis, newAngRot1 - angRot1, out newAngVel1);
Vector3.Add(ref newAngVel1, ref body0.transformRate.AngularVelocity, out newAngVel1);
Vector3 newAngVel2;// = body1.AngVel + (newAngRot2 - angRot2) * hingeAxis;
Vector3.Multiply(ref hingeAxis, newAngRot2 - angRot2, out newAngVel2);
Vector3.Add(ref newAngVel2, ref body1.transformRate.AngularVelocity, out newAngVel2);
body0.AngularVelocity = newAngVel1;
body1.AngularVelocity = newAngVel2;
}
// the extra torque
if (extraTorque != 0.0f)
{
Vector3 torque1;// = extraTorque * Vector3.Transform(hingeAxis, body0.Orientation);
Vector3.Transform(ref hingeDirection, ref body0.transform.Orientation, out torque1);
Vector3.Multiply(ref torque1, extraTorque, out torque1);
body0.AddWorldTorque(torque1);
body1.AddWorldTorque(-torque1);
}
}
public bool HingeEnabled
{
get { return hingeEnabled; }
}
/// <summary>
/// Are we broken
/// </summary>
public bool IsBroken
{
get { return broken; }
}
}
}
