Translated the basic trilateration algorithm to C++