This week, I finished implementing the entire path planning algorithm. It takes in two inputs, an origin and a list of nodes the cart needs to visit; and it returns the optimal route.

As promised, the algorithm pre-computes the shortest distance between each path of nodes in the map using A* with the heuristic function being the euclidean distance between the two nodes. The shortest paths between each path of nodes are stored in data structure to be used in later stage of the algorithm.

While implementing the second stage of the algorithm, I encountered then solved a bug within the original algorithm.

Originally as stated in the last report, I had: “Let’s assume at some point my current route becomes A->C->B->A->D->A. Then for all node_i left that I still need to visit, I figure out which of the following is the smallest: 1. A->i + i->C; 2. C->i + i->B; 3. B->i + i->A; 4. A->i + i->D; 5. A->i + i->A; 6. B->i + i->B; 7. C->i + i->C; 8. D->i + i->D.” This is not entirely correct. I should really be finding the smallest among: 1. A->i + i->C – A->C; 2. C->i + i->B – B->C; 3. B->i + i->A – B->A; 4. A->i + i->D – A->D; 5. A->i + i->A; 6. B->i + i->B; 7. C->i + i->C; 8. D->i + i->D. This is because if I insert i between A and C, I no longer need to incur the cost of going from A->C.

After fixing the bug mentioned above, I implemented the rest of the algorithm exactly as stated in the last report.

I also did extensive testing on the algorithm for correctness. For multiple times, the algorithm finds the true shortest path that I missed by finding the shortest path by hand.

Below is an image generated to help with the mapping