public void TeleportToLocation(NavBeacon loc)
 {
     if (loc != null)
     {
         Debug.Log("Teleporting to: " + loc.Name);
         navRigidbody.position = loc.GetOrbitPosition();
         currentLocation       = loc;
         destination           = null;
     }
     else
     {
         Debug.Log("ERROR: Teleport location set to null");
     }
 }
 public void Move(NavBeacon loc)
 {
     if (destination == null)
     {
         if (loc != currentLocation)
         {
             destination         = loc;
             currentLocation     = null;
             initialPosition     = navRigidbody.position;
             destinationPosition = destination.GetOrbitPosition();
             CalculateTrajectory();
             StartStateMachine();
         }
         else
         {
             Debug.Log("ERROR: Cannot move to current location.");
         }
     }
     else
     {
         Debug.Log("ERROR: Movement command sent when destination already set.");
     }
 }