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);
}
}
public void dMassRotateTest()
{
Ode.dMass newMass = new Ode.dMass();
newMass.c = new Ode.dVector4(); // Just to be safe the dMass
newMass.I = new Ode.dMatrix3(); // structure is initialized
Ode.dMassSetSphere(ref newMass, 0.2f, 3.5f);
Ode.dMatrix3 r = new Ode.dMatrix3(
new float[] {
1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f
}
);
try {
Ode.dMassRotate(ref newMass, r);
}
catch (Exception e) {
Assert.Fail("dMassRotate failed with exception: " + e.GetType());
}
}
public BodyBox(World hostWorld, Space space, Vector3f position, Vector3f size, Vector3f force)
{
this.hostWorld = hostWorld;
this.space = space;
bodyID = Ode.dBodyCreate(hostWorld.getID());
// create a mass object, in this case a box of size 50 x 0.2 x 50
Ode.dMass mass = new Ode.dMass();
//Ode.dMassSetBox(ref mass, 200.0f, radius, radius, radius);
Ode.dMassSetBoxTotal(ref mass, 200.0f, size.x, size.y, size.z);
// set it's mass to 1000.0f. If this value is too low,
// you'll get some wierd collisions
//mass.mass = 1000.0f;
// set the mass object on the body
Ode.dBodySetMass(bodyID, ref mass);
// Set the body's position
Ode.dBodySetPosition(bodyID, position.x, position.y, position.z);
// Set an initial force on the body. This will be
// wiped to zero after the first frame.
Ode.dBodyAddForce(bodyID, force.x, force.y, force.z);
// create a collion geometry to go with our rigid body.
// without this, the rigid body will not collide with anything.
geomID = Ode.dCreateBox(space.getSpaceID(), size.x, size.y, size.z);
// assign a rigid body to the collision geometry. If we didn't do this,
// the object would be a static object much like our ground plane.
Ode.dGeomSetBody(geomID, bodyID);
this.position = position.copy();
this.rotationOGL = new Color4f(0.0f, 0.0f, 0.0f, 0.0f);
}
/// <summary>
/// Application's entry point.
/// </summary>
public static void Main()
{
// Create a world
IntPtr world = Ode.dWorldCreate();
// Add gravity to the world (pull down on the Y axis 9.81 meters/second
Ode.dWorldSetGravity(world, 0, -9.81f, 0);
// Create a rigid body (in the world)
IntPtr body = Ode.dBodyCreate(world);
// Create some mass, we're creating a sphere with a radius of 0.05 centimeters
// and a constant density of 2500 (about that of glass)
Ode.dMass mass = new Ode.dMass();
Ode.dMassSetSphere(ref mass, 2500, 0.05f);
// If you printed the values of mass now, you'd see the mass would be about 1.3 kilograms
// We'll change that to 1 kilogram here
mass.mass = 1;
// If you printed the values of mass now, you'd notice that the inertia tensions values
// were also updated
// We'll set the body's mass to the mass we just created
Ode.dBodySetMass(body, ref mass);
// Set the body's position (in the world) to 2 meters above the 'ground'
Ode.dBodySetPosition(body, 0, 2, 0);
// Apply a force to the body, we're sending it vertical, up the Y axis
// This force is only applied for the first step, forces will be reset to zero
// at the end of each step
Ode.dBodyAddForce(body, 0, 200, 0);
// The simulation loop's 'time', in seconds
float time = 0;
// The 'time' increment, in seconds
float deltaTime = 0.04f;
// Run the simulation loop for 2 seconds
while (time < 2)
{
// Get the body's current position
Ode.dVector3 position = Ode.dBodyGetPosition(body);
// Get the body's current linear velocity
Ode.dVector3 velocity = Ode.dBodyGetLinearVel(body);
// Print out the 'time', the body's position, and its velocity
Console.WriteLine("{0:0.00} sec: pos=({1:0.00}, {2:0.00}, {3:0.00}) vel={4:0.00}, {5:0.00}, {6:0.00})",
time, position[0], position[1], position[2], velocity[0], velocity[1], velocity[2]);
// Move the bodies in the world
Ode.dWorldStep(world, deltaTime);
// Increment the time
time += deltaTime;
}
Console.WriteLine();
Console.WriteLine("Press Enter to exit...");
Console.ReadLine();
}
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 );
}
