public virtual void Update(GameTime gameTime)
{
float dT = (float)gameTime.ElapsedGameTime.TotalSeconds;
Position += Velocity * dT;
float l = AngularVelocity.Length();
if (l > 0.001f)
{
Quaternion deltaRot = new Quaternion();
Vector3 theta = AngularVelocity * dT;
float thetaMagSq = theta.LengthSquared();
float s;
if (thetaMagSq * thetaMagSq / 24.0f < 1e-6f)
{
deltaRot.W = 1.0f - thetaMagSq / 2.0f;
s = 1.0f - thetaMagSq / 6.0f;
}
else
{
float thetaMag = (float)Math.Sqrt(thetaMagSq);
deltaRot.W = (float)Math.Cos(thetaMag);
s = (float)Math.Sin(thetaMag) / thetaMag;
}
deltaRot.X = theta.X * s;
deltaRot.Y = theta.Y * s;
deltaRot.Z = theta.Z * s;
Rotation = deltaRot * Rotation;
}
}
public void SimulateAir(Controller input, float dt)
{
const float M = 180.0f; // mass
const float J = 10.5f; // moment of inertia
const float v_max = 2300.0f;
const float w_max = 5.5f;
const float boost_force = 178500.0f;
const float throttle_force = 12000.0f;
Vector3 g = new Vector3(0.0f, 0.0f, -651.47f);
Vector3 rpy = new Vector3(input.Roll, input.Pitch, input.Yaw);
// air control torque coefficients
Vector3 T = new Vector3(-400.0f, -130.0f, 95.0f);
// air damping torque coefficients
Vector3 H = new Vector3(
-50.0f, -30.0f * (1.0f - Math.Abs(input.Pitch)),
-20.0f * (1.0f - Math.Abs(input.Yaw)));
float thrust = 0.0f;
if (input.Boost && Boost > 0)
{
thrust = (boost_force + throttle_force);
Boost--;
}
else
{
thrust = input.Throttle * throttle_force;
}
Velocity += (g + (thrust / M) * Forward) * dt;
Position += Velocity * dt;
Vector3 w_local = Vector3.Transform(AngularVelocity, Quaternion.Inverse(Rotation));
Vector3 old_w = AngularVelocity;
AngularVelocity += Vector3.Transform(T * rpy + H * w_local, Rotation) * (dt / J);
Vector3 angleAxis = 0.5f * (AngularVelocity + old_w) * dt;
Quaternion R = Quaternion.CreateFromAxisAngle(Vector3.Normalize(angleAxis), angleAxis.Length());
Rotation = Quaternion.Multiply(R, Rotation);
// if the velocities exceed their maximum values, scale them back
Velocity /= Math.Max(1.0f, Velocity.Length() / v_max);
AngularVelocity /= Math.Max(1.0f, AngularVelocity.Length() / w_max);
}
public MyVector3 GetMomentumAtPoint(MyVector3 point)
{
MyVector3 momentum = new MyVector3(); //Create momentum
if (AngularVelocity.Length() > 0)
{
MyVector3 pointVelocity = Velocity + VectorMaths.VectorCrossProduct(AngularVelocity, Transformation.Translation - point); //Set point velocity to be velocity + the cross product of the angular velocity and translation - point
momentum = Mass * pointVelocity; //Set momentum to be mass * point velocity
}
else
{
momentum = Mass * Velocity; //set momentum to be mass * velcity
}
return(momentum); //Return momentum
}