How to join?

Developer
Nov 10, 2007 at 9:49 PM
Hi. is there any chance to join your project?

/// <summary>
/// Constraints a point on one body to be fixed to a point on another body
/// </summary>
public class ConstraintMaxDistance : Constraint
{
// maximum relative velocity induced at the constraint points - proportional
// to the error
const float maxVelMag = 20.0f;
const float minVelForProcessing = 0.01f;

// configuration
Body mBody0;
Body mBody1;
Vector3 mBody0Pos;
Vector3 mBody1Pos;
float mMaxDistance;
// stuff that gets updated
Vector3 mR0;
Vector3 mR1;
Vector3 mWorldPos;
Vector3 mCurrentRelPos0;

public ConstraintMaxDistance(Body body0, Vector3 body0Pos, Body body1, Vector3 body1Pos, float maxDistance)
{
Initialise(body0, body0Pos, body1, body1Pos, maxDistance);

}

public void Initialise(Body body0, Vector3 body0Pos, Body body1, Vector3 body1Pos, float maxDistance)
{
mBody0Pos = body0Pos;
mBody1Pos = body1Pos;
mBody0 = body0;
mBody1 = body1;
mMaxDistance = maxDistance;

if (mBody0 != null) mBody0.AddConstraint(this);
if (mBody1 != null) mBody1.AddConstraint(this);
}

public override void PreApply(float dt)
{
//SetUnsatisfied();
this.Satisfied = false;

mR0 = Vector3.Transform(mBody0Pos, mBody0.Orientation);
mR1 = Vector3.Transform(mBody1Pos, mBody1.Orientation);

//MultMatrix33(mR0, mBody0.Orientation, mBody0Pos);
//MultMatrix33(mR1, mBody1.Orientation, mBody1Pos);

Vector3 worldPos0 = mBody0.Position + mR0;
Vector3 worldPos1 = mBody1.Position + mR1;

mWorldPos = 0.5f * (worldPos0 + worldPos1);

// current location of point 0 relative to point 1
//SubVector3(mCurrentRelPos0, worldPos0, worldPos1);
mCurrentRelPos0 = worldPos0 - worldPos1;

}

public override bool Apply(float dt)
{

this.Satisfied = true;

bool body0FrozenPre = !mBody0.IsActive;
bool body1FrozenPre = !mBody1.IsActive;

if (body0FrozenPre && body1FrozenPre)
return false;

Vector3 currentVel0 = mBody0.Velocity + Vector3.Cross(mBody0.AngVel, mR0);
Vector3 currentVel1 = mBody1.Velocity + Vector3.Cross(mBody1.AngVel, mR1);

// predict a new location
Vector3 predRelPos0 = (mCurrentRelPos0 + (currentVel0 - currentVel1) * dt);

// if the new position is out of range then clamp it
Vector3 clampedRelPos0 = predRelPos0;

float clampedRelPos0Mag = clampedRelPos0.Length();

if (clampedRelPos0Mag <= JiggleMath.Epsilon)
return false;

if (clampedRelPos0Mag > mMaxDistance)
clampedRelPos0 *= mMaxDistance / clampedRelPos0Mag;

// now claculate desired vel based on the current pos, new/clamped
// pos and dt
Vector3 desiredRelVel0 = ((clampedRelPos0 - mCurrentRelPos0) / System.Math.Max(dt, JiggleMath.Epsilon));

// Vr is -ve the total velocity change
Vector3 Vr = (currentVel0 - currentVel1) - desiredRelVel0;

float normalVel = Vr.Length();

// limit it
if (normalVel > maxVelMag)
{
Vr *= (maxVelMag / normalVel);
normalVel = maxVelMag;
}
else if (normalVel < minVelForProcessing)
{
return false;
}

Vector3 N = Vr / normalVel;

float denominator = mBody0.InvMass + mBody1.InvMass +
Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(mR0, N), mBody0.WorldInvInertia), mR0)) +
Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(mR1, N), mBody1.WorldInvInertia), mR1));

if (denominator < JiggleMath.Epsilon)
return false;

float normalImpulse = -normalVel / denominator;

if (!mBody0.Immovable)
mBody0.ApplyWorldImpulse(normalImpulse * N, mWorldPos);

if (!mBody1.Immovable)
mBody1.ApplyWorldImpulse(-normalImpulse * N, mWorldPos);

