protected override void OnSimulationStep()
{
foreach (Body body in Bodies)
{
if (!body.Static && !body.Sleeping)
{
ODEBody odeBody = (ODEBody)body;
// Apply linear and angular damping; if using the "add opposing
// forces" method, be sure to do this before calling ODE step
// function.
odeBody.DoDamping();
if (body.CCD)
{
odeBody.ccdLastPosition = odeBody.Position;
}
}
}
int collisionEventCount;
IntPtr collisionEvents;
Ode.DoSimulationStep(neoAxisAdditionsID, out collisionEventCount, out collisionEvents);
//process collision events
{
unsafe
{
Ode.CollisionEventData *pointer = (Ode.CollisionEventData *)collisionEvents;
for (int n = 0; n < collisionEventCount; n++)
{
ODEBody.GeomData geomData1 = shapesDictionary[pointer->shapeDictionaryIndex1];
ODEBody.GeomData geomData2 = shapesDictionary[pointer->shapeDictionaryIndex2];
//// Invalidate the "freely-spinning" parameters.
//geomData1.odeBody.freelySpinning = false;
//geomData2.odeBody.freelySpinning = false;
if (EnableCollisionEvents)
{
Shape shape1 = geomData1.shape;
Shape shape2 = geomData2.shape;
if (IsBodyCollisionEventHandled(shape1.Body) ||
IsBodyCollisionEventHandled(shape2.Body))
{
Vec3 pos;
Vec3 normal;
Convert.ToNet(ref pointer->position, out pos);
Convert.ToNet(ref pointer->normal, out normal);
normal = -normal;
AddCollisionEvent(shape1, shape2, ref pos, ref normal, pointer->depth);
}
}
pointer++;
}
}
}
// Take a simulation step.
Ode.dWorldQuickStep(worldID, StepSize);
// Remove all joints from the contact group.
Ode.dJointGroupEmpty(contactJointGroupID);
//update from ODE
foreach (Body body in Bodies)
{
if (!body.Static)
{
ODEBody odeBody = (ODEBody)body;
odeBody.UpdateDataFromLibrary();
if (!odeBody.Sleeping)
{
//ODE bug fix
//need still?
if (float.IsNaN(odeBody.Position.X))
{
odeBody.Position = odeBody.OldPosition;
}
if (float.IsNaN(odeBody.Rotation.X))
{
odeBody.Rotation = odeBody.OldRotation;
}
//// Fix angular velocities for freely-spinning bodies that have
//// gained angular velocity through explicit integrator inaccuracy.
//odeBody.DoAngularVelocityFix();
if (odeBody.CCD)
{
odeBody.CCDStep();
}
}
}
}
foreach (Joint joint in Joints)
{
switch (joint.JointType)
{
case Joint.Type.Hinge:
((ODEHingeJoint)joint).UpdateDataFromLibrary();
break;
case Joint.Type.Universal:
((ODEUniversalJoint)joint).UpdateDataFromLibrary();
break;
case Joint.Type.Hinge2:
((ODEHinge2Joint)joint).UpdateDataFromLibrary();
break;
case Joint.Type.Ball:
((ODEBallJoint)joint).UpdateDataFromLibrary();
break;
case Joint.Type.Slider:
((ODESliderJoint)joint).UpdateDataFromLibrary();
break;
case Joint.Type.Fixed:
break;
default:
Trace.Assert(false);
break;
}
}
}