Improved center of mass computation algorithm