Added distance function, add centre of mass vector function