public override void InternalUpdateEngineTransform() { Matrix R = data.Transform; R.Translation = Vector3.Zero; if (!data.IsStatic) { bodyID.Rotation = R; bodyID.Position = data.Transform.Translation; } else if (isPlaceable) { foreach (GeomData iter in geomDataList) { GeomData geomData = iter; if (geomData.TransformID == null) { // No geom transform. geomData.GeomID.Rotation = R; geomData.GeomID.Position = data.Transform.Translation; } else { // Using geom transform. geomData.TransformID.Rotation = R; geomData.TransformID.Position = data.Transform.Translation; } } } }
/// <summary> /// Adds a new GeomData object to the internal list and sets up the ODE geom. /// </summary> /// <param name="newGeom"></param> protected void SetupNewGeom(GeomData newGeom) { if (!data.IsStatic) { if (newGeom.TransformID == null) { // No geom transform. newGeom.GeomID.RigidBody = bodyID; } else { // Use geom transform. newGeom.TransformID.RigidBody = bodyID; } } if (newGeom.TransformID != null) { // Setup geom transform. Tao.Ode.Ode.dGeomTransformSetGeom(newGeom.TransformID, newGeom.GeomID); Matrix R = newGeom.Shape.Offset; R.Translation = Vector3.Zero; if (newGeom.GeomID != null) { newGeom.GeomID.Rotation = R; newGeom.GeomID.Position = newGeom.Shape.Offset.Translation; } } // Set the GeomData reference for later use (e.g. in collision // handling). if (newGeom.TransformID == null) { // No geom transform. Tao.Ode.Ode.dGeomSetData(newGeom.GeomID, GCHandle.ToIntPtr(GCHandle.Alloc(newGeom))); } else { // Using geom transform. Tao.Ode.Ode.dGeomSetData(newGeom.TransformID, GCHandle.ToIntPtr(GCHandle.Alloc(newGeom))); } // Store the GeomData pointer. geomDataList.Add(newGeom); // Make sure the initial transform is setup; this needs to come after // the geom data has been added. if (data.IsStatic && isPlaceable) { this.Transform = data.Transform; } }
/// Collision callback functor for ray casting. Stores results /// in an unsorted array. static unsafe internal void RaycastCollisionCallback(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, RaycastCollisionCallback); } else { // Colliding two geoms. // Sometimes we get a case where the ray geom is passed in // as both objects, which is stupid. if (o0 == o1) { return; } // Get a pointer to the ODESimulator. OdeSimulator sim = GCHandle.FromIntPtr(data).Target as OdeSimulator; // Get pointers to the two geoms' GeomData structure. One // of these (the one NOT belonging to the ray geom) // will always be non-NULL. /*GeomData geomData0 = (GeomData)Tao.Ode.Ode.dGeomGetData(o0); * GeomData geomData1 = (GeomData)Tao.Ode.Ode.dGeomGetData(o1);*/ GeomData geomData0 = GCHandle.FromIntPtr(o0).Target as GeomData; GeomData geomData1 = GCHandle.FromIntPtr(o1).Target as GeomData; // Find the contact group of the collided Solid. int geomContactGroup = Defaults.Shape.ContactGroup; if (geomData0 != null) { geomContactGroup = geomData0.Shape.ContactGroup; } else { geomContactGroup = geomData1.Shape.ContactGroup; } // Check if the two Solids' contact groups generate contacts // when they collide. bool makeContacts = sim.GroupsMakeContacts(geomContactGroup, sim.InternalGetRayContactGroup()); 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[Defaults.Ode.maxRaycastContacts]; Tao.Ode.Ode.dContactGeom[] contactArray = new Tao.Ode.Ode.dContactGeom[Defaults.Ode.MaxRaycastContacts]; int numContacts = Tao.Ode.Ode.dCollide(o0, o1, Defaults.Ode.MaxRaycastContacts, contactArray, sizeof(Tao.Ode.Ode.dContactGeom)); if (0 == numContacts) { return; } else { // These two geoms must be intersecting. We will store // only the closest RaycastResult. int closest = 0; for (int i = 0; i < numContacts; i++) { if (contactArray[i].depth < contactArray[closest].depth) { closest = i; } } // Only one of the geoms will be part of a Solid we // want to store; the other is the ray. Solid solid = null; if (geomData0 != null) { solid = geomData0.Solid; } else { solid = geomData1.Solid; } Vector3 intersection = FromOdeVector3(contactArray[closest].pos); Vector3 normal = FromOdeVector3(contactArray[closest].normal); sim.InternalAddRaycastResult(solid, intersection, normal, contactArray[closest].depth); } } }
/// 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); }