Add CompressedJacobianMatrix and modified rigid body simulation algorithm