Body[] DoVolumeCastGeneral(dGeomID volumeCastGeomID, int contactGroup)
{
int count;
IntPtr data;
Ode.DoVolumeCast(neoAxisAdditionsID, volumeCastGeomID, contactGroup, out count,
out data);
if (count == 0)
{
return(emptyVolumeCastResult);
}
unsafe
{
int *pointer = (int *)data;
for (int n = 0; n < count; n++)
{
int shapeDictionaryIndex = *pointer;
ODEBody.GeomData geomData = shapesDictionary[shapeDictionaryIndex];
volumeCastResult.AddWithCheckAlreadyContained(geomData.shape.Body);
pointer++;
}
}
Body[] result = new Body[volumeCastResult.Count];
volumeCastResult.CopyTo(result, 0);
volumeCastResult.Clear();
return(result);
}
public override void SetShapePairFlags(Shape shape1, Shape shape2, ShapePairFlags flags)
{
base.SetShapePairFlags(shape1, shape2, flags);
ODEBody body1 = (ODEBody)shape1.Body;
ODEBody body2 = (ODEBody)shape2.Body;
ODEBody.GeomData geomData1 = body1.GetGeomDataByShape(shape1);
ODEBody.GeomData geomData2 = body2.GetGeomDataByShape(shape2);
if (geomData1 != null && geomData2 != null)
{
dGeomID geomID1 = (geomData1.transformID != dGeomID.Zero) ?
geomData1.transformID : geomData1.geomID;
dGeomID geomID2 = (geomData2.transformID != dGeomID.Zero) ?
geomData2.transformID : geomData2.geomID;
bool value = (flags & ShapePairFlags.DisableContacts) != 0;
Ode.SetShapePairDisableContacts(geomID1, geomID2, value);
}
}
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;
}
}
}