Пример #1
0
        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;
                    }
                }
            }
        }
Пример #2
0
        /// <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;
            }
        }
Пример #3
0
        /// 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);
                }
            }
        }
Пример #4
0
        /// 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);
                }
            }
        }
Пример #5
0
        /// 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);
                    }
                }
            }
        }
Пример #6
0
        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);
        }