// This function easily wins when run under a profiler - gprof puts it
// at around 17%. Almost all of this is this function - not any
// (profiled) function it calls.
public void get_lift_and_drag(FlightModel flightModel,
Vec3 wind_rel_in, // in
out Vec3 linear_lift, // lift force
out Vec3 linear_drag, // drag force
out Vec3 linear_position, // loc of force
out Vec3 pitch_force,
out Vec3 pitch_position,
float density,
int num,
float aileron, float elevator, float rudder)
{
float CL, CD, lift_force, drag_force;
SwitchVec(ref wind_rel_in);
debugWind = wind_rel_in;
if (Utils.LogOn)
{
int abc = 123;
abc++;
}
Quat Qalpha_beta = Mat3.FromRotateByZ(MathFunctions.DegToRad(rotation)).ToQuat() *
Mat3.FromRotateByX(MathFunctions.DegToRad(-inc)).ToQuat();
Mat3 alpha_beta = Qalpha_beta.ToMat3();
Mat3 alpha_beta_inv;
Quat temp = Qalpha_beta;
temp.Inverse();
temp.ToMat3(out alpha_beta_inv);
Vec3 wind_rel = alpha_beta * wind_rel_in;
float alph = MathFunctions.RadToDeg((float)Math.Atan2(wind_rel.Y, -wind_rel.Z)); // angle of inclination
float speed2 = wind_rel.Y * wind_rel.Y + wind_rel.Z * wind_rel.Z;
float speed = MathFunctions.Sqrt(speed2);
if (alph > 180)
alph -= 360;
else if (alph < -180)
alph += 360;
FlyingMode flying;
float flying_float;
// calculate and store the control input (might be used in the
// display)
float control_input =
control_per_chan_1 * aileron +
control_per_chan_2 * elevator +
control_per_chan_3 * rudder;
debugControl = control_input;
CL = get_CL(alph, control_input, out flying, out flying_float);
CD = get_CD(alph, control_input, flying, CL);
float force_scale = get_area * 0.5f * speed2;
lift_force = force_scale * CL; // main variation in density is air/water
drag_force = density * force_scale * CD;
float flying_offset;
if (flying_float > 0)
{
flying_offset = x_offset[(int)FlyingMode.STALLED] +
flying_float * (x_offset[(int)FlyingMode.FORWARD] -
x_offset[(int)FlyingMode.STALLED]);
}
else
{
flying_offset = x_offset[(int)FlyingMode.STALLED] +
-flying_float * (x_offset[(int)FlyingMode.REVERSE] -
x_offset[(int)FlyingMode.STALLED]);
}
// we need cos(alph) etc. Rather than using trig, we can calculate the
// value directly...
// const float cos_deg_alph = cos_deg(alph);
// const float sin_deg_alph = sin_deg(alph);
bool doit = (Math.Abs(speed) > 0.01);
float cos_deg_alph = doit ? -wind_rel.Z / speed : 0.0f;
float sin_deg_alph = doit ? wind_rel.Y / speed : 0.0f; ;
float force_up = lift_force * cos_deg_alph + drag_force * sin_deg_alph;
float force_forward = lift_force * sin_deg_alph - drag_force * cos_deg_alph;
linear_lift = new Vec3(0, lift_force, 0);
linear_drag = new Vec3(0, 0, -drag_force);
linear_position = position + new Vec3(0, flying_offset, 0);
debugLinearPosition = flying_offset;
// calculate the pitching moment - represented by a single
// force at the trailing edge
float pitching_force = get_area * 0.5f * wind_rel.Z * Math.Abs(wind_rel.Z) *
(CM_0 + CM_per_deg * control_input);
pitch_force = new Vec3(0.0f, pitching_force, 0.0f);
pitch_position = position;
pitch_position.Y -= 0.5f * chord;
debugPitchPosition = pitch_position.Y;
// need to rotate the force back...
linear_lift = alpha_beta_inv * linear_lift;
linear_drag = alpha_beta_inv * linear_drag;
pitch_force = alpha_beta_inv * pitch_force;
linear_lift /= 500;
linear_drag /= 500;
pitch_force /= 500;
linear_position /= 4;
pitch_position /= 4;
linear_lift *= flightModel.param.Mult_Lift;
linear_drag *= flightModel.param.Mult_Drag;
SwitchVec(ref linear_lift);
SwitchVec(ref linear_drag);
SwitchVec(ref pitch_force);
debugLift = linear_lift;
debugDrag = linear_drag;
debugPitchForce = pitch_force;
}
public void CreateGeometry(MapObject parent, FlightModel flightModel)
{
MapObjectAttachedMesh obj = new MapObjectAttachedMesh();
obj.MeshName = "Models\\DefaultBox\\DefaultBox.mesh";
float mScale = .2f;
obj.ScaleOffset = new Vec3(span * mScale, chord * mScale, .1f * mScale) * flightModel.scale;
obj.RotationOffset =
Mat3.FromRotateByY(MathFunctions.DegToRad(-rotation)).ToQuat() *
Mat3.FromRotateByX(MathFunctions.DegToRad(+inc)).ToQuat();
obj.PositionOffset = (position + Vec3.YAxis * flightModel.param.geometry_offset_for_wheels) * flightModel.scale;
parent.Attach(obj);
}