Add tests for center of mass calculation