public override Vector3 GetLocalLinearVelAtLocalPos(Vector3 p) { if (!data.IsStatic) { // First find the global velocity at the given point. Vector3 vel = bodyID.GetRelPointVel(p); // Now convert the velocity from global to local coordinates. vel = bodyID.BodyVectorFromWorld(vel); return(vel); } else { return(Vector3.Zero); } }
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(); } } } }