Moved distance computation to CollisionGeometry