internal void DoDamping() { if( bodyID == dBodyID.Zero ) return; Ode.dMass mass = new Ode.dMass(); Ode.dBodyGetMass( bodyID, ref mass ); // Linear damping if( LinearDamping != 0 ) { // The damping force depends on the damping amount, mass, and velocity // (i.e. damping amount and momentum). float factor = -LinearDamping * mass.mass; Vec3 force = LinearVelocity * factor; // Add a global force opposite to the global linear velocity. Ode.dBodyAddForce( bodyID, force.X, force.Y, force.Z ); } // Angular damping if( AngularDamping != 0 ) { Vec3 localVelocity; { Ode.dVector3 aVelLocal = new Ode.dVector3(); Ode.dBodyVectorFromWorld( bodyID, AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z, ref aVelLocal ); localVelocity = Convert.ToNet( aVelLocal ); } // The damping force depends on the damping amount, mass, and velocity // (i.e. damping amount and momentum). float factor = -AngularDamping; Vec3 momentum = new Vec3( Vec3.Dot( new Vec3( mass.I.M00, mass.I.M01, mass.I.M02 ), localVelocity ), Vec3.Dot( new Vec3( mass.I.M10, mass.I.M11, mass.I.M12 ), localVelocity ), Vec3.Dot( new Vec3( mass.I.M20, mass.I.M21, mass.I.M22 ), localVelocity ) ); Vec3 torque = momentum * factor; // Add a local torque opposite to the local angular velocity. Ode.dBodyAddRelTorque( bodyID, torque.X, torque.Y, torque.Z ); } }
void CalculateBodyMass() { float totalVolume = 0; if( MassMethod == MassMethods.Manually ) { for( int nShape = 0; nShape < Shapes.Length; nShape++ ) totalVolume += Shapes[ nShape ].Volume; if( totalVolume == 0 ) totalVolume = .001f; } Ode.dMass bodyMass = new Ode.dMass(); bool bodyMassInitialized = false; for( int nShape = 0; nShape < Shapes.Length; nShape++ ) { Shape shape = Shapes[ nShape ]; Ode.dMass shapeMass = new Ode.dMass(); float shapeDensity; if( MassMethod == MassMethods.Manually ) shapeDensity = Mass / totalVolume; else shapeDensity = shape.Density; if( shapeDensity <= 0 ) shapeDensity = .0001f; switch( shape.ShapeType ) { case Shape.Type.Box: { BoxShape boxShape = (BoxShape)shape; Ode.dMassSetBox( ref shapeMass, shapeDensity, boxShape.Dimensions.X, boxShape.Dimensions.Y, boxShape.Dimensions.Z ); } break; case Shape.Type.Sphere: { SphereShape sphereShape = (SphereShape)shape; Ode.dMassSetSphere( ref shapeMass, shapeDensity, sphereShape.Radius ); } break; case Shape.Type.Capsule: { CapsuleShape capsuleShape = (CapsuleShape)shape; Ode.dMassSetCapsule( ref shapeMass, shapeDensity, 3, capsuleShape.Radius, capsuleShape.Length ); } break; case Shape.Type.Cylinder: { CylinderShape cylinderShape = (CylinderShape)shape; Ode.dMassSetCylinder( ref shapeMass, shapeDensity, 3, cylinderShape.Radius, cylinderShape.Length ); } break; case Shape.Type.Mesh: { GeomData geomData = geomDatas[ nShape ]; //ignore this shape if( geomData == null ) continue; IntPtr geomID; if( geomData.transformID == dGeomID.Zero ) geomID = geomData.geomID; else geomID = geomData.transformID; //ignore this shape if( geomID == dGeomID.Zero ) continue; Ode.Aabb aabb = new Ode.Aabb(); Ode.dGeomGetAABB( geomID, ref aabb ); Ode.dMassSetBox( ref shapeMass, shapeDensity, aabb.maxx - aabb.minx, aabb.maxy - aabb.miny, aabb.maxz - aabb.minz ); //correct shapeMass.mass = shape.Volume * shapeDensity; if( shapeMass.mass <= 0 ) shapeMass.mass = .001f; } break; default: Trace.Assert( false ); break; } if( shape.Rotation != Quat.Identity ) { Mat3 mat3; shape.Rotation.ToMat3( out mat3 ); Ode.dMatrix3 odeMat3; Convert.ToODE( ref mat3, out odeMat3 ); Ode.dMassRotate( ref shapeMass, ref odeMat3 ); } if( shape.Position != Vec3.Zero ) { Ode.dMassTranslate( ref shapeMass, shape.Position.X, shape.Position.Y, shape.Position.Z ); } if( !bodyMassInitialized ) { bodyMass = shapeMass; bodyMassInitialized = true; } else Ode.dMassAdd( ref bodyMass, ref shapeMass ); } if( MassMethod == MassMethods.Manually ) { bodyMass.mass = Mass; if( bodyMass.mass <= 0 ) bodyMass.mass = .0001f; } if( bodyMass.mass != 0 ) { //if( CenterOfMassAuto ) // Log.Warning( "ODEBody: CenterOfMassAuto is not supported on ODE physics." ); //!!!!!!тут вручную введенное положение цента масс Ode.dMassTranslate( ref bodyMass, -bodyMass.c.X, -bodyMass.c.Y, -bodyMass.c.Z ); Ode.dBodySetMass( bodyID, ref bodyMass ); } ////calculate mNonSymmetricInertia //nonSymmetricInertia = // !AreEqual( bodyMass.I.M00, bodyMass.I.M11 ) || // !AreEqual( bodyMass.I.M11, bodyMass.I.M22 ); }