diff --git a/JPS.h b/JPS.h
index a643886..5a681fa 100644
--- a/JPS.h
+++ b/JPS.h
@@ -252,7 +252,11 @@ template <typename GRID> class Searcher
 {
 public:
 	Searcher(const GRID& g)
-		: grid(g), endNode(NULL), skip(1), stepsRemain(0), stepsDone(0)
+		: grid(g), endNode(NULL), skip(1), stepsRemain(0), stepsDone(0), useDiagonals(true)
+	{}
+
+	Searcher(const GRID& g, bool useDiagonals)
+		: grid(g), endNode(NULL), skip(1), stepsRemain(0), stepsDone(0), useDiagonals(useDiagonals)
 	{}
 
 	// single-call
@@ -278,6 +282,7 @@ template <typename GRID> class Searcher
 	int skip;
 	int stepsRemain;
 	size_t stepsDone;
+	bool useDiagonals;
 	OpenList open;
 
 	NodeGrid nodegrid;
@@ -661,7 +666,7 @@ template <typename GRID> bool Searcher<GRID>::generatePath(PathVector& path, uns
 	if(!endNode)
 		return false;
 	size_t offset = path.size();
-	if(step)
+	if(step && useDiagonals)
 	{
 		const Node *next = endNode;
 		const Node *prev = endNode->parent;
@@ -690,6 +695,59 @@ template <typename GRID> bool Searcher<GRID>::generatePath(PathVector& path, uns
 		}
 		while (prev);
 	}
+	else if (step && !useDiagonals)
+	{
+		const Node *next = endNode;
+		const Node *prev = endNode->parent;
+		if(!prev)
+			return false;
+		do
+		{
+			const unsigned x = next->pos.x, y = next->pos.y;
+			int dx = int(prev->pos.x - x);
+			int dy = int(prev->pos.y - y);
+			JPS_ASSERT(!dx || !dy || abs(dx) == abs(dy)); // known to be straight, if diagonal
+			const int steps = std::max(abs(dx), abs(dy));
+			dx /= std::max(abs(dx), 1);
+			dy /= std::max(abs(dy), 1);
+			dx *= int(step);
+			dy *= int(step);
+			int dxa = 0, dya = 0;
+			
+			for(int i = 0; i < steps; i += step)
+			{
+				if (dx && dy)
+				{
+					if (grid(x + dxa + dx, y + dya))
+					{
+						path.push_back(Pos(x + dxa, y + dya));
+						dxa += dx;
+
+						path.push_back(Pos(x + dxa, y + dya));
+						dya += dy;
+					}
+					else
+					{
+						path.push_back(Pos(x + dxa, y + dya));
+						dya += dy;
+
+						path.push_back(Pos(x + dxa, y + dya));
+						dxa += dx;
+					}
+				}
+				else
+				{
+					path.push_back(Pos(x+dxa, y+dya));
+
+					dxa += dx;
+					dya += dy;
+				}
+			}
+			next = prev;
+			prev = prev->parent;
+		}
+		while (prev);
+	}
 	else
 	{
 		const Node *next = endNode;
@@ -910,10 +968,11 @@ using Internal::Searcher;
 //       This will also skip through walls if they are less than <skip> blocks thick at any reachable position.
 template <typename GRID> bool findPath(PathVector& path, const GRID& grid, unsigned startx, unsigned starty, unsigned endx, unsigned endy,
                                        unsigned step = 0, int skip = 0, // optional params
+                                       bool useDiagonals = true, // whether to use diagonal steps
                                        size_t *stepsDone = NULL, size_t *nodesExpanded = NULL // for information
                                        )
 {
-	Searcher<GRID> search(grid);
+	Searcher<GRID> search(grid, useDiagonals);
 	search.setSkip(skip);
 	bool found = search.findPath(path, Pos(startx, starty), Pos(endx, endy), step);
 	if(stepsDone)
diff --git a/test/main b/test/main
new file mode 100755
index 0000000..ab61392
Binary files /dev/null and b/test/main differ
diff --git a/test/main.cpp b/test/main.cpp
index a47a4f7..0c11404 100644
--- a/test/main.cpp
+++ b/test/main.cpp
@@ -93,7 +93,7 @@ int main(int argc, char **argv)
 	size_t stepsDone, nodesExpanded;
 	for(size_t i = 1; i < waypoints.size(); ++i)
 	{
-		bool found = JPS::findPath(path, grid, waypoints[i-1].x, waypoints[i-1].y, waypoints[i].x, waypoints[i].y, step, 0, &stepsDone, &nodesExpanded);
+		bool found = JPS::findPath(path, grid, waypoints[i-1].x, waypoints[i-1].y, waypoints[i].x, waypoints[i].y, step, 0, true, &stepsDone, &nodesExpanded);
 		if(found)
 		{
 			assert(path[0] != waypoints[i-1]);
diff --git a/test/main2 b/test/main2
new file mode 100755
index 0000000..31fcc45
Binary files /dev/null and b/test/main2 differ
diff --git a/test/main2.cpp b/test/main2.cpp
index ef5f281..365720a 100644
--- a/test/main2.cpp
+++ b/test/main2.cpp
@@ -95,7 +95,7 @@ double runScenario(const char *file)
 		int runs = 0;
 
 		// single-call
-		//bool found = JPS::findPath(path, grid, ex.GetStartX(), ex.GetStartY(), ex.GetGoalX(), ex.GetGoalY(), 0, 0, &stepsDone, &nodesExpanded);
+		//bool found = JPS::findPath(path, grid, ex.GetStartX(), ex.GetStartY(), ex.GetGoalX(), ex.GetGoalY(), 0, 0, true, &stepsDone, &nodesExpanded);
 
 		// Testing incremental runs
 		bool found = false;