Move more of the distance calculation to c++
Add cpp source file, should be moved to own project