Fixing bugs in sixDoFRigidBodyVelocity