mBody0.SetConstraintsAndCollisionsUnsatisfied();
mBody1.SetConstraintsAndCollisionsUnsatisfied();

this.Satisfied = true;

return true;
}

public override void Destroy()
{
mBody0 = null;
mBody1 = null;

DisableConstraint();
}
}
Developer
Nov 10, 2007 at 10:24 PM
/// <summary>
/// Constraints a point on one body to be fixed to a point on another body
/// </summary>
public class ConstraintPoint : Constraint
{
const float mMaxVelMag = 20.0f;
const float minVelForProcessing = 0.01f;

Vector3 mBody0Pos;
Body mBody0;
Vector3 mBody1Pos;
Body mBody1;
float mAllowedDistance;
float mTimescale;

// some values that we calculate once in pre_apply
Vector3 mWorldPos; ///< average of the two joint positions
Vector3 mR0; ///< position relative to body 0 (in world space)
Vector3 mR1;
Vector3 mVrExtra; ///< extra vel for restoring deviation

public ConstraintPoint(Body body0, Vector3 body0Pos, Body body1, Vector3 body1Pos, float allowedDistance, float timescale)
{
Initialise(body0, body0Pos, body1, body1Pos, allowedDistance, timescale);

}

public void Initialise(Body body0, Vector3 body0Pos, Body body1, Vector3 body1Pos, float allowedDistance, float timescale)
{
mBody0Pos = body0Pos;
mBody1Pos = body1Pos;
mBody0 = body0;
mBody1 = body1;

mAllowedDistance = allowedDistance;
mTimescale = timescale;

if (mTimescale < JiggleMath.Epsilon)
mTimescale = JiggleMath.Epsilon;

if (mBody0 != null) mBody0.AddConstraint(this);
if (mBody1 != null) mBody1.AddConstraint(this);
}

public override void PreApply(float dt)
{
this.Satisfied = false;

mR0 = Vector3.Transform(mBody0Pos, mBody0.Orientation);
mR1 = Vector3.Transform(mBody1Pos, mBody1.Orientation);

//MultMatrix33(mR0, mBody0->GetOrientation(), mBody0Pos);
//MultMatrix33(mR1, mBody1->GetOrientation(), mBody1Pos);

Vector3 worldPos0 = mBody0.Position + mR0;
Vector3 worldPos1 = mBody1.Position + mR1;

mWorldPos = 0.5f * (worldPos0 + worldPos1);

// add a "correction" based on the deviation of point 0
Vector3 deviation = worldPos0 - worldPos1;

float deviationAmount = deviation.Length();

if (deviationAmount > mAllowedDistance)
{
mVrExtra = ((deviationAmount - mAllowedDistance) /
(deviationAmount * System.Math.Max(mTimescale, dt))) * deviation;
}
else
{
mVrExtra = Vector3.Zero;
}
}

public override bool Apply(float dt)
{
this.Satisfied = true;

bool body0FrozenPre = !mBody0.IsActive;
bool body1FrozenPre = !mBody1.IsActive;

// if (body0FrozenPre && body1FrozenPre)
// return false;


Vector3 currentVel0 = (mBody0.Velocity + Vector3.Cross(mBody0.AngVel, mR0));
Vector3 currentVel1 = (mBody1.Velocity + Vector3.Cross(mBody1.AngVel, mR1));

// add a "correction" based on the deviation of point 0
Vector3 Vr = (mVrExtra + currentVel0 - currentVel1);

float normalVel = Vr.Length();

if (normalVel < minVelForProcessing)
return false;

// limit things
if (normalVel > mMaxVelMag)
{
Vr *= mMaxVelMag / normalVel;
normalVel = mMaxVelMag;
}

Vector3 N = Vr / normalVel;

float numerator = -normalVel;

float denominator = mBody0.InvMass + mBody1.InvMass +
Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(mR0, N), mBody0.WorldInvInertia), mR0)) +
Vector3.Dot(N, Vector3.Cross(Vector3.Transform(Vector3.Cross(mR1, N), mBody1.WorldInvInertia), mR1));

if (denominator < JiggleMath.Epsilon)
return false;

Vector3 normalImpulse = (numerator / denominator) * N;

if (!mBody0.Immovable)
mBody0.ApplyWorldImpulse(normalImpulse, mWorldPos);

if (!mBody1.Immovable)
mBody1.ApplyWorldImpulse(-normalImpulse, mWorldPos);

