Use augmented mass matrix and inverse of augmentd mass matrix in ConstraintDynamics.
- We apply joint daming and stiff forces in implicit way. This modifies the original mass matrix from M to (M + dt * D + dt * dt * K) where D is diagonal damping matrix, K is diagonal stiffness matrix, and dt is the simulation time step.
- The user also can get both of the original mass matrix and augmented mass matrix (inverse of the matrices too)