Mathmatical!  better insertPathPoint algorithm