/// Returns the current amount of stress on this Joint.
protected virtual float CalcStress()
{
float currentStress = 0;
switch (data.BreakMode)
{
case JointBreakMode.Unbreakable:
// Nothing to do.
break;
case JointBreakMode.Threshold:
// Fall through...
case JointBreakMode.Accumulated:
{
Tao.Ode.Ode.dJointFeedback jf = Tao.Ode.Ode.dJointGetFeedback(jointID);
Vector3 f1 = jf.f1;
Vector3 t1 = jf.t1;
Vector3 f2 = jf.f2;
Vector3 t2 = jf.t2;
f1 -= f2;
t1 -= t2;
// This is a simplification, but it should still work.
currentStress = f1.Length() + t1.Length();
break;
}
default:
throw new PhysicsException("Unknown bug");
//break;
}
return(currentStress);
}
/// Note: For a perfect save/restore in ODE, the "warm starting"
/// data should be stored in JointData. However, there is
/// currently no easy way to get this data.
public override void Init(JointData data)
{
if (initCalled)
{
// If this Joint has already been initialized, destroy the ODE
// Joint ID.
jointID.Destroy();
if (aMotorID != null)
{
aMotorID.Destroy();
}
// Reset event handler.
jointBreakEventHandler = null;
}
base.Init(data);
initCalled = true;
switch (data.Type)
{
case JointType.Hinge:
// Create an ODE hinge Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateHinge(worldID, IntPtr.Zero));
numAxes = 1;
// Set the rotational property of each axis.
axisRotational[0] = true;
axisRotational[1] = false;
axisRotational[2] = false;
// Set the ODE fudge factor for each axis.
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor,
Defaults.Ode.JointFudgeFactor);
break;
case JointType.Universal:
// Create an ODE universal Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateUniversal(worldID, IntPtr.Zero));
numAxes = 2;
// Set the rotational property of each axis.
axisRotational[0] = true;
axisRotational[1] = true;
axisRotational[2] = false;
// Set the ODE fudge factor for each axis.
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor,
Defaults.Ode.JointFudgeFactor);
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor2,
Defaults.Ode.JointFudgeFactor);
break;
case JointType.Ball:
// Create an ODE ball Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateBall(worldID, IntPtr.Zero));
numAxes = 3;
// Set the rotational property of each axis.
axisRotational[0] = true;
axisRotational[1] = true;
axisRotational[2] = true;
// ODE ball Joints need this special "angular motor" to
// restrain their movement e.g. when limits are used.
aMotorID = new dJointID(Tao.Ode.Ode.dJointCreateAMotor(worldID, IntPtr.Zero));
Tao.Ode.Ode.dJointSetAMotorMode(aMotorID, (int)Tao.Ode.Ode.dAMotorMode.dAMotorEuler);
// Set the ODE fudge factor for each axis.
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor,
Defaults.Ode.JointFudgeFactor);
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor2,
Defaults.Ode.JointFudgeFactor);
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor3,
Defaults.Ode.JointFudgeFactor);
break;
case JointType.Slider:
// Create an ODE slider Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateSlider(worldID, IntPtr.Zero));
numAxes = 1;
// Set the rotational property of each axis.
axisRotational[0] = false;
axisRotational[1] = false;
axisRotational[2] = false;
// Set the ODE fudge factor for each axis.
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor,
Defaults.Ode.JointFudgeFactor);
break;
case JointType.Wheel:
// Create an ODE hinge2 Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateHinge2(worldID, IntPtr.Zero));
numAxes = 2;
// Set the rotational property of each axis.
axisRotational[0] = true;
axisRotational[1] = true;
axisRotational[2] = false;
// Set the ODE fudge factor for each axis.
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor,
Defaults.Ode.JointFudgeFactor);
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor2,
Defaults.Ode.JointFudgeFactor);
break;
case JointType.Fixed:
// Create an ODE fixed Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateFixed(worldID, IntPtr.Zero));
numAxes = 0;
// Set the rotational property of each axis.
axisRotational[0] = false;
axisRotational[1] = false;
axisRotational[2] = false;
break;
default:
throw new PhysicsException("Unknown bug");
//break;
}
// Tell ODE about the JointFeedback struct for this Joint.
jointFeedback = new Tao.Ode.Ode.dJointFeedback();
Tao.Ode.Ode.dJointSetFeedback(jointID, ref jointFeedback);
// Setup the Solids.
FilterSolidForStaticness(data.Solid0, data.Solid1);
if (!data.IsBroken)
{
// Attach the Joint to the ODE bodies.
AttachODEBodies(data.Solid0, data.Solid1);
}
// Setup the Joint's anchor.
SetAnchor(data.Anchor);
// Setup the Joint's axes.
SetAxis(0, data.Axis[0]);
SetAxis(1, data.Axis[1]);
SetAxis(2, data.Axis[2]);
// Set the ODE joint's userdata pointer.
if (JointType.Ball == data.Type)
{
Tao.Ode.Ode.dJointSetData(aMotorID, GCHandle.ToIntPtr(GCHandle.Alloc(this)));
}
else
{
Tao.Ode.Ode.dJointSetData(jointID, GCHandle.ToIntPtr(GCHandle.Alloc(this)));
}
}
