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