calculates distance in C++