AStar not even working all that well for 2-d with LQR style cost. Not really AStar, just Djikstra's. I'll try doing the 'optimal' version now