/// <summary>
/// Returns true if the given mass has a non-symmetric inertia tensor.
/// </summary>
/// <param name="mass"></param>
/// <returns></returns>
protected bool IsInertiaNonSymmetric(Tao.Ode.Ode.dMass mass)
{
if (!MathUtil.AreEqual(mass.I.M00, mass.I.M11) ||
!MathUtil.AreEqual(mass.I.M11, mass.I.M22))
{
return(true);
}
else
{
return(false);
}
}
public override void TranslateMass(Vector3 offset)
{
if (!data.IsStatic)
{
Tao.Ode.Ode.dMass m = bodyID.Mass;
Tao.Ode.Ode.dMassTranslate(ref m, offset.X, offset.Y, offset.Z);
bodyID.Mass = m;
// Update this since the mass changed.
nonSymmetricInertia = IsInertiaNonSymmetric(m);
}
}
/// <summary>
/// Adds the given mass to this Solid's existing mass. The offset is
/// relative to the Solid's center. This must be called before
/// setupNewGeom is called.
/// </summary>
/// <param name="newMass"></param>
/// <param name="offset"></param>
public void AddMass(Tao.Ode.Ode.dMass newMass, Matrix offset)
{
if (data.IsStatic)
{
return;
}
// First rotate and translate the new mass.
Matrix R = offset;
R.Translation = Vector3.Zero;
Tao.Ode.Ode.dMassRotate(ref newMass, R);
Vector3 t = offset.Translation;
Tao.Ode.Ode.dMassTranslate(ref newMass, t.X, t.Y, t.Z);
// If this mass is for the first Shape, just set the Solid's mass
// equal to the new one. ODE bodies start with an initial mass
// already setup, but we want to ignore that, not add to it.
if (geomDataList.Count == 0)
{
bodyID.Mass = newMass;
}
else
{
// Add new mass to the Solid's existing mass. First get the
// existing mass struct from ODE.
Tao.Ode.Ode.dMass totalMass = bodyID.Mass;
//Tao.Ode.Ode.dMassAdd(ref totalMass, newMass); // (KleMiX) strange bug
float denom = 1 / (totalMass.mass + newMass.mass);
totalMass.c.X = (totalMass.c.X * totalMass.mass + newMass.c.X * newMass.mass) * denom;
totalMass.c.Y = (totalMass.c.Y * totalMass.mass + newMass.c.Y * newMass.mass) * denom;
totalMass.c.Z = (totalMass.c.Z * totalMass.mass + newMass.c.Z * newMass.mass) * denom;
totalMass.mass += newMass.mass;
for (int i = 0; i < 12; i++)
{
totalMass.I[i] += newMass.I[i];
}
bodyID.Mass = totalMass;
}
// Update this since the mass changed.
Tao.Ode.Ode.dMass m = bodyID.Mass;
nonSymmetricInertia = IsInertiaNonSymmetric(m);
}
public override void SetMass(Mass mass, Matrix offset)
{
if (data.IsStatic)
{
return;
}
// First rotate and translate the new mass.
Tao.Ode.Ode.dMatrix3 R = offset;
Tao.Ode.Ode.dMass m = new Tao.Ode.Ode.dMass();
m.c = new Tao.Ode.Ode.dVector4(mass.Center.X, mass.Center.Y, mass.Center.Z, 0);
m.I = mass.Inertia;
m.mass = mass.MassValue;
Tao.Ode.Ode.dMassRotate(ref m, R);
Tao.Ode.Ode.dMassTranslate(ref m, offset.M41, offset.M42, offset.M43);
bodyID.Mass = m;
// Update this since the mass changed.
m = bodyID.Mass;
nonSymmetricInertia = IsInertiaNonSymmetric(m);
}
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();
}
}
}
}
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);
}
