Przemieszczanie się robota w układzie współrzędnych

Witam!
Piszę aplikację sterującą robotem. Dla ułatwienia(żeby nie głowić się nad znakiem) punkt zerowy jest w połowie zakresu zmiennej typu long. Współrzędna w której jest robot zapisywana jest do pliku. Więc tak; z punktu zerowego wyznaczam sobie 2 wektory, jeden wzdłuż osi y drugi to kierunek w którym mam się zwrócić, kąt i dystans liczy mi się dobrze i z pierwszego punktu. Potem jak chcę go przemieścić w inne miejsce z tego punktu zaczynają się schody.** Pytanie jak z dowolnego punktu na osi x y znając ostatnią współrzędną i kąt podjąć decyzję w którą strone robot ma się obrócić.** Z kolejnego punktu wezmę ostatni wektor i obliczę nowy, przynajmniej tak ja to widzę. Wątek zakładam ponieważ nie znalazłem za bardzo żadnego pomocnego algorytmu do poruszania się robotem w układzie współrzędnych

tzn gdzie jest problem? potrafisz wyliczyć kąt i odległość z punktu 0,0 a nie potrafisz dajmy na to z punktu 3,7 ?

Nie wiem na podstawie czego podjąć decyzje w którą stronę się obrócić będąc w innym punkcie.

przenieś sobie środek układu współrzędnych do nowego punktu i zrób tak samo....