private static void RotateByAirFriction(Quaternion currentOrientation, Vector3 airVelocity, float dt,
out Quaternion newOrientation)
{
float headingAngle = VectorHelper.GetHeadingAngle(currentOrientation);
float airflowHeadingAngle = VectorHelper.GetHeadingAngle(-airVelocity);
float relativeAirflowAngle = airflowHeadingAngle - headingAngle;
// Note: Simplificiation, we assume helicopter is nearly level.
// This way any vertical wind speeds won't cause the helicopter to rotate.
float horizontalWindMagnitudeFactor = VectorHelper.ToHorizontal3D(airVelocity).Length();
// Wind angle causes max rotation when wind is perfectly to the Right or Left of helicopter
// since the arm of the tail rotor is the longest then, as seen from the wind.
// As the wind approaches the frontside or tailside, the effect decreases.
var windAngleFactor = (float)Math.Abs(Math.Sin(relativeAirflowAngle));
// float windFactor = horizontalWindMagnitudeFactor*windAngleFactor;
// The yaw rate [rad/s] per unit of wind velocity [m/s]
const float yawRatePerVelocity = 0.1f;
float yawRate = windAngleFactor * horizontalWindMagnitudeFactor * yawRatePerVelocity;
// TODO temp //0.1745f; // ~10 degrees per second
float deltaYaw = yawRate * dt;
if (relativeAirflowAngle < 0)
{
deltaYaw = -deltaYaw;
}
newOrientation = VectorHelper.AddPitchRollYaw(currentOrientation, 0, 0, deltaYaw);
}
/// <summary>
/// Rotate helicopter entirely by joystick input (major simplification of physics).
/// </summary>
/// <param name="currentOrientation"></param>
/// <param name="dt"></param>
/// <param name="output"></param>
/// <param name="newOrientation"></param>
private static void RotateByJoystickInput(Quaternion currentOrientation, float dt, JoystickOutput output,
out Quaternion newOrientation)
{
// Move helicopter from previous state according to user/autopilot input
// Change roll, pitch and yaw according to input
// Invert yaw, so positive is clockwise
float deltaRoll = output.Roll * PhysicsConstants.MaxRollRate * dt;
float deltaPitch = output.Pitch * PhysicsConstants.MaxPitchRate * dt;
float deltaYaw = output.Yaw * PhysicsConstants.MaxYawRate * dt;
// Add deltas of pitch, roll and yaw to the rotation matrix
newOrientation = VectorHelper.AddPitchRollYaw(currentOrientation, deltaPitch, deltaRoll, deltaYaw);
// Get axis vectors for the new orientation
// newAxes = VectorHelper.GetAxes(newOrientation);
}
public override void Update(PhysicalHeliState startState, PhysicalHeliState endState, JoystickOutput output, TimeSpan startTime, TimeSpan endTime)
{
_accelerometer.Update(startState, endState, output, startTime, endTime);
_gyroscope.Update(startState, endState, output, startTime, endTime);
// Make sure we use orientation at start of timestep to transform to world, since IMU should be able to calculate accumulated position
// by this world acceleration, and according to my physics simulation the position is calculated before rotation is.
AccelerationWorld = _accelerometer.AccelerationWorld;
AccelerationLocal = new ForwardRightUp(_accelerometer.Forward, _accelerometer.Right, _accelerometer.Up);
AngularRateBody = AngularValues.FromRadians(_gyroscope.Rate);
AngularDeltaBody = AngularValues.FromRadians(_gyroscope.Delta);
PitchRollYaw delta = AngularDeltaBody.Radians;
AccumulatedOrientation =
VectorHelper.AddPitchRollYaw(AccumulatedOrientation, delta.Pitch, delta.Roll, delta.Yaw);
}
public void Test()
{
JoystickOutput output;
output.Pitch = 0.312312432f;
output.Roll = 0.512312432f;
output.Yaw = 0.912312432f;
const float dt = 0.017001f;
float pitchRate = output.Pitch * PhysicsConstants.MaxPitchRate;
float rollRate = output.Roll * PhysicsConstants.MaxRollRate;
float yawRate = output.Yaw * PhysicsConstants.MaxYawRate;
Quaternion orient1 = Quaternion.Identity;
Quaternion orient2 = Quaternion.Identity;
for (int i = 0; i < 10000; i++)
{
float deltaPitch = (output.Pitch * PhysicsConstants.MaxPitchRate) * dt;
float deltaRoll = (output.Roll * PhysicsConstants.MaxRollRate) * dt;
float deltaYaw = (output.Yaw * PhysicsConstants.MaxYawRate) * dt;
// Add deltas of pitch, roll and yaw to the rotation matrix
orient1 = VectorHelper.AddPitchRollYaw(orient1, deltaPitch, deltaRoll, deltaYaw);
deltaPitch = pitchRate * dt;
deltaRoll = rollRate * dt;
deltaYaw = yawRate * dt;
orient2 = VectorHelper.AddPitchRollYaw(orient2, deltaPitch, deltaRoll, deltaYaw);
}
Assert.AreEqual(orient1.X, orient2.X, "X");
Assert.AreEqual(orient1.Y, orient2.Y, "Y");
Assert.AreEqual(orient1.Z, orient2.Z, "Z");
Assert.AreEqual(orient1.W, orient2.W, "W");
}
