Fix centre of mass calc