Dijkstra's(?) Algorithm help

[Sparkyshires]

A little bit of backstory - I got an internship at an engineering company to help design an automatic ground vehicle that would follow magnetic tape around a building. I'm designing the software, and obviously a challenge I had to tackle was how to find the shortest route from point 1 to point 7 or whatever.

Because of this challenge, I wrote a function to find a shortest route given a map of line segments, as opposed to a grid as this is most often done in. I pretty much check to find all line segments connected to a start point, and then check to see if the other end of the line segment is the end point. If it's not, then rerun the program with the endpoints of the previous line segment as the new start points, and so on and so forth. As soon as you have a path from the initial start point to the end, compare the length to a global variable. If the path you found is shorter, replace it. This then finds all possible routes between your initial start point and your endpoint, and gives you the shortest.

HOWEVER, just about when i was done (like five lines left and some debugging) I found a thing called Dijkstras algorithm. I have tried reading about it over and over, but I cannot seem to grasp it does anyone here have knowledge enough about it to explain it in slightly layman terms for me to understand?

My curiosity right now is completely academic, as the speed at which its running (on a raspberry pi) and the size of the map (~30) it will likely be dealing with, means it can pre pocess every possible route in ~2.5 seconds. I'm simply interested for how I could have done better.

EDIT: I'm at school right now, and the firewall blocks github :/ but as soon as I get home I'll post the code. If you're interested in checking it right now, it's at http://github.com/infinity42/pathFinder

EDIT(2): Nevermind, I had it in an old e-mail! Also, if you check github it's in a bad state because I'm changing it from c to c++. We're doing the project in c but I prefer sandboxing in c++. here is my code:

#include <iostream>
#include <fstream>
#include <cstdlib>
#include <string>
#define LINE_SEG 21
#define VERTEX 20
#define MX_LGTH_MP 1023
using namespace std;

int pDist = MX_LGTH_MP;
int pStep = 0;
int shortestPath[LINE_SEG] = {0};

struct location {
	bool type; // true will be point, false is line
	int location;
} currentLoc;

struct Route {
	int initStart;		//the initial start location.  good for diagnostics
	int endLoc;		//the all holy end location.  where you are going
	int startLoc;		//the current "start" location.  used for which point you are considering yourself at
	int path[1023];		//the path so far
	int map[LINE_SEG][3];	//the map data pulled from map.csv
	int pSum;		//the total distance of this path so far
	int step;		//which step you are on now.  Useful for attaining final path without non-zero check in whitespace
	int pvPoint;		//the last point visited.  useful for just not going backwards and being dumb.  like you
};

int pathFind(Route route)
{
	bool notVisited = true;
	for(int i = 0; i < LINE_SEG; i++)
	{
		if(route.map[i][0] == route.startLoc && route.map[i][1] != route.pvPoint)
		{
			for(int k = 0; k < route.step; k++)
			{
				if(route.path[k] == i)
				{
					notVisited = false;
				}
			}
			if(notVisited)
			{
				if(route.map[i][1] == route.endLoc)
				{
					route.path[route.step] = i;
					route.step++;
					route.pSum = route.pSum + route.map[i][2];
					if(route.pSum < pDist)
					{
						pDist = route.pSum;
						pStep = route.step;
						for(int s = 0; s < route.step; s++)
						{
							shortestPath[s] = route.path[s];
						}
					}
					return route.step;
				}
				else
				{
					if (true)
					{
						Route newRoute = route;
						newRoute.path[newRoute.step] = i;
						newRoute.step++;
						newRoute.pSum = newRoute.pSum + newRoute.map[i][2];
						newRoute.pvPoint = newRoute.map[i][0];
						newRoute.startLoc = route.map[i][1];
						pathFind(newRoute);
					}
				}
			}
		}
		if(route.map[i][1] == route.startLoc && route.map[i][0] != route.pvPoint)
		{
			for(int k = 0; k < route.step; k++)
			{
				if(route.path[k] == i)
				{
					notVisited = false;
				}
			}
			if(notVisited)
			{
				if(route.map[i][0] == route.endLoc)
				{
					route.path[route.step] = i;
					route.step++;
					route.pSum = route.pSum + route.map[i][2];
					if(route.pSum < pDist)
					{
						pDist = route.pSum;
						pStep = route.step;
						for(int s = 0; s < route.step; s++)
						{
							shortestPath[s] = route.path[s];
						}
					}
					return route.step;
				}
				else
				{
					if (true)
					{
						Route newRoute = route;
						newRoute.path[newRoute.step] = i;
						newRoute.step++;
						newRoute.pSum = newRoute.pSum + newRoute.map[i][2];
						newRoute.pvPoint = newRoute.map[i][1];
						newRoute.startLoc = newRoute.map[i][0];
						pathFind(newRoute);
					}
				}
			}
		}
	}
}

Route initRoute(int startLoc, int endLoc) {
	Route blankRoute;
	blankRoute.initStart = startLoc;
	blankRoute.startLoc = startLoc;
	blankRoute.endLoc = endLoc;
	blankRoute.pSum = 0;
	blankRoute.step = 0;
	blankRoute.pvPoint = 99;
	for(int i = 0; i<LINE_SEG; i++)
		{
			blankRoute.path[i] = 0;
		}
	int a;
	const char* dataPointer;
	string data;
	ifstream map ("map.csv");
	//reads out the two input number
	for(int i = 0; i < LINE_SEG; i++)
	{
		for(int n = 0; n < 2; n++)
		{
			getline(map, data, ',');
			dataPointer = data.c_str();
			a = atoi(dataPointer);
			blankRoute.map[i][n] = a;
		}
		getline(map, data, '\n');
		dataPointer = data.c_str();
		a = atoi(dataPointer);
		blankRoute.map[i][2] = a;
	}
	return blankRoute;
}

