added a new check in does node exist based on only euclidean distance
properties, used when traveling between two known nodes.
changed heading calculations to range [-PI, PI]
Updated linking function to also link neighbors to the new node.