mBody0.SetConstraintsAndCollisionsUnsatisfied();
mBody1.SetConstraintsAndCollisionsUnsatisfied();

this.Satisfied = true;

return true;
}

public override void Destroy()
{
mBody0 = null;
mBody1 = null;

DisableConstraint();
}
}
Developer
Nov 10, 2007 at 11:22 PM
HingeJoint class - should be tested...

/// <summary>
/// virtual base class for all joints
/// All joints are expected to do the following in their constructor:
/// 1. create whatever constraints are necessary
/// 2. register these constraints with the physics engine
/// </summary>
public abstract class Joint : Controller
{
}

/// <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 HingeJoint : Joint
{
Vector3 mHingeAxis;
Vector3 mHingePosRel0;
Body mBody0;
Body mBody1;
bool mUsingLimit;
bool mHingeEnabled;
bool mBroken;
float mDamping;
float mExtraTorque; // allow extra torque applied per update

ConstraintPoint mMidPointConstraint;
ConstraintMaxDistance[] mSidePointConstraints; //2;
ConstraintMaxDistance mMaxDistanceConstraint;

/// <summary>
/// default constructor so you can initialise this joint later
/// </summary>
public HingeJoint()
{
}

public void Initialise(Body body0, Body body1, Vector3 hingeAxis, Vector3 hingePosRel0,
float hingeHalfWidth, float hingeFwdAngle, float hingeBckAngle, float sidewaysSlack, float damping)
{
mBody0 = body0;
mBody1 = body1;
mHingeAxis = hingeAxis;
mHingePosRel0 = hingePosRel0;
mUsingLimit = false;
mDamping = damping;

// tScalar allowedDistance = 0.005f;
mHingeAxis.Normalize();

Vector3 hingePosRel1 = body0.Position + hingePosRel0 - body1.Position;

// generate the two positions relative to each body
Vector3 relPos0a = hingePosRel0 + hingeHalfWidth * hingeAxis;
Vector3 relPos0b = hingePosRel0 - hingeHalfWidth * hingeAxis;

Vector3 relPos1a = hingePosRel1 + hingeHalfWidth * hingeAxis;
Vector3 relPos1b = hingePosRel1 - hingeHalfWidth * hingeAxis;

float timescale = 1.0f / 20.0f;
float allowedDistanceMid = 0.005f;
float allowedDistanceSide = sidewaysSlack * hingeHalfWidth;

mSidePointConstraints = new ConstraintMaxDistance2;

mSidePointConstraints0 = new ConstraintMaxDistance();
mSidePointConstraints1 = new ConstraintMaxDistance();

mSidePointConstraints0.Initialise(body0, relPos0a, body1, relPos1a, allowedDistanceSide);
mSidePointConstraints1.Initialise(body0, relPos0b, body1, relPos1b, allowedDistanceSide);

mMidPointConstraint = new ConstraintPoint();
mMidPointConstraint.Initialise(body0, hingePosRel0, body1, hingePosRel1, allowedDistanceMid, timescale);

if (hingeFwdAngle <= 150) // MAXHINGEANGLE_LIMIT
{
// choose a direction that is perpendicular to the hinge
Vector3 perpDir = new Vector3(0.0f, 0.0f, 1.0f);

if (Vector3.Dot(perpDir, hingeAxis) > 0.1f)
perpDir = Vector3.Up;

// now make it perpendicular to the hinge
Vector3 sideAxis = Vector3.Cross(hingeAxis, perpDir);
perpDir = Vector3.Cross(sideAxis, hingeAxis);
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(hingeAxis, -angleToMiddle));

// work out the "string" length
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * JiggleMath.SinDeg(hingeHalfAngle * 0.5f);

Vector3 hingePos = body1.Position + hingePosRel0;
Vector3 relPos0c = hingePos + hingeRelAnchorPos0 - body0.Position;
Vector3 relPos1c = hingePos + hingeRelAnchorPos1 - body1.Position;

mMaxDistanceConstraint = new ConstraintMaxDistance();

mMaxDistanceConstraint.Initialise(body0, relPos0c, body1, relPos1c, allowedDistance);

mUsingLimit = true;
}
if (mDamping <= 0.0f)
mDamping = -1.0f; // just make sure that a value of 0.0 doesn't mess up...
else
JiggleMath.Limit(ref mDamping, 0, 1);
}

