Added Waypoint Functions

Began to add Steve's initial waypoint functions - distance, bearing, and
the functions needed to assist inn calculating them.