private TimestepResult ByPhysics(TimestepStartingCondition startCondition, JoystickOutput output,
TimeSpan stepEndTime, float dt)
{
Vector3 newPosition;
Vector3 newVelocity;
ClassicalMechanics.ConstantAcceleration(
dt,
startCondition.Position,
startCondition.Velocity,
startCondition.Acceleration,
out newPosition, out newVelocity);
Quaternion newOrientation, afterJoystick;
RotateByJoystickInput(startCondition.Orientation, dt, output, out afterJoystick);
newOrientation = afterJoystick;
return(new TimestepResult(newPosition, newVelocity, newOrientation, stepEndTime));
}
private TimestepResult SimulateStep(TimestepStartingCondition startCondition, JoystickOutput output,
TimeSpan stepDuration, TimeSpan stepEndTime)
{
// Find new position and velocity from a constant acceleration over timestep
var dt = (float)stepDuration.TotalSeconds;
// TimestepResult result;
if (_useTerrainCollision)
{
// Note we override gravity here because the gravity acceleration is already accounted for in startCondition.Acceleration vector
_collision.SetGravity(Vector3.Zero);
float heliMass = _collision.HelicopterBody.Mass;
_collision.HelicopterBody.Position = Conversion.ToJitterVector(startCondition.Position);
_collision.HelicopterBody.LinearVelocity = Conversion.ToJitterVector(startCondition.Velocity);
_collision.HelicopterBody.AddForce(Conversion.ToJitterVector(heliMass * startCondition.Acceleration));
var localAngVelocity = new Vector3(
output.Pitch * PhysicsConstants.MaxPitchRate,
output.Yaw * PhysicsConstants.MaxYawRate,
-output.Roll * PhysicsConstants.MaxRollRate);
var worldAngVelocity = VectorHelper.MapFromWorld(localAngVelocity, startCondition.Orientation);
_collision.HelicopterBody.AngularVelocity = Conversion.ToJitterVector(worldAngVelocity);
// Simulate physics
// Vector3 preForward = Vector3.Transform(Vector3.Forward, startCondition.Orientation);
_collision.World.Step(dt, false);
// TODO Testing with Jitter Physics
return(new TimestepResult(
Conversion.ToXNAVector(_collision.HelicopterBody.Position),
Conversion.ToXNAVector(_collision.HelicopterBody.LinearVelocity),
Quaternion.CreateFromRotationMatrix(Conversion.ToXNAMatrix(_collision.HelicopterBody.Orientation)),
stepEndTime));
// Vector3 postForward = Vector3.Transform(Vector3.Forward, result.Orientation);
}
else
{
Vector3 newPosition;
Vector3 newVelocity;
ClassicalMechanics.ConstantAcceleration(
dt,
startCondition.Position,
startCondition.Velocity,
startCondition.Acceleration,
out newPosition, out newVelocity);
// TODO Add wind
// Rotate helicopter by joystick input after moving it
// Vector3 airVelocity = -startCondition.Velocity + Vector3.Zero;
// TODO Re-apply rotation by air friction as soon as the Kalman Filter works fine without it
Quaternion newOrientation, afterJoystick;
RotateByJoystickInput(startCondition.Orientation, dt, output, out afterJoystick);
// RotateByAirFriction(afterJoystick, airVelocity, dt, out newOrientation);
newOrientation = afterJoystick;
return(new TimestepResult(newPosition, newVelocity, newOrientation, stepEndTime));
}
}
