/// <summary>
///
/// </summary>
/// <param name="timeStep"></param>
/// <param name="damping"></param>
public void Update(double timeStep, double damping)
{
var r = Current.ToMatrix();
var v = Velocity * (1.0 - damping) + r.Apply(_inertiaInv * r.ApplyTranspose(_torqueSum)) * timeStep;
if (_weightSum > 0.0)
{
v += r.Apply(r.ApplyTranspose(_deltaSum) * _inertiaInv) * (timeStep / _weightSum);
}
Current = new Quaterniond(v * timeStep) * Current;
Velocity = v;
Clear();
}
