/// <summary>
/// Calculate the torques along the different axis of the airframe.
/// </summary>
/// <param name="elapsedTime"></param>
private void CalculateTorques(float elapsedTime, Vector3 wind)
{
if (ApiModel.CalculateTorques(elapsedTime, wind))
{
return;
}
// Torque = F * r * sin(alpha)
// TODO differential lift
float airspeedX = ApiModel.Vx + wind.X;
float airspeedY = ApiModel.Vy + wind.Y;
float airspeedZ = ApiModel.Vz + wind.Z;
float airspeedXY = (float)Math.Sqrt(airspeedX * airspeedX + airspeedY * airspeedY);
float airspeedXZ = (float)Math.Sqrt(airspeedX * airspeedX + airspeedZ * airspeedZ);
ApiModel.Tx = (float)((-ApiAircraftParameters.RollDamping * Math.Sign(airspeedX) * ApiModel.Wx +
ApiAircraftParameters.AileronEfficiency * airspeedX * ApiModel.Ailerons +
ApiAircraftParameters.DihedralAngle * ApiAircraftParameters.DihedralEfficiency * airspeedX * ApiModel.Beta) * airspeedX +
(Lift * ApiAircraftParameters.SpinFactor * ApiModel.Wz) / (Math.Abs(airspeedX) + 1.0f) +
ApiAircraftParameters.AileronEfficiency * ApiAircraftParameters.PropWashAilerons * ApiModel.Throttle * ApiModel.Ailerons);
ApiModel.Ty = (float)((-ApiAircraftParameters.PitchDamping * ApiModel.Wy -
Math.Sin(ApiModel.Alpha) * ApiAircraftParameters.PitchStability +
ApiAircraftParameters.ElevatorEfficiency * airspeedX * Elevator + ApiAircraftParameters.PitchTrim) * airspeedXY +
ApiAircraftParameters.ElevatorEfficiency * ApiAircraftParameters.PropWashElevator * ApiModel.Throttle * ApiModel.Elevator);
ApiModel.Tz = (float)((-ApiAircraftParameters.YawDamping * ApiModel.Wz -
Math.Sin(ApiModel.Beta) * ApiAircraftParameters.YawStability +
ApiAircraftParameters.RudderEfficiency * airspeedX * ApiModel.Rudder) * airspeedXZ +
ApiAircraftParameters.RudderEfficiency * ApiAircraftParameters.PropWashRudder * ApiModel.Throttle * ApiModel.Rudder);
}
