Add direct Vincenty calculation with tests