unsafe internal static void CalculateJointStress( dJointID jointID,
out float force, out float torque )
{
IntPtr jointFeedback = Ode.dJointGetFeedbackAsIntPtr( jointID );
if( jointFeedback == IntPtr.Zero )
{
//create jointFeedback and begin use on next simulation step
jointFeedback = Ode.dAlloc( (uint)Marshal.SizeOf( typeof( Ode.dJointFeedback ) ) );
//zero memory
unsafe
{
Ode.dJointFeedback* p = (Ode.dJointFeedback*)jointFeedback;
p->f1 = new Ode.dVector3();
p->t1 = new Ode.dVector3();
p->f2 = new Ode.dVector3();
p->t2 = new Ode.dVector3();
}
Ode.dJointSetFeedbackAsIntPtr( jointID, jointFeedback );
force = 0;
torque = 0;
return;
}
Ode.dJointFeedback* ptr = (Ode.dJointFeedback*)jointFeedback;
Vec3 f1 = Convert.ToNet( ptr->f1 );
Vec3 t1 = Convert.ToNet( ptr->t1 );
Vec3 f2 = Convert.ToNet( ptr->f2 );
Vec3 t2 = Convert.ToNet( ptr->t2 );
// This is a simplification, but it should still work.
force = ( f1 - f2 ).Length();
torque = ( t1 - t2 ).Length();
}
public override bool IsStability()
{
if( jointID == dJointID.Zero )
return true;
Ode.dVector3 a1 = new Ode.dVector3();
Ode.dVector3 a2 = new Ode.dVector3();
Ode.dJointGetHingeAnchor( jointID, ref a1 );
Ode.dJointGetHingeAnchor2( jointID, ref a2 );
const float limit = .01f;// .001f;
return Math.Abs( a1.X - a2.X ) < limit &&
Math.Abs( a1.Y - a2.Y ) < limit &&
Math.Abs( a1.Z - a2.Z ) < limit;
}
internal void UpdateDataFromLibrary()
{
if( jointID == dJointID.Zero )
return;
Ode.dVector3 odeAnchor = new Ode.dVector3();
Ode.dVector3 odeAxisDirection = new Ode.dVector3();
Ode.dJointGetHingeAnchor( jointID, ref odeAnchor );
Ode.dJointGetHingeAxis( jointID, ref odeAxisDirection );
Vec3 anchor = Convert.ToNet( odeAnchor );
Vec3 axisDirection = Convert.ToNet( odeAxisDirection );
UpdateDataFromLibrary( ref anchor, ref axisDirection );
}
internal void UpdateDataFromLibrary()
{
if( jointID == dJointID.Zero )
return;
Ode.dVector3 odeAnchor = new Ode.dVector3();
Ode.dJointGetUniversalAnchor( jointID, ref odeAnchor );
Vec3 anchor = Convert.ToNet( odeAnchor );
Vec3 axis1Direction = Body1.Rotation * axis1LocalAxis;
Vec3 axis2Direction = Body1.Rotation * axis2LocalAxis;
UpdateDataFromLibrary( ref anchor, ref axis1Direction, ref axis2Direction );
}
internal void DoDamping()
{
if( bodyID == dBodyID.Zero )
return;
Ode.dMass mass = new Ode.dMass();
Ode.dBodyGetMass( bodyID, ref mass );
// Linear damping
if( LinearDamping != 0 )
{
// The damping force depends on the damping amount, mass, and velocity
// (i.e. damping amount and momentum).
float factor = -LinearDamping * mass.mass;
Vec3 force = LinearVelocity * factor;
// Add a global force opposite to the global linear velocity.
Ode.dBodyAddForce( bodyID, force.X, force.Y, force.Z );
}
// Angular damping
if( AngularDamping != 0 )
{
Vec3 localVelocity;
{
Ode.dVector3 aVelLocal = new Ode.dVector3();
Ode.dBodyVectorFromWorld( bodyID, AngularVelocity.X, AngularVelocity.Y,
AngularVelocity.Z, ref aVelLocal );
localVelocity = Convert.ToNet( aVelLocal );
}
// The damping force depends on the damping amount, mass, and velocity
// (i.e. damping amount and momentum).
float factor = -AngularDamping;
Vec3 momentum = new Vec3(
Vec3.Dot( new Vec3( mass.I.M00, mass.I.M01, mass.I.M02 ), localVelocity ),
Vec3.Dot( new Vec3( mass.I.M10, mass.I.M11, mass.I.M12 ), localVelocity ),
Vec3.Dot( new Vec3( mass.I.M20, mass.I.M21, mass.I.M22 ), localVelocity ) );
Vec3 torque = momentum * factor;
// Add a local torque opposite to the local angular velocity.
Ode.dBodyAddRelTorque( bodyID, torque.X, torque.Y, torque.Z );
}
}
