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();
}
}
}
}