Mostly working center of mass calculation