Fix computation of M, InvM, AugM, InvAugM
- add additional intermediate variables to SoftBodyNode and PointMassthem
- add unit test for these computation