/// <summary>
/// Register the constraints
/// </summary>
public void EnableHinge()
{
if (mHingeEnabled)
return;

if (mBody0 != null)
{
mMidPointConstraint.EnableConstraint();
mSidePointConstraints0.EnableConstraint();
mSidePointConstraints1.EnableConstraint();

if (mUsingLimit && !mBroken)
mMaxDistanceConstraint.EnableConstraint();

EnableController();
}
mHingeEnabled = true;
}

/// <summary>
/// deregister the constraints
/// </summary>
public void DisableHinge()
{
if (!mHingeEnabled)
return;

if (mBody0 != null)
{
mMidPointConstraint.DisableConstraint();
mSidePointConstraints0.DisableConstraint();
mSidePointConstraints1.DisableConstraint();

if (mUsingLimit && !mBroken)
mMaxDistanceConstraint.DisableConstraint();

DisableController();
}
mHingeEnabled = false;
}

/// <summary>
/// Just remove the limit constraint
/// </summary>
public void Break()
{
if (mBroken)
return;

if (mUsingLimit)
mMaxDistanceConstraint.DisableConstraint();

mBroken = true;

}

/// <summary>
/// Just enable the limit constraint
/// </summary>
public void Mend()
{
if (!mBroken)
return;

if (mUsingLimit)
mMaxDistanceConstraint.EnableConstraint();

mBroken = false;

}


public override void UpdateController(float dt)
{
if (mBody0 == null || mBody1 == null)
return;

//Assert(0 != mBody0);
//Assert(0 != mBody1);

if (mDamping > 0.0f)
{
// Some hinges can bend in wonky ways. Derive the effective hinge axis
// using the relative rotation of the bodies.
Vector3 hingeAxis = mBody1.AngVel - mBody0.AngVel;
JiggleMath.Vector3NormaliseSafe(ref hingeAxis);

float angRot1 = Vector3.Dot(mBody0.AngVel, hingeAxis);
float angRot2 = Vector3.Dot(mBody1.AngVel, hingeAxis);

float avAngRot = 0.5f * (angRot1 + angRot2);

float frac = 1.0f - mDamping;
float newAngRot1 = avAngRot + (angRot1 - avAngRot) * frac;
float newAngRot2 = avAngRot + (angRot2 - avAngRot) * frac;

Vector3 newAngVel1 = mBody0.AngVel + (newAngRot1 - angRot1) * hingeAxis;
Vector3 newAngVel2 = mBody1.AngVel + (newAngRot2 - angRot2) * hingeAxis;

mBody0.AngVel = newAngVel1;
mBody1.AngVel = newAngVel2;
}

// the extra torque
if (mExtraTorque != 0.0f)
{
Vector3 torque1 = mExtraTorque * Vector3.Transform(mHingeAxis, mBody0.Orientation);
mBody0.AddWorldTorque(torque1);
mBody1.AddWorldTorque(-torque1);
}
}

public bool HingeEnabled
{
get { return mHingeEnabled; }
}

/// <summary>
/// Are we broken
/// </summary>
public bool IsBroken
{
get { return mBroken; }
}

}


public static class JiggleMath
{

public const float Epsilon = 1.0e-6f;
public const float PIDIV180 = 0.0174532925199432957692369076848861F;
public const float 180DIV_PI = 57.2957795130823208767981548141052F;


// NormalizeSafe

public static float[] VectorToArray(ref Vector3 vec)
{
return new float3 { vec.X, vec.Y, vec.Z };
}

public static float Deg2Rad(float deg)
{
return (deg * PIDIV180);
}

public static float Rad2Deg(float rad)
{
return (rad * 180DIV_PI);
}


public static float SinDeg(float deg)
{
return (float)System.Math.Sin(Deg2Rad(deg));
}

public static void Limit(ref float val, float min, float max)
{
if (val < min)
val = min;
else
if (val > max)
val = max;
}

public static void Vector3NormaliseSafe(ref Vector3 vec, Vector3 safe)
{
float lSq = vec.LengthSquared();

if (lSq > Epsilon)
{
vec *= (1.0f / (float)System.Math.Sqrt(lSq));
}
else
{
vec = safe;
}
Coordinator
Nov 12, 2007 at 12:41 PM
hey, thanks for the code... i add you to the developer list. sorry i saw your post so late :(

if you have any questions write me an email
jigglex@googlemail.com