public OdeSolid(dWorldID worldID, dSpaceID spaceID) { this.worldID = worldID; this.spaceID = spaceID; isPlaceable = true; collisionCount = 0; nonSymmetricInertia = false; isFreelySpinning = true; prevAngVelMagSquared = 0; data = new SolidData(); if (!data.IsStatic) { // Create an ODE body with default ODE mass parameters (total // mass = 1). This should have an initial mass of 0 until shapes // are added, but ODE won't allow a mass of 0. This will be // adjusted appropriately when the first shape is added. bodyID = this.worldID.CreateBody(); //mBodyID = new Body(mWorldID); } Init(data); }
/// <summary> /// Attach this joint to some new bodies. /// /// If the joint is already attached, it will be detached from the old bodies first. /// To attach this joint to only one body, set body1 or body2 to zero - a zero body /// refers to the static environment. /// Setting both bodies to zero puts the joint into "limbo", i.e. it will have no /// effect on the simulation. /// Some joints, like hinge-2 need to be attached to two bodies to work. /// </summary> public void Attach(dBodyID b1Id, dBodyID b2Id) { if (IsNull()) { return; } Tao.Ode.Ode.dJointAttach(this, b1Id, b2Id); }
protected override void StepPhysics() { // Apply linear and angular damping; if using the "add opposing // forces" method, be sure to do this before calling ODE step // function. if (solidList != null) { foreach (OdeSolid solid in solidList) { if (!solid.Static) { if (solid.Sleeping) { // Reset velocities, force, & torques of objects that go // to sleep; ideally, this should happen in the ODE // source only once - when the object initially goes to // sleep. /*ODE.Body body = solid.InternalGetBodyID(); * body.ApplyLinearVelocityDrag(0, 0, 0); * body.ApplyAngularVelocityDrag(0, 0, 0); * body.AddForce(0, 0, 0); * body.AddTorque(0, 0, 0);*/ IntPtr body = solid.InternalGetBodyID(); Tao.Ode.Ode.dBodySetLinearVel(body, 0, 0, 0); Tao.Ode.Ode.dBodySetAngularVel(body, 0, 0, 0); Tao.Ode.Ode.dBodySetForce(body, 0, 0, 0); Tao.Ode.Ode.dBodySetTorque(body, 0, 0, 0); } else { // Dampen Solid motion. 3 possible methods: // 1) apply a force/torque in the opposite direction of // motion scaled by the body's velocity // 2) same as 1, but scale the opposing force by // the body's momentum (this automatically handles // different mass values) // 3) reduce the body's linear and angular velocity by // scaling them by 1 - damping * stepsize /*ODE.Body body = solid.InternalGetBody(); * ODE.Mass mass = body.Mass;*/ dBodyID body = solid.InternalGetBodyID(); Tao.Ode.Ode.dMass mass = body.Mass; //Tao.Ode.Ode.dBodyGetMass(body, ref mass); // (mike) // Method 2 // Since the ODE mass.I inertia matrix is local, angular // velocity and torque also need to be local. // Linear damping float linearDamping = solid.LinearDamping; if (0 != linearDamping) { Vector3 lVelGlobal = Tao.Ode.Ode.dBodyGetLinearVel(body); // C# compiler automatically calls an implicit convertion here // The damping force depends on the damping amount, // mass, and velocity (i.e. damping amount and // momentum). float dampingFactor = -linearDamping * mass.mass; Vector3 dampingForce = new Vector3(dampingFactor * lVelGlobal.X, dampingFactor * lVelGlobal.Y, dampingFactor * lVelGlobal.Z); // Add a global force opposite to the global linear // velocity. body.AddForce(dampingForce); //(mike)//Tao.Ode.Ode.dBodyAddForce(body, dampingForce.X, dampingForce.Y, dampingForce.Z); } // Angular damping float angularDamping = solid.AngularDamping; if (0 != angularDamping) { Vector3 aVelGlobal = Tao.Ode.Ode.dBodyGetAngularVel(body); // C# compiler automatically calls an implicit convertion here //(mike) //Tao.Ode.Ode.dBodyVectorFromWorld(body, aVelGlobal.X, aVelGlobal.Y, aVelGlobal.Z, ref temp); Vector3 aVelLocal = body.BodyVectorFromWorld(aVelGlobal); // The damping force depends on the damping amount, // mass, and velocity (i.e. damping amount and // momentum). float dampingFactor = -angularDamping; Vector3 aMomentum; // Make adjustments for inertia tensor. //dMULTIPLYOP0_331( aMomentum, = , mass.I, aVelLocal ); (KleMiX) ??? aMomentum.X = mass.I.M00 * aVelLocal.X + mass.I.M01 * aVelLocal.Y + aVelLocal.X + mass.I.M02 * aVelLocal.Z; aMomentum.Y = mass.I.M10 * aVelLocal.X + mass.I.M11 * aVelLocal.Y + aVelLocal.X + mass.I.M12 * aVelLocal.Z; aMomentum.Z = mass.I.M20 * aVelLocal.X + mass.I.M21 * aVelLocal.Y + aVelLocal.X + mass.I.M22 * aVelLocal.Z; Vector3 dampingTorque = new Vector3(dampingFactor * aMomentum.X, dampingFactor * aMomentum.Y, dampingFactor * aMomentum.Z); // Add a local torque opposite to the local angular // velocity. //body.AddRelativeTorque(dampingTorque); Tao.Ode.Ode.dBodyAddRelTorque(body, dampingTorque.X, dampingTorque.Y, dampingTorque.Z); } } } } } // Do collision detection; add contacts to the contact joint group. //space.Collide(); Tao.Ode.Ode.dSpaceCollide(space, GCHandle.ToIntPtr(GCHandle.Alloc(this)), OdeHelper.CollisionCallbackRunner); // Take a simulation step. if (SolverType.QuickStep == solverType) { //world.QuickStep(stepSize, quickWorldIterations); Tao.Ode.Ode.dWorldQuickStep(world, stepSize); } else { //world.TimeStep(stepSize); Tao.Ode.Ode.dWorldStep(world, stepSize); } // Remove all joints from the contact group. //contactJointGroup.Clear(); Tao.Ode.Ode.dJointGroupEmpty(contactJointGroup); // Fix angular velocities for freely-spinning bodies that have // gained angular velocity through explicit integrator inaccuracy. if (solidList != null) { foreach (OdeSolid s in solidList) { if (!s.Sleeping && !s.Static) { s.InternalDoAngularVelFix(); } } } }
/// 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); } } } }