private void UpdateRigidBodyParameters(RigidBody o, float dt)
{
RigidBodyData data = o.RigidBodyData;
if (data.Mass != 0)
{
data.Acceleration = data.Forces / data.Mass;
}
else
{
data.Acceleration = new MyVector(0, 0, 0);
}
data.Velocity.Add(data.Acceleration * dt);
data.AngularAcceleration = data.InertiaInverse * (data.Moments -
(data.AngularVelocity ^ (data.Inertia * data.AngularVelocity)));
data.AngularVelocity += data.AngularAcceleration * dt;
//data.Orientation += data.Orientation * (data.AngularVelocity * (dt / 2));
//data.Orientation.Normalize();
MyMatrix rot = data.Orientation.RotationMatrix;
MyMatrix rotinv = rot.Inverse;
data.InertiaInverseGlobal = rot * data.InertiaInverse * rotinv;
data.VelocityBody = data.Velocity;
data.VelocityBody.Rotate(~data.Orientation);
data.Forces = new MyVector(0, 0, 0);
data.Moments = new MyVector(0, 0, 0);
o.RigidBodyData = data;
}
private void CalculateRigidBodyForces(RigidBody o)
{
RigidBodyData data = o.RigidBodyData;
//data.Forces = new MyVector(0, 0, 0);
//data.Moments = new MyVector(0, 0, 0);
if (doGravity)
data.Forces = o.Mass * gravity;
if (doLinearDrag)
{
data.Forces -= data.Velocity * air_density * (data.Velocity.Length * o.LinearDragCoefficient *
o.BoundingSphereRadius * o.BoundingSphereRadius);
}
if (doAngularDrag)
{
data.Moments -= data.AngularVelocity * air_density * (data.AngularVelocity.Length * o.AngularDragCoefficient *
o.BoundingSphereRadius * o.BoundingSphereRadius);
data.Moments -= data.AngularVelocity * air_density * (o.AngularDragCoefficient *
o.BoundingSphereRadius * o.BoundingSphereRadius);
//constantDrag
if (data.AngularVelocity.Length > 0)
{
data.Moments -= data.AngularVelocity * air_density * (data.AngularVelocity.Length * o.AngularDragCoefficient *
o.BoundingSphereRadius * o.BoundingSphereRadius) / (float)Math.Sqrt(data.AngularVelocity.Length);
}
if (data.AngularVelocity.LengthSq < 0.00001f)
data.AngularVelocity.X = data.AngularVelocity.Y = data.AngularVelocity.Z = 0;
}
o.RigidBodyData = data;
}
