Added scaling to mass and moment mass calculations. Tweaked the physics a bit and tried to debug them.