public void Update(double dt)
{
ForceModel.Update(dt);
v = P / M;
w = Iinv * L;
F = R * ForceModel.Force;
T = R * ForceModel.Torque;
if (Solver == null)
{
Solver = new RK4Solver(TimeDerivatives, 0, State());
}
Solver.Step(dt);
ApplyState(Solver.State);
R = q.ToRotationMatrix();
var Rinv = R.Inverse();
ForceModel.Translation = x;
ForceModel.Rotation = R;
ForceModel.Velocity = Rinv * v;
ForceModel.AngularVelocity = Rinv * w;
}
protected override void updateLocalVelocity(ForceModel model)
{
if (model == mainRotor)
{
base.updateLocalVelocity(model);
return;
}
double dMR = (model.Translation - mainRotor.Translation).Norm(2);
Vector <double> washvel = mainRotor.Rotation * mainRotor.GetDownwashVelocity(dMR);
if (washvel.Norm(2) <= 0.01)
{
base.updateLocalVelocity(model);
return;
}
// Ramp downwash velocity to zero from distance to center 0.9 to 1.1, to avoid too abrupt changes
double dWashCenter = washvel.Cross(mainRotor.Translation - model.Translation).Norm(2) / washvel.Norm(2);
double Rdw = mainRotor.GetDownwashRadius(dMR);
double ndwc = dWashCenter / Rdw;
if (ndwc > 1.1)
{
washvel = Vector <double> .Build.Zero3();
}
else if (ndwc > 0.9)
{
washvel *= 1.0 - (ndwc - 0.9) / (1.1 - 0.9);
}
// Compensate for wake acceleration (see Dreier figure 12.7) - this is really improvised
// i.e. tilt wash vector downward further from main rotor
double w_w0 = washvel.Norm(2) / mainRotor.GetDownwashVelocity(0).Norm(2) - 1.0;
double skew = Math.Atan2(washvel.x(), -washvel.z());
Matrix <double> R = Matrix <double> .Build.RotationY(skew *w_w0);
washvel = R * washvel;
model.AngularVelocity = model.InvRotation * AngularVelocity;
model.Velocity = model.InvRotation * (Velocity + washvel - model.Translation.Cross(AngularVelocity));
}
virtual protected void updateLocalVelocity(ForceModel model)
{
model.AngularVelocity = model.InvRotation * AngularVelocity;
model.Velocity = model.InvRotation * (Velocity - model.Translation.Cross(AngularVelocity));
}
public void SetModel(string name, ForceModel model)
{
SubModels[name] = model;
}