void PushToWorld()
{
if (!Static)
{
bodyID = Ode.dBodyCreate(scene.worldID);
UpdateSleepiness();
Ode.dBodySetPosition(bodyID, Position.X, Position.Y, Position.Z);
Ode.dQuaternion odeQuat;
Convert.ToODE(Rotation, out odeQuat);
Ode.dBodySetQuaternion(bodyID, ref odeQuat);
Ode.dBodySetLinearVel(bodyID, LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z);
Ode.dBodySetAngularVel(bodyID, AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z);
if (Sleeping)
{
Ode.dBodyDisable(bodyID);
}
else
{
Ode.dBodyEnable(bodyID);
}
if (!EnableGravity)
{
Ode.dBodySetGravityMode(bodyID, 0);
}
}
bodyData = Ode.CreateBodyData(bodyID);
CreateGeomDatas();
//no shapes
if (geomDatas == null || geomDatas.Length == 0)
{
PopFromWorld();
return;
}
if (!Static)
{
CalculateBodyMass();
}
RecreateAttachedJoints();
}
//public override Mat3 GetInertiaTensor()
//{
// if( Static )
// return Mat3.Identity;
// if( bodyID == dBodyID.Zero )
// return Mat3.Zero;
// Ode.dMass mass = new Ode.dMass();
// Ode.dBodyGetMass( bodyID, ref mass );
// return new Mat3(
// mass.I.M00, mass.I.M01, mass.I.M02,
// mass.I.M10, mass.I.M11, mass.I.M12,
// mass.I.M20, mass.I.M21, mass.I.M22 );
//}
protected override void ApplyForce(ForceType type, ref Vec3 vector, ref Vec3 pos)
{
if (bodyID == dBodyID.Zero)
{
return;
}
Ode.dBodyEnable(bodyID);
switch (type)
{
case ForceType.Local:
Ode.dBodyAddRelForce(bodyID, vector.X, vector.Y, vector.Z);
break;
case ForceType.Global:
Ode.dBodyAddForce(bodyID, vector.X, vector.Y, vector.Z);
break;
case ForceType.LocalTorque:
Ode.dBodyAddRelTorque(bodyID, vector.X, vector.Y, vector.Z);
break;
case ForceType.GlobalTorque:
Ode.dBodyAddTorque(bodyID, vector.X, vector.Y, vector.Z);
break;
case ForceType.LocalAtLocalPos:
Ode.dBodyAddRelForceAtRelPos(bodyID, vector.X, vector.Y, vector.Z, pos.X, pos.Y, pos.Z);
break;
case ForceType.LocalAtGlobalPos:
Ode.dBodyAddRelForceAtPos(bodyID, vector.X, vector.Y, vector.Z, pos.X, pos.Y, pos.Z);
break;
case ForceType.GlobalAtLocalPos:
Ode.dBodyAddForceAtRelPos(bodyID, vector.X, vector.Y, vector.Z, pos.X, pos.Y, pos.Z);
break;
case ForceType.GlobalAtGlobalPos:
Ode.dBodyAddForceAtPos(bodyID, vector.X, vector.Y, vector.Z, pos.X, pos.Y, pos.Z);
break;
}
//// Invalidate the "freely-spinning" parameter.
//freelySpinning = false;
}
protected override void OnSetSleeping()
{
if (PushedToWorld && bodyID != IntPtr.Zero)
{
if (Sleeping)
{
ClearForces();
Ode.dBodyDisable(bodyID);
LinearVelocity = Vec3.Zero;
AngularVelocity = Vec3.Zero;
}
else
{
Ode.dBodyEnable(bodyID);
}
}
}