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; }