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