Planning on making moveSpatialMassMatrix a static method inside RigidBodyAddedMass, rather than a separate function. Moving the test function to the appropriate place