finished my work on c-space distance calculation