void findFastestPath(Route route) {
	pathFind(route);
	cout << route.initStart << ',' << route.endLoc << ',' << pDist << ',' << pStep << ',';
	for(int i = 0; i < pStep; i++)
	{
		cout << shortestPath[i] << ' ';
	}
	cout << endl;
	pDist = MX_LGTH_MP;
	pStep = 0;
	for(int i = 0; i < LINE_SEG; i++)
		{
			shortestPath[i] = 0;
		}
}

/*
 *Accepts two arguments, a start location and an end location
 * runs off of an insanely basic map at the moment.
 */
int main(int argc, char* argv[]) {
	//each element of the argv array after zero is a space delimited
	//input.  this allows/forces you to input two integers and then have
	//them stored.  pretty cool amirite?
	int initStart = atoi(argv[1]);
	int endLoc = atoi(argv[2]);
	Route route = initRoute(initStart, endLoc);
	findFastestPath(route);
	for(int stt = 1; stt < VERTEX+1; stt++)
		{
			for(int end = 1; end < VERTEX+1; end++)
				{
					route = initRoute(stt, end);
					findFastestPath(route);
				}
		}
	return 0;
}

[Ty Tremblay]

NOTE: CD prevents you from adding more than 5 images, so you'll see links after I hit the limit.


Imagine a 5x5 grid with a starting position S and a finish position F:


Next, imagine yourself standing at S and looking at the squares you can travel to in one jump. For the purposes of this exercise, we'll say you can only jump in cardinal directions one space. Mark the spaces you can make it to with a distance. For this first step, there are only two squares we can reach and they both have a distance of 1:


Now, mark your starting location (S) as visited and don't let your algorithm go back to S again. Next, choose the square that has the lowest distance value that also hasn't been visited (since both of the squares have a distance of 1, we'll just choose the first one). We'll use green as the square that we're currently working with:


Assign distance values from your current square. All of our jumps from current square to next square will have a distance of 1 in this exercise. Make sure you remember that you've already "traveled" a distance of 1, so add that value (also known as your current square's distance) to the distances you're assigning:


Mark your current square as visited. Then, go back through the list of squares you haven't visited and pick the one with the lowest distance value.


Repeat the distance process but with one difference. If you come to a square that already has a distance value, only change that value if the distance value you've calculated from your current square is smaller. Typical implementations of this algorithm set all distances to infinity, knowing that they'll be overwritten as the algorithm plays out:
http://i.imgur.com/WoRvqYM.png

Keep moving to the closest unvisited square, marking distances, and repeating and your graph will look like it does below. Note that you can add a little optimization here if you find your finish square early but, for the purposes of this exercise, we'll ignore that:
http://i.imgur.com/YAGNoSs.png

After that, it's a matter of starting at your finish square, finding the lowest distance value, moving to that square and repeating. This will leave you with the shortest path from start to finish:
http://i.imgur.com/mx3VLWx.png

Here's is an example with a square we're not allowed to go to. Note that there are actually 2 shortest paths and that you'll have to use additional logic to choose one (I chose to make as few turns as possible):
http://i.imgur.com/C8l6cLx.png

A huge benefit of Dijkstra's Algorithm is that, for static environments, it can be calculated ahead of time and stored in memory. Then all you have to know is where you are and where you want to go and the shortest path from S to F will have already been found.

[Jared Russell]

And once you grasp Dijkstra's algorithm, you'll probably want to learn about A*.

[Sparkyshires]

Okay, I think I get it. Just a couple questions:

• 1. How would this correlate into a map of line segments?
• 2. How would you go about putting something else in there for the time it may take to turn?
• 3. And what exactly is the a*?

[hzheng_449]

1) Traditionally Dijkstra's is thought described in terms of a graph with vertices connected edges. The example with the in the previous post has the centers of each grid as vertices and all adjacent grid centers are connected by edges. (Pretend a chessboard was represented as a graph with each square being an edge)

For a map of line segments, each point at the end of a line segment would be the vertex. And the line segment would be the edge. It also follows that the length of the line segment would be the edge weight.

2) You could make a function that adds a "maneuver" cost to each edge, and the algorithm would work fine. (Ie it takes 2 seconds to turn to get to a line and 9 seconds to travel the line, so I just say the overall time it takes to go down a line is 11 seconds.

3) A* is another path-finding algorithm (There is a link in the previous post) with a much lower computational cost than Dijkstra's method. I recommend you first get a solid understanding of Dijkstra's and some basic graph theory before looking too deeply into A*.

[Ty Tremblay]

Originally Posted by Sparkyshires

Okay, I think I get it. Just a couple questions:

• 1. How would this correlate into a map of line segments?
• 2. How would you go about putting something else in there for the time it may take to turn?
• 3. And what exactly is the a*?

1. I used squares because it was easy to generate the examples with ppt that way. Use intersections of your line segments. Since you're following a magnetic line, I'm assuming you can only turn at intersections, so call those intersections your nodes.

2. Dijkstra's Algorithm explanations use the term "distance" because that's the easiest term to understand when beginning to learn path planning. However, it's important to realize that physical distance is often not the only factor when navigating from point to point. A better term to use is "cost" instead of "distance." This would let you factor in time for turning and even things like energy conservation and a risk factor for getting stuck.

3. hzheng_449 hit the nail on the head. If your algorithm is a bike on training wheels, Dijkstra is without training wheels, and A* is a motorcycle. It's best to learn to ride without training wheels before jumping on a motorcycle.

[Sparkyshires]

Interesting, I've been looking into both, dijkstras more so. Thank's ya'll very much!