/// <summary>
/// Add a geom to a space.
///
/// This does nothing if the geom is already in the space.
///
/// This function can be called automatically if a space argument is given to
/// a geom creation function.
/// </summary>
public void Add(dGeomID geomId)
{
if (IsNull())
{
return;
}
Tao.Ode.Ode.dSpaceAdd(this, geomId);
}
/// <summary>
/// Remove a geom from a space.
///
/// This does nothing if the geom is not actually in the space.
///
/// This function is called automatically by dGeomDestroy if the geom is in a space.
/// </summary>
public void Remove(dGeomID geomId)
{
if (IsNull())
{
return;
}
Tao.Ode.Ode.dSpaceRemove(this, geomId);
}
/// Special collision callback functor for volume collision
/// checking.
static unsafe internal void VolumeCollisionCallback(IntPtr data, IntPtr o0, IntPtr o1)
{
if (Tao.Ode.Ode.dGeomIsSpace(o0) != 0 || Tao.Ode.Ode.dGeomIsSpace(o1) != 0)
{
// Colliding a space with either a geom or another space.
Tao.Ode.Ode.dSpaceCollide2(o0, o1, data, VolumeCollisionCallback);
}
else
{
// Get a pointer to the ODESimulator.
OdeSimulator sim = GCHandle.FromIntPtr(data).Target as OdeSimulator;
// Get pointers to the two geoms' GeomData structure. Both
// of these should always be non-NULL.
/*GeomData geomData0 = (GeomData)Tao.Ode.Ode.dGeomGetData(o0);
* GeomData geomData1 = (GeomData)Tao.Ode.Ode.dGeomGetData(o1);*/
// CANYONSHOOTER BEGIN
dGeomID g0 = o0;
dGeomID g1 = o1;
GeomData geomData0 = GCHandle.FromIntPtr(Tao.Ode.Ode.dGeomGetData(g0)).Target as GeomData;
GeomData geomData1 = GCHandle.FromIntPtr(Tao.Ode.Ode.dGeomGetData(g1)).Target as GeomData;
/*
* GeomData geomData0 = GCHandle.FromIntPtr(o0).Target as GeomData;
* GeomData geomData1 = GCHandle.FromIntPtr(o1).Target as GeomData;
*/
// CANYONSHOOTER END
// Get pointers to the geoms' ShapeData structures.
ShapeData shape0 = geomData0.Shape;
ShapeData shape1 = geomData1.Shape;
// Check if the two Solids' contact groups generate contacts
// when they collide.
bool makeContacts = sim.GroupsMakeContacts(shape0.ContactGroup, shape1.ContactGroup);
if (!makeContacts)
{
return;
}
// Now actually test for collision between the two geoms.
// This is a fairly expensive operation.
//ODE.Joints.ContactGeom[] contactArray = new ODE.Joints.ContactGeom[1];
Tao.Ode.Ode.dContactGeom[] contactArray = new Tao.Ode.Ode.dContactGeom[1];
int numContacts = Tao.Ode.Ode.dCollide(o0, o1, 1, contactArray, sizeof(Tao.Ode.Ode.dContactGeom));
if (0 == numContacts)
{
return;
}
else
{
// These two geoms must be intersecting.
// Get pointers to the geoms' Solids.
Solid solid0 = geomData0.Solid;
Solid solid1 = geomData1.Solid;
// Not sure at this point if we can know that o1 is the
// volume object, so we'll just call this twice. It
// will automatically keep from adding the same Solid
// multiple times by using its collision count. Later,
// the volume Solid will be removed from this list.
sim.InternalAddCollidedSolid(solid0);
sim.InternalAddCollidedSolid(solid1);
}
}
}
/// Main collision callback functor.
static unsafe internal void CollisionCallback(IntPtr data, dGeomID o0, dGeomID o1)
{
if (o0.IsSpace || o1.IsSpace)
{
// Colliding a space with either a geom or another space.
Tao.Ode.Ode.dSpaceCollide2(o0, o1, data, CollisionCallbackRunner);
if (o0.IsSpace)
{
// Colliding all geoms internal to the space.
Tao.Ode.Ode.dSpaceCollide(o0, data, CollisionCallbackRunner);
}
if (o1.IsSpace)
{
// Colliding all geoms internal to the space.
Tao.Ode.Ode.dSpaceCollide(o1, data, CollisionCallbackRunner);
}
}
else
{
// Colliding two geoms.
// The following is a set of special cases where we might
// want to skip collision detection (but not all are
// enforced here for various reasons):
// 1. Two static Solids (neither geom has a body) AND
// neither Solid has a CollisionEventHandler AND there are
// not global handlers: this is enforced.
// 2. Two Shapes that are part of the same Solid (they
// share a body): this is not enforced because ODE
// already takes care of it.
// 3. Two sleeping Solids (note: both must have bodies to
// check this): this is enforced. (ODE should handle
// this, but it doesn't.)
// 4. Two Solids connected by a fixed Joint: this is
// enforced.
// 5. Two Solids connected by a Joint (besides ODE
// contact joints, which should never generate more
// contacts) with contacts disabled (note: both must have
// bodies to check this): this is enforced.
// 6. Solid0 is static, Solid1 is dynamic and is sleeping,
// static-to-sleeping contacts are ignored by the
// Simulator, and neither Solid has a
// CollisionEventHandler AND there are no global handlers:
// this is enforced.
// 7. Solid1 is static, Solid0 is dynamic and is sleeping,
// static-to-sleeping contacts are ignored by the
// Simulator, and neither Solid has a
// CollisionEventHandler AND there are no global handlers:
// this is enforced.
// 8. The two Solids' contact groups do not generate
// contacts when they collide AND neither Solid has a
// CollisionEventHandler AND there are no global handlers.
// Get the geoms' ODE body IDs.
dBodyID o0BodyID = o0.RigidBody;
dBodyID o1BodyID = o1.RigidBody;
bool solid0Static = PtrWrapper.IsEmpty(o0BodyID); // (mike)
bool solid1Static = PtrWrapper.IsEmpty(o1BodyID); // {mike}
// Check if both Solids are dynamic (i.e. have ODE bodies).
bool bothHaveBodies = true;
if (solid0Static || solid1Static)
{
bothHaveBodies = false;
}
// If the two Solids are connected by a common Joint, get
// a pointer to that Joint.
Joint commonJoint = null;
if (bothHaveBodies)
{
int connExcluding = Tao.Ode.Ode.dAreConnectedExcluding(o0BodyID, o1BodyID, (int)Tao.Ode.Ode.dJointTypes.dJointTypeContact);
if (connExcluding != 0)
{
// This will become non-NULL only if there exists an ODE
// joint connecting the two bodies.
commonJoint = GetCommonJoint(o0BodyID, o1BodyID);
}
}
// Get pointers to the geoms' GeomData structures.
GeomData geomData0 = GCHandle.FromIntPtr(Tao.Ode.Ode.dGeomGetData(o0)).Target as GeomData;
GeomData geomData1 = GCHandle.FromIntPtr(Tao.Ode.Ode.dGeomGetData(o1)).Target as GeomData;
// Get pointers to the geoms' ShapeData structures.
ShapeData shape0 = geomData0.Shape;
ShapeData shape1 = geomData1.Shape;
// Get a pointer to the ODESimulator.
OdeSimulator sim = GCHandle.FromIntPtr(data).Target as OdeSimulator;
// Check if the two Solids' contact groups generate contacts
// when they collide.
bool makeContacts = sim.GroupsMakeContacts(shape0.ContactGroup, shape1.ContactGroup);
// Find out whether the Simulator has static-to-sleeping
// contacts disabled.
bool ignoreStaticSleepingContacts = !sim.IsStaticSleepingContactsEnabled;
// Get pointers to the geoms' Solids.
Solid solid0 = geomData0.Solid;
Solid solid1 = geomData1.Solid;
// Get pointers to the two Solids' CollisionEventHandlers.
// These will be NULL if the Solids don't use
// CollisionEventHandlers.
CollisionEventProcessor handler0 = solid0.CollisionEventHandler;
CollisionEventProcessor handler1 = solid1.CollisionEventHandler;
bool neitherHasEventHandler = !(handler0 != null || handler1 != null);
bool hasNoGlobalHandler = sim.NumGlobalCollisionEventHandlers == 0;
// Now do the actual tests to see if we should return early.
// It is important here that we don't call dBodyIsEnabled on
// a static body because that crashes ODE.
bool case1 = neitherHasEventHandler && hasNoGlobalHandler &&
solid0Static && solid1Static;
//bool case2= o0BodyID == o1BodyID;
bool case3 = bothHaveBodies && Tao.Ode.Ode.dBodyIsEnabled(o0BodyID) == 0 &&
Tao.Ode.Ode.dBodyIsEnabled(o1BodyID) == 0;
bool case4 = commonJoint != null &&
commonJoint.Type == JointType.Fixed;
bool case5 = commonJoint != null &&
!commonJoint.ContactsEnabled;
bool case6 = solid0Static && null != o1BodyID && o1BodyID.IsNotNull() &&
Tao.Ode.Ode.dBodyIsEnabled(o1BodyID) == 0 &&
ignoreStaticSleepingContacts &&
neitherHasEventHandler && hasNoGlobalHandler;
bool case7 = solid1Static && null != o0BodyID && o0BodyID.IsNotNull() &&
Tao.Ode.Ode.dBodyIsEnabled(o0BodyID) == 0 &&
ignoreStaticSleepingContacts &&
neitherHasEventHandler && hasNoGlobalHandler;
bool case8 = !makeContacts && neitherHasEventHandler &&
hasNoGlobalHandler;
if (case1 || case3 || case4 || case5 || case6 || case7 || case8)
{
return;
}
// Now actually test for collision between the two geoms.
// This is one of the more expensive operations.
IntPtr theWorldID = sim.World;
IntPtr theJointGroupID = sim.JointGroup;
int contGeomSize = sizeof(Tao.Ode.Ode.dContactGeom);
Tao.Ode.Ode.dContactGeom[] contactArray = new Tao.Ode.Ode.dContactGeom[sim.MaxContacts];
int numContacts = 0;
/*try
* {*/
numContacts = Tao.Ode.Ode.dCollide(o0, o1, sim.MaxContacts /*was 15*/, contactArray, contGeomSize);
// Was big perfomance problem here (KleMiX)
/*}
* catch(Exception)
* {
* return;
* }*/
// If the two objects didn't make any contacts, they weren't
// touching, so just return.
if (0 == numContacts)
{
return;
}
// If at least one of the Solids has a CollisionEventHandler,
// send it a CollisionEvent.
if (handler0 != null || handler1 != null || !hasNoGlobalHandler)
{
// Call the CollisionEventHandlers. Note that we only
// use one contact point per collision: just the first one
// in the contact array. The order of the Solids
// passed to the event handlers is important: the first
// one should be the one whose event handler is
// getting called.
CollisionEvent e = new CollisionEvent();
e.ThisSolid = solid0;
e.OtherSolid = solid1;
e.Position.X = contactArray[0].pos.X;
e.Position.Y = contactArray[0].pos.Y;
e.Position.Z = contactArray[0].pos.Z;
e.Normal.X = contactArray[0].normal.X;
e.Normal.Y = contactArray[0].normal.Y;
e.Normal.Z = contactArray[0].normal.Z;
e.Depth = contactArray[0].depth;
if (handler0 != null)
{
handler0.HandleCollisionEvent(e);
}
if (handler1 != null)
{
// For the other Solid's CollisionEventHandler, we need
// to invert the normal and swap the Solid pointers.
e.Normal *= -1;
e.ThisSolid = solid1;
e.OtherSolid = solid0;
handler1.HandleCollisionEvent(e);
}
sim.InternalRecordCollision(e);
}
if (makeContacts)
{
// Invalidate the "freely-spinning" parameters.
((OdeSolid)solid0).InternalSetFreelySpinning(false);
((OdeSolid)solid1).InternalSetFreelySpinning(false);
for (int i = 0; i < numContacts; ++i)
{
Material m0 = shape0.Material;
Material m1 = shape1.Material;
Tao.Ode.Ode.dContact tempContact = new Tao.Ode.Ode.dContact();
tempContact.surface.mode = (int)Tao.Ode.Ode.dContactFlags.dContactBounce |
(int)Tao.Ode.Ode.dContactFlags.dContactSoftERP;
// Average the hardness of the two materials.
float hardness = (m0.Hardness + m1.Hardness) * 0.5f;
// Convert hardness to ERP. As hardness goes from
// 0.0 to 1.0, ERP goes from min to max.
tempContact.surface.soft_erp = hardness *
(Defaults.Ode.MaxERP - Defaults.Ode.MinERP) +
Defaults.Ode.MinERP;
// Don't use contact CFM anymore. Just let it use
// the global value set in the ODE world.
//tempContact.surface.soft_cfm =
// defaults::ode::minCFM;
// As friction goes from 0.0 to 1.0, mu goes from 0.0
// to max, though it is set to dInfinity when
// friction == 1.0.
if (1.0 == m0.Friction && 1.0 == m1.Friction)
{
tempContact.surface.mu = float.PositiveInfinity;
}
else
{
tempContact.surface.mu = (float)Math.Sqrt(m0.Friction * m1.Friction) * Defaults.Ode.MaxFriction;
}
// Average the bounciness of the two materials.
float bounciness = (m0.Bounciness + m1.Bounciness) * 0.5f;
// ODE's bounce parameter, a.k.a. restitution.
tempContact.surface.bounce = bounciness;
// ODE's bounce_vel parameter is a threshold:
// the relative velocity of the two objects must be
// greater than this for bouncing to occur at all.
tempContact.surface.bounce_vel = Defaults.BounceThreshold;
tempContact.geom = contactArray[i];
//ODE.Joints.Contact contactJoint = new ODE.Joints.Contact(theWorldID, tempContact, theJointGroupID);
IntPtr contactJoint = Tao.Ode.Ode.dJointCreateContact(theWorldID, theJointGroupID, ref tempContact);
//contactJoint.Attach(o0BodyID, o1BodyID);
Tao.Ode.Ode.dJointAttach(contactJoint, o0BodyID, o1BodyID);
}
}
}
}
public override void AddShape(ShapeData data)
{
if (data.Material.Density < 0)
{
return;
}
dGeomID newGeomID = null;
dGeomID newTransformID = null;
dTriMeshDataID newTrimeshDataID = null;
dSpaceID tempSpaceID = null;
Tao.Ode.Ode.dMass newMass = new Tao.Ode.Ode.dMass();
if (new Matrix() == data.Offset)
{
// No offset transform.
tempSpaceID = spaceID;
newTransformID = null;
}
else
{
// Use ODE's geom transform object.
tempSpaceID = IntPtr.Zero;
newTransformID = spaceID.CreateGeomTransform();
}
// Allocate a new GeomData object.
GeomData newGeomData = new GeomData();
switch (data.Type)
{
case ShapeType.Box:
{
BoxShapeData boxData = data as BoxShapeData;
newGeomID = tempSpaceID.CreateBoxGeom(boxData.Dimensions.X,
boxData.Dimensions.Y,
boxData.Dimensions.Z);
Tao.Ode.Ode.dMassSetBox(ref newMass, data.Material.Density,
boxData.Dimensions.X,
boxData.Dimensions.Y,
boxData.Dimensions.Z);
break;
}
case ShapeType.Sphere:
{
SphereShapeData sphereData = data as SphereShapeData;
newGeomID = tempSpaceID.CreateSphereGeom(sphereData.Radius);
Tao.Ode.Ode.dMassSetSphere(ref newMass, data.Material.Density, sphereData.Radius);
break;
}
case ShapeType.Capsule:
{
CapsuleShapeData capsuleData = data as CapsuleShapeData;
newGeomID = tempSpaceID.CreateCylinderGeom(capsuleData.Radius, capsuleData.Length);
// The "direction" parameter orients the mass along one of the
// body's local axes; x=1, y=2, z=3. This axis MUST
// correspond with the axis along which the capsule is
// initially aligned, which is the capsule's local Z axis.
/*Tao.Ode.Ode.dMassSetCylinder(ref newMass, data.Material.Density,
* 3, capsuleData.Radius, capsuleData.Length);*/
Tao.Ode.Ode.dMassSetCapsule(ref newMass, data.Material.Density, 3,
capsuleData.Radius, capsuleData.Length);
break;
}
case ShapeType.Plane:
{
PlaneShapeData planeData = data as PlaneShapeData;
if (!this.data.IsStatic)
{
//OPAL_LOGGER( "warning" ) << "opal::ODESolid::addPlane: " <<
//"Plane Shape added to a non-static Solid. " <<
//"The Solid will be made static." << std::endl;
// ODE planes can't have bodies, so make it static.
this.Static = true;
}
// TODO: make this fail gracefully and print warning: plane
// offset transform ignored.
if (newTransformID != null)
{
break;
}
// ODE planes must have their normal vector (abc) normalized.
Vector3 normal = new Vector3(planeData.Abcd[0], planeData.Abcd[1], planeData.Abcd[2]);
normal.Normalize();
newGeomID = tempSpaceID.CreatePlaneGeom(normal.X, normal.Y, normal.Z, planeData.Abcd[3]);
// Note: ODE planes cannot have mass, but this is already
// handled since static Solids ignore mass.
// Solids with planes are the only non-"placeable" Solids.
isPlaceable = false;
break;
}
//case RAY_SHAPE:
//{
// RayShapeData& rayData = (RayShapeData&)data;
// newGeomID = dCreateRay(spaceID,
// (dReal)rayData.ray.getLength());
// Point3r origin = rayData.ray.getOrigin();
// Vec3r dir = rayData.ray.getDir();
// dGeomRaySet(newGeomID, (dReal)origin[0], (dReal)origin[1],
// (dReal)origin[2], (dReal)dir[0], (dReal)dir[1],
// (dReal)dir[2]);
// // Note: rays don't have mass.
// break;
//}
case ShapeType.Mesh:
{
MeshShapeData meshData = data as MeshShapeData;
// Setup trimesh data pointer. It is critical that the
// size of OPAL reals at this point match the size of ODE's
// dReals.
newTrimeshDataID = dTriMeshDataID.Create();
// Old way... This is problematic because ODE interprets
// the vertex array as an array of dVector3s which each
// have 4 elements.
//dGeomTriMeshDataBuildSimple(newTrimeshDataID,
// (dReal*)meshData.vertexArray, meshData.numVertices,
// (int*)meshData.triangleArray, 3 * meshData.numTriangles);
//#ifdef dSINGLE
newTrimeshDataID.BuildSingle(meshData.VertexArray, 3 * sizeof(float),
meshData.NumVertices,
meshData.TriangleArray,
3 * meshData.NumTriangles,
3 * sizeof(int));
//#else
// dGeomTriMeshDataBuildDouble( newTrimeshDataID,
// ( void* ) meshData.vertexArray, 3 * sizeof( real ),
// meshData.numVertices, ( void* ) meshData.triangleArray,
// 3 * meshData.numTriangles, 3 * sizeof( unsigned int ) );
//#endif
newGeomID = tempSpaceID.CreateTriMeshGeom(newTrimeshDataID);
// This is a simple way to set the mass of an arbitrary
// mesh. Ideally we would compute the exact volume and
// intertia tensor. Instead we just use a box
// inertia tensor from its axis-aligned bounding box.
float[] aabb = newGeomID.AABB;
// dGeomGetAABB( newGeomID, aabb );
Tao.Ode.Ode.dMassSetBox(ref newMass, data.Material.Density,
aabb[1] - aabb[0], aabb[3] - aabb[2],
aabb[5] - aabb[4]);
break;
}
default:
throw new InvalidOperationException("OdeSolid.AddShape");
}
// This will do nothing if this is a static Solid.
AddMass(newMass, data.Offset);
// Store new Shape.
this.data.AddShape(data);
// Update the Solid's local AABB. The new shape's local AABB
// must be transformed using the shape's offset from the Solid.
BoundingBox shapeAABB = data.LocalAABB;
//data.LocalAABBgetLocalAABB( shapeAABB );
Vector3 minExtents = Vector3.Transform(shapeAABB.Min, data.Offset);
Vector3 maxExtents = Vector3.Transform(shapeAABB.Max, data.Offset);
shapeAABB.Min = minExtents;
shapeAABB.Max = maxExtents;
AddToLocalAABB(shapeAABB);
if (newGeomData == null)
{
return;
}
// Setup GeomData.
newGeomData.Solid = this;
newGeomData.Shape = this.data.GetShapeData(this.data.NumShapes - 1);
newGeomData.GeomID = newGeomID;
newGeomData.TransformID = newTransformID;
newGeomData.SpaceID = spaceID;
newGeomData.TrimeshDataID = newTrimeshDataID;
// Setup the geom.
SetupNewGeom(newGeomData);
}
