Error in application of parallel axis theorem in sixDoFRigidBodyMotion class
There is a bug in the application of the parallel axis theorem in the constructor here:
where we do momentOfInertia_ += mass_*diag(I*magSqr(R) - sqr(R));
The matrix sqr(R)
is generally not diagonal and by taking diag() of it, we omit its off-diagonal terms, which is an error.
Thus, simulations where the centre of rotation and centre of mass do not coincide must be expected to give wrong results.
The correct behaviour in such simulations should be tested and verified with a simple test case.