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;
}
public void InitStopped()
{
Qeng = Omega = dOmega = 0;
load = 0;
RotSpeed = 0;
phase = Phase.CUTOFF;
throttle = 0;
Solver = new RK4Solver(ODEFunction, 0, Vector <double> .Build.DenseOfArray(new double[] { 0, 0, 0 }));
initialized = true;
}
public void Init(double Q0)
{
Qeng = load = Q0;
Omega = Omega0;
dOmega = 0;
RotSpeed = Omega;
phase = Phase.RUN;
throttle = 1.0;
Solver = new RK4Solver(ODEFunction, 0, Vector <double> .Build.DenseOfArray(new double[] { Q0, 0, 0 }));
initialized = true;
}