void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
if( needCreate == created )
return;
if( needCreate )
{
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateFixed( ( (ODEPhysicsScene)Scene ).worldID, IntPtr.Zero );
Ode.SetJointContactsEnabled( jointID, false );//ContactsEnabled );
Ode.dJointAttach( jointID, odeBody1.bodyID, odeBody2.bodyID );
Ode.dJointSetFixed( jointID );
Ode.BodyDataAddJoint( odeBody1.bodyData, jointID );
Ode.BodyDataAddJoint( odeBody2.bodyData, jointID );
}
else
{
DestroyODEJoint();
}
}
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateFixed(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, false); //ContactsEnabled );
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
Ode.dJointSetFixed(jointID);
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateSlider(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
Ode.dJointSetSliderParam(jointID, Ode.dJointParams.dParamFudgeFactor,
Defines.jointFudgeFactor);
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
axis.UpdateToLibrary(true);
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
void nearCallback(IntPtr data, dGeomID o1, dGeomID o2)
{
dBodyID b1 = ode.dGeomGetBody(o1);
dBodyID b2 = ode.dGeomGetBody(o2);
const int MAX_CONTACTS = 5;
ode.dContact[] contact = new ode.dContact[MAX_CONTACTS];
for (int i = 0; i < MAX_CONTACTS; i++)
{
contact[i].surface.mode = (int)(ode.dContactType.Bounce | ode.dContactType.SoftCFM);
contact[i].surface.mu = Mathf.Infinity;
contact[i].surface.mu2 = 0;
contact[i].surface.bounce = 0.01f;
contact[i].surface.bounce_vel = 0.1f;
contact[i].surface.soft_cfm = 0.01f;
}
int numc = ode.dCollide(o1, o2, MAX_CONTACTS, ref contact[0].geom, System.Runtime.InteropServices.Marshal.SizeOf(typeof(ode.dContact)));
if (numc > 0)
{
for (int i = 0; i < numc; i++)
{
dJointID c = ode.dJointCreateContact(world, contactgroup, ref contact[i]);
ode.dJointAttach(c, b1, b2);
}
}
}
public Joint(World world, dJointID ptr, OdeJoint.JointType type)
{
this.world = world;
this.type = type;
this.ptr = ptr;
id = InstanceMap.Add(this);
ode.dJointSetData(ptr, (IntPtr)id);
}
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
if (Math.Abs(Vec3.Dot(axis1.Direction, axis2.Direction)) > .095f)
{
Log.Warning("UniversalJoint: Invalid axes.");
return;
}
if (Axis1.LimitsEnabled && Axis1.LimitLow > Axis1.LimitHigh)
{
Log.Warning("UniversalJoint: Invalid axis1 limits (low > high).");
return;
}
if (Axis2.LimitsEnabled && Axis2.LimitLow > Axis2.LimitHigh)
{
Log.Warning("UniversalJoint: Invalid axis2 limits (low > high).");
return;
}
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateUniversal(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
Ode.dJointSetUniversalParam(jointID, Ode.dJointParams.dParamFudgeFactor,
Defines.jointFudgeFactor);
Ode.dJointSetUniversalParam(jointID, Ode.dJointParams.dParamFudgeFactor2,
Defines.jointFudgeFactor);
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
Ode.dJointSetUniversalAnchor(jointID, Anchor.X, Anchor.Y, Anchor.Z);
axis1.UpdateToLibrary(true);
axis2.UpdateToLibrary(true);
axis1LocalAxis = Body1.Rotation.GetInverse() * axis1.Direction;
axis2LocalAxis = Body1.Rotation.GetInverse() * axis2.Direction;
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
internal static void FreeJointFeedback( dJointID jointID )
{
IntPtr jointFeedback = Ode.dJointGetFeedbackAsIntPtr( jointID );
if( jointFeedback != IntPtr.Zero )
{
Ode.dFree( jointFeedback, (uint)Marshal.SizeOf( typeof( Ode.dJointFeedback ) ) );
jointFeedback = IntPtr.Zero;
}
}
internal static void FreeJointFeedback(dJointID jointID)
{
IntPtr jointFeedback = Ode.dJointGetFeedbackAsIntPtr(jointID);
if (jointFeedback != IntPtr.Zero)
{
Ode.dFree(jointFeedback, (uint)Marshal.SizeOf(typeof(Ode.dJointFeedback)));
jointFeedback = IntPtr.Zero;
}
}
internal Joint(dJointID joint, World world, JointGroup group)
{
id = joint;
handle = GCHandle.Alloc(this);
NativeMethods.dJointSetData(id, GCHandle.ToIntPtr(handle));
id.Owner = world;
if (group != null)
{
group.Add(this);
}
}
internal void DestroyODEJoint()
{
if (jointID != dJointID.Zero)
{
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
Ode.BodyDataRemoveJoint(odeBody1.bodyData, jointID);
Ode.BodyDataRemoveJoint(odeBody2.bodyData, jointID);
Utils.FreeJointFeedback(jointID);
Ode.dJointDestroy(jointID);
jointID = dJointID.Zero;
}
}
internal void DestroyODEJoint()
{
if( jointID != dJointID.Zero )
{
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
Ode.BodyDataRemoveJoint( odeBody1.bodyData, jointID );
Ode.BodyDataRemoveJoint( odeBody2.bodyData, jointID );
Utils.FreeJointFeedback( jointID );
Ode.dJointDestroy( jointID );
jointID = dJointID.Zero;
}
}
internal static bool UpdateJointBreakState( Joint joint, dJointID jointID )
{
if( jointID == dJointID.Zero )
return false;
float force;
float torque;
CalculateJointStress( jointID, out force, out torque );
if( joint.BreakMaxForce != 0 && force >= joint.BreakMaxForce )
return true;
if( joint.BreakMaxTorque != 0 && torque >= joint.BreakMaxTorque )
return true;
return false;
}
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
if (Axis.LimitsEnabled && Axis.LimitLow > Axis.LimitHigh)
{
Log.Warning("HingeJoint: Invalid axis limits (low > high).");
return;
}
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateHinge(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
Ode.dJointSetHingeParam(jointID, Ode.dJointParams.dParamFudgeFactor,
Defines.jointFudgeFactor);
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
Ode.dJointSetHingeAnchor(jointID, Anchor.X, Anchor.Y, Anchor.Z);
axis.UpdateToLibrary(true);
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
internal static bool UpdateJointBreakState(Joint joint, dJointID jointID)
{
if (jointID == dJointID.Zero)
{
return(false);
}
float force;
float torque;
CalculateJointStress(jointID, out force, out torque);
if (joint.BreakMaxForce != 0 && force >= joint.BreakMaxForce)
{
return(true);
}
if (joint.BreakMaxTorque != 0 && torque >= joint.BreakMaxTorque)
{
return(true);
}
return(false);
}
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();
}
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 extern static int dJointGetAMotorAxisRel( dJointID joint, int anum );
public extern static void dJointSetAMotorAxis( dJointID joint, int anum, int rel, dReal x, dReal y, dReal z );
public extern static void dJointSetAMotorNumAxes( dJointID joint, int num );
public extern static void dJointSetAMotorMode( dJointID joint, int mode );
public extern static dReal dJointGetUniversalAngle2Rate( dJointID joint );
public extern static dReal dJointGetUniversalParam( dJointID joint, dJointParams parameter );
public extern static void BodyDataRemoveJoint( IntPtr bodyData, dJointID jointID );
internal override dReal GetParam(dJointID id, dJointParam parameter)
{
return(NativeMethods.dJointGetPistonParam(id, parameter));
}
internal abstract dReal GetParam(dJointID id, dJointParam parameter);
internal abstract void SetParam(dJointID id, dJointParam parameter, dReal value);
public UniversalJoint(World world, dJointID ptr) : base(world, ptr, OdeJoint.JointType.Universal)
{
}
public AMotor(World world, dJointID ptr) : base(world, ptr, OdeJoint.JointType.AMotor)
{
}
public BallJoint(World world, dJointID ptr) : base(world, ptr, OdeJoint.JointType.Ball)
{
}
public extern static dReal dJointGetAMotorAngleRate( dJointID joint, int anum );
public extern static dReal dJointGetAMotorParam( dJointID joint, dJointParams parameter );
internal override void SetParam(dJointID id, dJointParam parameter, dReal value)
{
NativeMethods.dJointSetPistonParam(id, parameter, value);
}
public HingeJoint(World world, dJointID ptr) : base(world, ptr, OdeJoint.JointType.Hinge)
{
}
internal JointLimitMotor(Joint joint, int axis)
{
id = joint.id;
index = axis;
}
public extern static void dJointAddUniversalTorques( dJointID joint, dReal torque1, dReal torque2 );
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
if (needCreate && (odeBody1.bodyID == dBodyID.Zero || odeBody2.bodyID == dBodyID.Zero))
{
Log.Warning("ODEHinge2Joint: It is necessary that both bodies were not static.");
needCreate = false;
}
if (needCreate == created)
{
return;
}
if (needCreate)
{
if (Math.Abs(Vec3.Dot(axis1.Direction, axis2.Direction)) > .095f)
{
Log.Warning("Hinge2Joint: Invalid axes.");
return;
}
if (Axis1.LimitsEnabled && Axis1.LimitLow > Axis1.LimitHigh)
{
Log.Warning("Hinge2Joint: Invalid axis1 limits (low > high).");
return;
}
if (Axis2.LimitsEnabled && Axis2.LimitLow > Axis2.LimitHigh)
{
Log.Warning("Hinge2Joint: Invalid axis2 limits (low > high).");
return;
}
jointID = Ode.dJointCreateHinge2(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
Ode.dJointSetHinge2Param(jointID, Ode.dJointParams.dParamFudgeFactor,
Defines.jointFudgeFactor);
Ode.dJointSetHinge2Param(jointID, Ode.dJointParams.dParamFudgeFactor2,
Defines.jointFudgeFactor);
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
Ode.dJointSetHinge2Anchor(jointID, Anchor.X, Anchor.Y, Anchor.Z);
axis1.UpdateToLibrary(true);
axis2.UpdateToLibrary(true);
UpdateSuspension();
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
public extern static void dJointSetFixed( dJointID joint );
public extern static void dJointGetHinge2Axis2( dJointID joint, ref dVector3 result );
public extern static int dJointGetAMotorMode( dJointID joint );
public extern static dReal dJointGetHinge2Angle2Rate( dJointID joint );
public extern static int dJointGetAMotorNumAxes( dJointID joint );
public extern static dReal dJointGetHinge2Param( dJointID joint, dJointParams parameter );
public extern static void dJointGetAMotorAxis( dJointID joint, int anum, ref dVector3 result );
public extern static void dJointAddHinge2Torques( dJointID joint, dReal torque1, dReal torque2 );
public extern static void dJointSetAMotorAngle( dJointID joint, int anum, dReal angle );
public extern static void dJointSetUniversalAxis2( dJointID joint, dReal x, dReal y, dReal z );
public extern static void dJointSetAMotorParam( dJointID joint, dJointParams parameter, dReal value );
public extern static void dJointGetUniversalAxis2( dJointID joint, ref dVector3 result );
public extern static void dJointAddAMotorTorques( dJointID joint, dReal torque1, dReal torque2, dReal torque3 );
public extern static void dJointSetUniversalParam( dJointID joint, dJointParams parameter, dReal value );
public extern static void SetJointContactsEnabled( dJointID jointID, [MarshalAs( UnmanagedType.U1 )] bool contactsEnabled );
void OnNewContact(ref ode.dContact contact, dBodyID body1, dBodyID body2)
{
dJointID c = ode.dJointCreateContact(world.ptr, world.contactgroup, ref contact);
ode.dJointAttach(c, body1, body2);
}
