From 26ce262131a8d54f8b7cc15bcb72253926149ff3 Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Wed, 4 May 2022 14:29:36 +1000 Subject: [PATCH 01/16] added changes for https://github.com/hjweide/pyastar2d/issues/29 --- src/cpp/astar.cpp | 69 +++++++++++++++++++++++++++------- src/pyastar2d/astar_wrapper.py | 17 ++++++++- tests/test_astar.py | 35 +++++++++++++++++ 3 files changed, 107 insertions(+), 14 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index 11131eb..24485e0 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -27,15 +27,33 @@ bool operator<(const Node &n1, const Node &n2) { // See for various grid heuristics: // http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#S7 // L_\inf norm (diagonal distance) -inline float linf_norm(int i0, int j0, int i1, int j1) { - return std::max(std::abs(i0 - i1), std::abs(j0 - j1)); +inline float linf_norm(int current_x, int current_y, int goal_x, int goal_y) { + return std::max(std::abs(current_x - goal_x), std::abs(current_y - goal_y)); } // L_1 norm (manhattan distance) -inline float l1_norm(int i0, int j0, int i1, int j1) { - return std::abs(i0 - i1) + std::abs(j0 - j1); +inline float l1_norm(int current_x, int current_y, int goal_x, int goal_y) { + return std::abs(current_x - goal_x) + std::abs(current_y - goal_y); } +// Orthogonal x (moves by x first) +inline float l1_orthogonal_x(int current_x, int current_y, int goal_x, int goal_y) { + return std::abs(current_x - goal_x); +} + +// Orthogonal y (moves by y first) +inline float l1_orthogonal_y(int current_x, int current_y, int goal_x, int goal_y) { + return std::abs(current_y - goal_y); +} + +// tie breaker (prefer straight paths to goal) +inline float tie_breaker_func(int current_x, int current_y, int goal_x, int goal_y, int start_x, int start_y) { + int dx1 = current_x - goal_x; + int dy1 = current_y - goal_y; + int dx2 = start_x - goal_x; + int dy2 = start_y - goal_y; + return std::abs(dx1*dy2 - dx2*dy1); +} // weights: flattened h x w grid of costs // h, w: height and width of grid @@ -49,13 +67,17 @@ static PyObject *astar(PyObject *self, PyObject *args) { int start; int goal; int diag_ok; + int heuristic_override; + int tiebreaker_coefficient; if (!PyArg_ParseTuple( - args, "Oiiiii", // i = int, O = object + args, "Oiiiiiii", // i = int, O = object &weights_object, &h, &w, &start, &goal, - &diag_ok)) + &diag_ok, &heuristic_override, + &tiebreaker_coefficient + )) return NULL; float* weights = (float*) weights_object->data; @@ -73,6 +95,25 @@ static PyObject *astar(PyObject *self, PyObject *args) { nodes_to_visit.push(start_node); int* nbrs = new int[8]; + + // Get the heuristic method to use + float (*heuristic_func)(int, int, int, int); + + if (heuristic_override == 1) { + heuristic_func = linf_norm; + } else if (heuristic_override == 2) { + heuristic_func = l1_norm; + } else if (heuristic_override == 3) { + heuristic_func = l1_orthogonal_x; + } else if (heuristic_override == 4) { + heuristic_func = l1_orthogonal_y; + } else { // default + if (diag_ok) { + heuristic_func = linf_norm; + } else { + heuristic_func = l1_norm; + } + } while (!nodes_to_visit.empty()) { // .top() doesn't actually remove the node @@ -104,13 +145,15 @@ static PyObject *astar(PyObject *self, PyObject *args) { float new_cost = costs[cur.idx] + weights[nbrs[i]]; if (new_cost < costs[nbrs[i]]) { // estimate the cost to the goal based on legal moves - if (diag_ok) { - heuristic_cost = linf_norm(nbrs[i] / w, nbrs[i] % w, - goal / w, goal % w); - } - else { - heuristic_cost = l1_norm(nbrs[i] / w, nbrs[i] % w, - goal / w, goal % w); + heuristic_cost = heuristic_func(nbrs[i] / w, nbrs[i] % w, + goal / w, goal % w); + + // add tiebreaker cost + if (tiebreaker_coefficient > 0) { + heuristic_cost = heuristic_cost + + tiebreaker_coefficient/1000.0f * tie_breaker_func(nbrs[i] / w, nbrs[i] % w, + goal / w, goal % w, + start / w, start % w); } // paths with lower expected cost are explored first diff --git a/src/pyastar2d/astar_wrapper.py b/src/pyastar2d/astar_wrapper.py index 95c5827..aba7a28 100644 --- a/src/pyastar2d/astar_wrapper.py +++ b/src/pyastar2d/astar_wrapper.py @@ -19,6 +19,8 @@ ctypes.c_int, # start index in flattened grid ctypes.c_int, # goal index in flattened grid ctypes.c_bool, # allow diagonal + ctypes.c_int, # heuristic_override + ctypes.c_int, # tiebreaker_coefficient ] @@ -26,7 +28,19 @@ def astar_path( weights: np.ndarray, start: Tuple[int, int], goal: Tuple[int, int], - allow_diagonal: bool = False) -> Optional[np.ndarray]: + allow_diagonal: bool = False, + heuristic_override: int = 0, + tiebreaker_coefficient: int = 0) -> Optional[np.ndarray]: + """ + Run astar algorithm on 2d weights. + + param np.ndarray weights: A grid of weights e.g. np.ones((10, 10), dtype=np.float32) + param Tuple[int, int] start: (x, y) point to start + param Tuple[int, int] goal: (x, y) point to end + param bool allow_diagonal: Whether to allow diagonal moves + param int heuristic_override: Override heuristic 0=auto, 1=diagonal, 2=manhattan, 3=orthogonal-x, 4=orthogonal-y + param int tiebreaker_coefficient: Add tiebreaker to heuristic cost, 0=disable, positive enables it by that amount/1000.0 + """ assert weights.dtype == np.float32, ( f"weights must have np.float32 data type, but has {weights.dtype}" ) @@ -49,5 +63,6 @@ def astar_path( path = pyastar2d.astar.astar( weights.flatten(), height, width, start_idx, goal_idx, allow_diagonal, + heuristic_override, tiebreaker_coefficient ) return path diff --git a/tests/test_astar.py b/tests/test_astar.py index 59904a3..d171a3a 100644 --- a/tests/test_astar.py +++ b/tests/test_astar.py @@ -125,3 +125,38 @@ def test_bad_weights_dtype(): with pytest.raises(AssertionError) as exc: pyastar2d.astar_path(weights, (0, 0), (2, 2)) assert "float64" in exc.value.args[0] + + +def test_orthogonal_x(): + weights = np.ones((5, 5), dtype=np.float32) + # Run x + path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=3) + expected = np.array([[0, 0], [1, 0], [2, 0], [3, 0], [4, 0], [4, 1], [4, 2], [4, 3], [4, 4]]) + + assert np.all(path == expected) + + +def test_orthogonal_y(): + weights = np.ones((5, 5), dtype=np.float32) + # Run y + path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=4) + expected = np.array([[0, 0], [0, 1], [0, 2], [0, 3], [0, 4], [1, 4], [2, 4], [3, 4], [4, 4]]) + + assert np.all(path == expected) + +def test_tiebreaker_coefficient(): + weights = np.array([[1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, np.inf, 1, 1, 1], + [1, 1, 1, np.inf, 1, 1, 1], + [1, 1, 1, np.inf, 1, 1, 1], + [1, 1, 1, np.inf, 1, 1, 1], + [1, 1, 1, np.inf, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1]], dtype=np.float32) + weights = weights.T + path = pyastar2d.astar_path(weights, (0, 3), (6, 3), allow_diagonal=False, tiebreaker_coefficient=10000) + expected = np.array([[0, 3], [1, 3], [2, 3], [2, 4], + [2, 5], [2, 6], [3, 6], [4, 6], + [4, 5], [4, 4], [4, 3], [5, 3], + [6, 3]]) + + assert np.all(path == expected) From 045ddb1f3c2b7d8c9447670a5191ce9a05345aec Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Thu, 5 May 2022 14:33:38 +1000 Subject: [PATCH 02/16] minor update after comments --- src/cpp/astar.cpp | 8 ++++---- src/pyastar2d/astar_wrapper.py | 8 +++++--- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index 24485e0..82c328c 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -68,10 +68,10 @@ static PyObject *astar(PyObject *self, PyObject *args) { int goal; int diag_ok; int heuristic_override; - int tiebreaker_coefficient; + float tiebreaker_coefficient; if (!PyArg_ParseTuple( - args, "Oiiiiiii", // i = int, O = object + args, "Oiiiiiif", // i = int, O = object &weights_object, &h, &w, &start, &goal, @@ -149,9 +149,9 @@ static PyObject *astar(PyObject *self, PyObject *args) { goal / w, goal % w); // add tiebreaker cost - if (tiebreaker_coefficient > 0) { + if (tiebreaker_coefficient > 0.0f) { heuristic_cost = heuristic_cost + - tiebreaker_coefficient/1000.0f * tie_breaker_func(nbrs[i] / w, nbrs[i] % w, + tiebreaker_coefficient * tie_breaker_func(nbrs[i] / w, nbrs[i] % w, goal / w, goal % w, start / w, start % w); } diff --git a/src/pyastar2d/astar_wrapper.py b/src/pyastar2d/astar_wrapper.py index aba7a28..6ca99c2 100644 --- a/src/pyastar2d/astar_wrapper.py +++ b/src/pyastar2d/astar_wrapper.py @@ -20,7 +20,7 @@ ctypes.c_int, # goal index in flattened grid ctypes.c_bool, # allow diagonal ctypes.c_int, # heuristic_override - ctypes.c_int, # tiebreaker_coefficient + ctypes.c_float, # tiebreaker_coefficient ] @@ -30,7 +30,7 @@ def astar_path( goal: Tuple[int, int], allow_diagonal: bool = False, heuristic_override: int = 0, - tiebreaker_coefficient: int = 0) -> Optional[np.ndarray]: + tiebreaker_coefficient: float = 0.0) -> Optional[np.ndarray]: """ Run astar algorithm on 2d weights. @@ -39,7 +39,9 @@ def astar_path( param Tuple[int, int] goal: (x, y) point to end param bool allow_diagonal: Whether to allow diagonal moves param int heuristic_override: Override heuristic 0=auto, 1=diagonal, 2=manhattan, 3=orthogonal-x, 4=orthogonal-y - param int tiebreaker_coefficient: Add tiebreaker to heuristic cost, 0=disable, positive enables it by that amount/1000.0 + param float tiebreaker_coefficient: Add tiebreaker to heuristic cost, 0=disable, positive enables it by that amount/1000.0 + + Important: Please take care when using heuristic_override and tiebreaker_coefficient, they may not take the shortest path. """ assert weights.dtype == np.float32, ( f"weights must have np.float32 data type, but has {weights.dtype}" From 21207cea2166ea8e51e3b12d231378b68c39b080 Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Thu, 5 May 2022 14:36:51 +1000 Subject: [PATCH 03/16] minor update after comments --- src/pyastar2d/astar_wrapper.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/pyastar2d/astar_wrapper.py b/src/pyastar2d/astar_wrapper.py index 6ca99c2..6792df9 100644 --- a/src/pyastar2d/astar_wrapper.py +++ b/src/pyastar2d/astar_wrapper.py @@ -33,14 +33,14 @@ def astar_path( tiebreaker_coefficient: float = 0.0) -> Optional[np.ndarray]: """ Run astar algorithm on 2d weights. - + param np.ndarray weights: A grid of weights e.g. np.ones((10, 10), dtype=np.float32) param Tuple[int, int] start: (x, y) point to start param Tuple[int, int] goal: (x, y) point to end param bool allow_diagonal: Whether to allow diagonal moves param int heuristic_override: Override heuristic 0=auto, 1=diagonal, 2=manhattan, 3=orthogonal-x, 4=orthogonal-y - param float tiebreaker_coefficient: Add tiebreaker to heuristic cost, 0=disable, positive enables it by that amount/1000.0 - + param float tiebreaker_coefficient: Add tiebreaker to heuristic cost, 0=disable, positive enables it by that amount + Important: Please take care when using heuristic_override and tiebreaker_coefficient, they may not take the shortest path. """ assert weights.dtype == np.float32, ( From 4fbd4ddb357cca0598b2e9b5cf3b9344302e53b6 Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Sat, 7 May 2022 08:47:16 +1000 Subject: [PATCH 04/16] removed tiebreaker funct, added better orthogonal x/y --- src/cpp/astar.cpp | 56 ++++++++++++++++++++++----------------------- tests/test_astar.py | 4 ++-- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index 82c328c..438d942 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -27,32 +27,33 @@ bool operator<(const Node &n1, const Node &n2) { // See for various grid heuristics: // http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#S7 // L_\inf norm (diagonal distance) -inline float linf_norm(int current_x, int current_y, int goal_x, int goal_y) { +inline float linf_norm(int current_x, int current_y, int goal_x, int goal_y, int max_x, int max_y) { return std::max(std::abs(current_x - goal_x), std::abs(current_y - goal_y)); } // L_1 norm (manhattan distance) -inline float l1_norm(int current_x, int current_y, int goal_x, int goal_y) { +inline float l1_norm(int current_x, int current_y, int goal_x, int goal_y, int max_x, int max_y) { return std::abs(current_x - goal_x) + std::abs(current_y - goal_y); } -// Orthogonal x (moves by x first) -inline float l1_orthogonal_x(int current_x, int current_y, int goal_x, int goal_y) { - return std::abs(current_x - goal_x); -} - -// Orthogonal y (moves by y first) -inline float l1_orthogonal_y(int current_x, int current_y, int goal_x, int goal_y) { - return std::abs(current_y - goal_y); +// Orthogonal x (moves by x first, then half way by y) +inline float l1_orthogonal_x(int current_x, int current_y, int goal_x, int goal_y, int max_x, int max_y) { + int dx = std::abs(current_x - goal_x); + if (dx > (max_x * 0.5)) { + return dx; + } else { + return std::abs(current_y - goal_y); + } } -// tie breaker (prefer straight paths to goal) -inline float tie_breaker_func(int current_x, int current_y, int goal_x, int goal_y, int start_x, int start_y) { - int dx1 = current_x - goal_x; - int dy1 = current_y - goal_y; - int dx2 = start_x - goal_x; - int dy2 = start_y - goal_y; - return std::abs(dx1*dy2 - dx2*dy1); +// Orthogonal y (moves by y first, then half way by x) +inline float l1_orthogonal_y(int current_x, int current_y, int goal_x, int goal_y, int max_x, int max_y) { + int dy = std::abs(current_y - goal_y); + if (dy > (max_y * 0.5)) { + return dy; + } else { + return std::abs(current_x - goal_x); + } } // weights: flattened h x w grid of costs @@ -97,7 +98,7 @@ static PyObject *astar(PyObject *self, PyObject *args) { int* nbrs = new int[8]; // Get the heuristic method to use - float (*heuristic_func)(int, int, int, int); + float (*heuristic_func)(int, int, int, int, int, int); if (heuristic_override == 1) { heuristic_func = linf_norm; @@ -114,6 +115,10 @@ static PyObject *astar(PyObject *self, PyObject *args) { heuristic_func = l1_norm; } } + + // calculate max_x and max_y + int max_x = std::abs(goal / w - start / w); + int max_y = std::abs(goal % w - start % w); while (!nodes_to_visit.empty()) { // .top() doesn't actually remove the node @@ -145,16 +150,11 @@ static PyObject *astar(PyObject *self, PyObject *args) { float new_cost = costs[cur.idx] + weights[nbrs[i]]; if (new_cost < costs[nbrs[i]]) { // estimate the cost to the goal based on legal moves - heuristic_cost = heuristic_func(nbrs[i] / w, nbrs[i] % w, - goal / w, goal % w); - - // add tiebreaker cost - if (tiebreaker_coefficient > 0.0f) { - heuristic_cost = heuristic_cost + - tiebreaker_coefficient * tie_breaker_func(nbrs[i] / w, nbrs[i] % w, - goal / w, goal % w, - start / w, start % w); - } + // tiebreaker_coefficient can be changed to give different paths + heuristic_cost = (1 + tiebreaker_coefficient) * + heuristic_func(nbrs[i] / w, nbrs[i] % w, + goal / w, goal % w, + max_x , max_y); // paths with lower expected cost are explored first float priority = new_cost + heuristic_cost; diff --git a/tests/test_astar.py b/tests/test_astar.py index d171a3a..7e74754 100644 --- a/tests/test_astar.py +++ b/tests/test_astar.py @@ -131,7 +131,7 @@ def test_orthogonal_x(): weights = np.ones((5, 5), dtype=np.float32) # Run x path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=3) - expected = np.array([[0, 0], [1, 0], [2, 0], [3, 0], [4, 0], [4, 1], [4, 2], [4, 3], [4, 4]]) + expected = np.array([[0, 0], [1, 0], [1, 1], [1, 2], [2, 2], [2, 3], [2, 4], [3, 4], [4, 4]]) assert np.all(path == expected) @@ -140,7 +140,7 @@ def test_orthogonal_y(): weights = np.ones((5, 5), dtype=np.float32) # Run y path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=4) - expected = np.array([[0, 0], [0, 1], [0, 2], [0, 3], [0, 4], [1, 4], [2, 4], [3, 4], [4, 4]]) + expected = np.array([[0, 0], [0, 1], [1, 1], [1, 2], [2, 2], [3, 2], [4, 2], [4, 3], [4, 4]]) assert np.all(path == expected) From 885327ab74f8c15981a896b82c1770261e3dcbd8 Mon Sep 17 00:00:00 2001 From: peterchenadded Date: Sat, 7 May 2022 10:05:46 +1000 Subject: [PATCH 05/16] Update astar.cpp Fixed issue where there were bumps --- src/cpp/astar.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index 438d942..9ef3cb4 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -40,7 +40,7 @@ inline float l1_norm(int current_x, int current_y, int goal_x, int goal_y, int m inline float l1_orthogonal_x(int current_x, int current_y, int goal_x, int goal_y, int max_x, int max_y) { int dx = std::abs(current_x - goal_x); if (dx > (max_x * 0.5)) { - return dx; + return dx + max_y; } else { return std::abs(current_y - goal_y); } @@ -50,7 +50,7 @@ inline float l1_orthogonal_x(int current_x, int current_y, int goal_x, int goal_ inline float l1_orthogonal_y(int current_x, int current_y, int goal_x, int goal_y, int max_x, int max_y) { int dy = std::abs(current_y - goal_y); if (dy > (max_y * 0.5)) { - return dy; + return dy + max_x; } else { return std::abs(current_x - goal_x); } From 261cc8a04896de7f3eaa8799df7a84f892b414bd Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Sat, 7 May 2022 22:28:37 +1000 Subject: [PATCH 06/16] more updates after feed back --- src/cpp/astar.cpp | 70 +++++++++++++++++----------------- src/pyastar2d/astar_wrapper.py | 19 +++++---- tests/test_astar.py | 23 +---------- 3 files changed, 49 insertions(+), 63 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index 438d942..8454b30 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -24,35 +24,41 @@ bool operator<(const Node &n1, const Node &n2) { return n1.cost > n2.cost; } +enum Heuristic { default, diagonal_distance, manhattan_distance, orthogonal_x, orthogonal_y }; + // See for various grid heuristics: // http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#S7 // L_\inf norm (diagonal distance) -inline float linf_norm(int current_x, int current_y, int goal_x, int goal_y, int max_x, int max_y) { - return std::max(std::abs(current_x - goal_x), std::abs(current_y - goal_y)); +inline float linf_norm(int i0, int j0, int i1, int j1, int, int) { + return std::max(std::abs(i0 - i1), std::abs(j0 - j1)); } // L_1 norm (manhattan distance) -inline float l1_norm(int current_x, int current_y, int goal_x, int goal_y, int max_x, int max_y) { - return std::abs(current_x - goal_x) + std::abs(current_y - goal_y); +inline float l1_norm(int i0, int j0, int i1, int j1, int, int) { + return std::abs(i0 - i1) + std::abs(j0 - j1); } // Orthogonal x (moves by x first, then half way by y) -inline float l1_orthogonal_x(int current_x, int current_y, int goal_x, int goal_y, int max_x, int max_y) { - int dx = std::abs(current_x - goal_x); - if (dx > (max_x * 0.5)) { - return dx; +inline float h_orthogonal_x(int i0, int j0, int i1, int j1, int i2, int j2) { + int di = std::abs(i0 - i1); + int dim = std::abs(i1 - i2); + int djm = std::abs(j1 - j2); + if (di > (dim * 0.5)) { + return di + djm; } else { - return std::abs(current_y - goal_y); + return std::abs(j0 - j1); } } // Orthogonal y (moves by y first, then half way by x) -inline float l1_orthogonal_y(int current_x, int current_y, int goal_x, int goal_y, int max_x, int max_y) { - int dy = std::abs(current_y - goal_y); - if (dy > (max_y * 0.5)) { - return dy; +inline float h_orthogonal_y(int i0, int j0, int i1, int j1, int i2, int j2) { + int dj = std::abs(j0 - j1); + int djm = std::abs(j1 - j2); + int dim = std::abs(i1 - i2); + if (dj > (djm * 0.5)) { + return dj + dim; } else { - return std::abs(current_x - goal_x); + return std::abs(i0 - i1); } } @@ -69,15 +75,13 @@ static PyObject *astar(PyObject *self, PyObject *args) { int goal; int diag_ok; int heuristic_override; - float tiebreaker_coefficient; if (!PyArg_ParseTuple( - args, "Oiiiiiif", // i = int, O = object + args, "Oiiiiii", // i = int, O = object &weights_object, &h, &w, &start, &goal, - &diag_ok, &heuristic_override, - &tiebreaker_coefficient + &diag_ok, &heuristic_override )) return NULL; @@ -100,25 +104,23 @@ static PyObject *astar(PyObject *self, PyObject *args) { // Get the heuristic method to use float (*heuristic_func)(int, int, int, int, int, int); - if (heuristic_override == 1) { - heuristic_func = linf_norm; - } else if (heuristic_override == 2) { - heuristic_func = l1_norm; - } else if (heuristic_override == 3) { - heuristic_func = l1_orthogonal_x; - } else if (heuristic_override == 4) { - heuristic_func = l1_orthogonal_y; - } else { // default + if (heuristic_override == Heuristic::default) { if (diag_ok) { heuristic_func = linf_norm; } else { heuristic_func = l1_norm; } + } else { + if (heuristic_override == Heuristic::diagonal_distance) { + heuristic_func = linf_norm; + } else if (heuristic_override == Heuristic::manhattan_distance) { + heuristic_func = l1_norm; + } else if (heuristic_override == Heuristic::orthogonal_x) { + heuristic_func = h_orthogonal_x; + } else if (heuristic_override == Heuristic::orthogonal_y) { + heuristic_func = h_orthogonal_y; + } } - - // calculate max_x and max_y - int max_x = std::abs(goal / w - start / w); - int max_y = std::abs(goal % w - start % w); while (!nodes_to_visit.empty()) { // .top() doesn't actually remove the node @@ -150,11 +152,9 @@ static PyObject *astar(PyObject *self, PyObject *args) { float new_cost = costs[cur.idx] + weights[nbrs[i]]; if (new_cost < costs[nbrs[i]]) { // estimate the cost to the goal based on legal moves - // tiebreaker_coefficient can be changed to give different paths - heuristic_cost = (1 + tiebreaker_coefficient) * - heuristic_func(nbrs[i] / w, nbrs[i] % w, + heuristic_cost = heuristic_func(nbrs[i] / w, nbrs[i] % w, goal / w, goal % w, - max_x , max_y); + start / w, start % w); // paths with lower expected cost are explored first float priority = new_cost + heuristic_cost; diff --git a/src/pyastar2d/astar_wrapper.py b/src/pyastar2d/astar_wrapper.py index 6792df9..3a178b6 100644 --- a/src/pyastar2d/astar_wrapper.py +++ b/src/pyastar2d/astar_wrapper.py @@ -1,6 +1,7 @@ import ctypes import numpy as np import pyastar2d.astar +from enum import IntEnum from typing import Optional, Tuple @@ -20,17 +21,23 @@ ctypes.c_int, # goal index in flattened grid ctypes.c_bool, # allow diagonal ctypes.c_int, # heuristic_override - ctypes.c_float, # tiebreaker_coefficient ] +class Heuristic(IntEnum): + """The supported heuristics.""" + + default = 0 + diagonal_distance = 1 + manhattan_distance = 2 + orthogonal_x = 3 + orthogonal_y = 4 def astar_path( weights: np.ndarray, start: Tuple[int, int], goal: Tuple[int, int], allow_diagonal: bool = False, - heuristic_override: int = 0, - tiebreaker_coefficient: float = 0.0) -> Optional[np.ndarray]: + heuristic_override: Heuristic = Heuristic.default) -> Optional[np.ndarray]: """ Run astar algorithm on 2d weights. @@ -38,10 +45,8 @@ def astar_path( param Tuple[int, int] start: (x, y) point to start param Tuple[int, int] goal: (x, y) point to end param bool allow_diagonal: Whether to allow diagonal moves - param int heuristic_override: Override heuristic 0=auto, 1=diagonal, 2=manhattan, 3=orthogonal-x, 4=orthogonal-y - param float tiebreaker_coefficient: Add tiebreaker to heuristic cost, 0=disable, positive enables it by that amount + param Heuristic heuristic_override: Override heuristic, see Heuristic(IntEnum) - Important: Please take care when using heuristic_override and tiebreaker_coefficient, they may not take the shortest path. """ assert weights.dtype == np.float32, ( f"weights must have np.float32 data type, but has {weights.dtype}" @@ -65,6 +70,6 @@ def astar_path( path = pyastar2d.astar.astar( weights.flatten(), height, width, start_idx, goal_idx, allow_diagonal, - heuristic_override, tiebreaker_coefficient + int(heuristic_override) ) return path diff --git a/tests/test_astar.py b/tests/test_astar.py index 7e74754..c548990 100644 --- a/tests/test_astar.py +++ b/tests/test_astar.py @@ -129,34 +129,15 @@ def test_bad_weights_dtype(): def test_orthogonal_x(): weights = np.ones((5, 5), dtype=np.float32) - # Run x path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=3) - expected = np.array([[0, 0], [1, 0], [1, 1], [1, 2], [2, 2], [2, 3], [2, 4], [3, 4], [4, 4]]) + expected = np.array([[0, 0], [1, 0], [2, 0], [2, 1], [2, 2], [2, 3], [2, 4], [3, 4], [4, 4]]) assert np.all(path == expected) def test_orthogonal_y(): weights = np.ones((5, 5), dtype=np.float32) - # Run y path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=4) - expected = np.array([[0, 0], [0, 1], [1, 1], [1, 2], [2, 2], [3, 2], [4, 2], [4, 3], [4, 4]]) - - assert np.all(path == expected) - -def test_tiebreaker_coefficient(): - weights = np.array([[1, 1, 1, 1, 1, 1, 1], - [1, 1, 1, np.inf, 1, 1, 1], - [1, 1, 1, np.inf, 1, 1, 1], - [1, 1, 1, np.inf, 1, 1, 1], - [1, 1, 1, np.inf, 1, 1, 1], - [1, 1, 1, np.inf, 1, 1, 1], - [1, 1, 1, 1, 1, 1, 1]], dtype=np.float32) - weights = weights.T - path = pyastar2d.astar_path(weights, (0, 3), (6, 3), allow_diagonal=False, tiebreaker_coefficient=10000) - expected = np.array([[0, 3], [1, 3], [2, 3], [2, 4], - [2, 5], [2, 6], [3, 6], [4, 6], - [4, 5], [4, 4], [4, 3], [5, 3], - [6, 3]]) + expected = np.array([[0, 0], [0, 1], [0, 2], [1, 2], [2, 2], [3, 2], [4, 2], [4, 3], [4, 4]]) assert np.all(path == expected) From 1062ebb0ecb053623a1fb17afadecb9fbc19b9b2 Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Sat, 7 May 2022 22:54:43 +1000 Subject: [PATCH 07/16] fixed c++ compiling error --- src/cpp/astar.cpp | 20 ++++++++++---------- src/pyastar2d/astar_wrapper.py | 14 +++++++------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index 8454b30..72e0720 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -24,7 +24,7 @@ bool operator<(const Node &n1, const Node &n2) { return n1.cost > n2.cost; } -enum Heuristic { default, diagonal_distance, manhattan_distance, orthogonal_x, orthogonal_y }; +enum Heuristic { DEFAULT, DIAGONAL_DISTANCE, MANHATTAN_DISTANCE, ORTHOGONAL_X, ORTHOGONAL_Y }; // See for various grid heuristics: // http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#S7 @@ -39,7 +39,7 @@ inline float l1_norm(int i0, int j0, int i1, int j1, int, int) { } // Orthogonal x (moves by x first, then half way by y) -inline float h_orthogonal_x(int i0, int j0, int i1, int j1, int i2, int j2) { +inline float orthogonal_x(int i0, int j0, int i1, int j1, int i2, int j2) { int di = std::abs(i0 - i1); int dim = std::abs(i1 - i2); int djm = std::abs(j1 - j2); @@ -51,7 +51,7 @@ inline float h_orthogonal_x(int i0, int j0, int i1, int j1, int i2, int j2) { } // Orthogonal y (moves by y first, then half way by x) -inline float h_orthogonal_y(int i0, int j0, int i1, int j1, int i2, int j2) { +inline float orthogonal_y(int i0, int j0, int i1, int j1, int i2, int j2) { int dj = std::abs(j0 - j1); int djm = std::abs(j1 - j2); int dim = std::abs(i1 - i2); @@ -104,21 +104,21 @@ static PyObject *astar(PyObject *self, PyObject *args) { // Get the heuristic method to use float (*heuristic_func)(int, int, int, int, int, int); - if (heuristic_override == Heuristic::default) { + if (heuristic_override == Heuristic::DEFAULT) { if (diag_ok) { heuristic_func = linf_norm; } else { heuristic_func = l1_norm; } } else { - if (heuristic_override == Heuristic::diagonal_distance) { + if (heuristic_override == Heuristic::DIAGONAL_DISTANCE) { heuristic_func = linf_norm; - } else if (heuristic_override == Heuristic::manhattan_distance) { + } else if (heuristic_override == Heuristic::MANHATTAN_DISTANCE) { heuristic_func = l1_norm; - } else if (heuristic_override == Heuristic::orthogonal_x) { - heuristic_func = h_orthogonal_x; - } else if (heuristic_override == Heuristic::orthogonal_y) { - heuristic_func = h_orthogonal_y; + } else if (heuristic_override == Heuristic::ORTHOGONAL_X) { + heuristic_func = orthogonal_x; + } else if (heuristic_override == Heuristic::ORTHOGONAL_Y) { + heuristic_func = orthogonal_y; } } diff --git a/src/pyastar2d/astar_wrapper.py b/src/pyastar2d/astar_wrapper.py index 3a178b6..0b9047c 100644 --- a/src/pyastar2d/astar_wrapper.py +++ b/src/pyastar2d/astar_wrapper.py @@ -25,19 +25,19 @@ class Heuristic(IntEnum): """The supported heuristics.""" - - default = 0 - diagonal_distance = 1 - manhattan_distance = 2 - orthogonal_x = 3 - orthogonal_y = 4 + + DEFAULT = 0 + DIAGONAL_DISTANCE = 1 + MANHATTAN_DISTANCE = 2 + ORTHOGONAL_X = 3 + ORTHOGONAL_Y = 4 def astar_path( weights: np.ndarray, start: Tuple[int, int], goal: Tuple[int, int], allow_diagonal: bool = False, - heuristic_override: Heuristic = Heuristic.default) -> Optional[np.ndarray]: + heuristic_override: Heuristic = Heuristic.DEFAULT) -> Optional[np.ndarray]: """ Run astar algorithm on 2d weights. From a96a15e09e170fb1856ba71dfa8947da53b8e7ec Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Sat, 7 May 2022 22:59:38 +1000 Subject: [PATCH 08/16] moved to switch statement --- src/cpp/astar.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index 72e0720..6ebd189 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -111,14 +111,21 @@ static PyObject *astar(PyObject *self, PyObject *args) { heuristic_func = l1_norm; } } else { - if (heuristic_override == Heuristic::DIAGONAL_DISTANCE) { - heuristic_func = linf_norm; - } else if (heuristic_override == Heuristic::MANHATTAN_DISTANCE) { - heuristic_func = l1_norm; - } else if (heuristic_override == Heuristic::ORTHOGONAL_X) { - heuristic_func = orthogonal_x; - } else if (heuristic_override == Heuristic::ORTHOGONAL_Y) { - heuristic_func = orthogonal_y; + switch(heuristic_override) { + case Heuristic::DIAGONAL_DISTANCE: + heuristic_func = linf_norm; + break; + case Heuristic::MANHATTAN_DISTANCE: + heuristic_func = l1_norm; + break; + case Heuristic::ORTHOGONAL_X: + heuristic_func = orthogonal_x; + break; + case Heuristic::ORTHOGONAL_Y: + heuristic_func = orthogonal_y; + break; + default: + return NULL; } } From 12bd30a6cdc9ecc7e104913dd7386b7b24214006 Mon Sep 17 00:00:00 2001 From: peterchenadded Date: Sat, 7 May 2022 23:05:31 +1000 Subject: [PATCH 09/16] Update test_astar.py Removed magical numbers --- tests/test_astar.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tests/test_astar.py b/tests/test_astar.py index c548990..deb8d4f 100644 --- a/tests/test_astar.py +++ b/tests/test_astar.py @@ -5,6 +5,8 @@ import sys import pyastar2d +from pyastar2d import Heuristic + def test_small(): weights = np.array([[1, 3, 3, 3, 3], @@ -129,7 +131,7 @@ def test_bad_weights_dtype(): def test_orthogonal_x(): weights = np.ones((5, 5), dtype=np.float32) - path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=3) + path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=Heuristic.ORTHOGONAL_X) expected = np.array([[0, 0], [1, 0], [2, 0], [2, 1], [2, 2], [2, 3], [2, 4], [3, 4], [4, 4]]) assert np.all(path == expected) @@ -137,7 +139,7 @@ def test_orthogonal_x(): def test_orthogonal_y(): weights = np.ones((5, 5), dtype=np.float32) - path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=4) + path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=Heuristic.ORTHOGONAL_Y) expected = np.array([[0, 0], [0, 1], [0, 2], [1, 2], [2, 2], [3, 2], [4, 2], [4, 3], [4, 4]]) assert np.all(path == expected) From c0ba0b1dba3d1856eb8a7ba6c026d70f13f77e14 Mon Sep 17 00:00:00 2001 From: peterchenadded Date: Sat, 7 May 2022 23:07:57 +1000 Subject: [PATCH 10/16] Update __init__.py Added heuristic --- src/pyastar2d/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pyastar2d/__init__.py b/src/pyastar2d/__init__.py index 3579838..644dc48 100644 --- a/src/pyastar2d/__init__.py +++ b/src/pyastar2d/__init__.py @@ -1,2 +1,2 @@ -from pyastar2d.astar_wrapper import astar_path -__all__ = ["astar_path"] +from pyastar2d.astar_wrapper import astar_path, Heuristic +__all__ = ["astar_path", "Heuristic"] From 22f0bab5bdf1ab15056010ce38d8f510e9521aad Mon Sep 17 00:00:00 2001 From: peterchenadded Date: Sat, 7 May 2022 23:13:46 +1000 Subject: [PATCH 11/16] Update astar.cpp Fix compiling error on linux --- src/cpp/astar.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index 6ebd189..88fc9f2 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -104,7 +104,7 @@ static PyObject *astar(PyObject *self, PyObject *args) { // Get the heuristic method to use float (*heuristic_func)(int, int, int, int, int, int); - if (heuristic_override == Heuristic::DEFAULT) { + if (heuristic_override == DEFAULT) { if (diag_ok) { heuristic_func = linf_norm; } else { @@ -112,16 +112,16 @@ static PyObject *astar(PyObject *self, PyObject *args) { } } else { switch(heuristic_override) { - case Heuristic::DIAGONAL_DISTANCE: + case DIAGONAL_DISTANCE: heuristic_func = linf_norm; break; - case Heuristic::MANHATTAN_DISTANCE: + case MANHATTAN_DISTANCE: heuristic_func = l1_norm; break; - case Heuristic::ORTHOGONAL_X: + case ORTHOGONAL_X: heuristic_func = orthogonal_x; break; - case Heuristic::ORTHOGONAL_Y: + case ORTHOGONAL_Y: heuristic_func = orthogonal_y; break; default: From e27887e607f4aedc7fe3cd80217160a413ebaaf5 Mon Sep 17 00:00:00 2001 From: peterchenadded Date: Sat, 7 May 2022 23:33:02 +1000 Subject: [PATCH 12/16] Update astar.cpp Moved heuristic override to its own method as per suggestion --- src/cpp/astar.cpp | 38 ++++++++++++++++++++------------------ 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index 88fc9f2..e19ab2f 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -62,6 +62,23 @@ inline float orthogonal_y(int i0, int j0, int i1, int j1, int i2, int j2) { } } +typedef float (*heuristic_ptr)(int, int, int, int, int, int); + +inline heuristic_ptr select_heuristic(int h) { + switch (h) { + case DIAGONAL_DISTANCE: + return linf_norm; + case MANHATTAN_DISTANCE: + return l1_norm; + case ORTHOGONAL_X: + return orthogonal_x; + case ORTHOGONAL_Y: + return orthogonal_y; + default: + return l1_norm; + } +} + // weights: flattened h x w grid of costs // h, w: height and width of grid // start, goal: index of start/goal in flattened grid @@ -101,9 +118,9 @@ static PyObject *astar(PyObject *self, PyObject *args) { int* nbrs = new int[8]; + heuristic_ptr heuristic_func; + // Get the heuristic method to use - float (*heuristic_func)(int, int, int, int, int, int); - if (heuristic_override == DEFAULT) { if (diag_ok) { heuristic_func = linf_norm; @@ -111,22 +128,7 @@ static PyObject *astar(PyObject *self, PyObject *args) { heuristic_func = l1_norm; } } else { - switch(heuristic_override) { - case DIAGONAL_DISTANCE: - heuristic_func = linf_norm; - break; - case MANHATTAN_DISTANCE: - heuristic_func = l1_norm; - break; - case ORTHOGONAL_X: - heuristic_func = orthogonal_x; - break; - case ORTHOGONAL_Y: - heuristic_func = orthogonal_y; - break; - default: - return NULL; - } + heuristic_func = select_heuristic(heuristic_override); } while (!nodes_to_visit.empty()) { From 19651bbc907177c1ad3eddb3ca41bd95882abe9b Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Sun, 8 May 2022 23:57:32 +1000 Subject: [PATCH 13/16] changes after feedback --- src/cpp/astar.cpp | 8 +++----- src/pyastar2d/astar_wrapper.py | 10 ++++------ 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index e19ab2f..76a136f 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -24,7 +24,7 @@ bool operator<(const Node &n1, const Node &n2) { return n1.cost > n2.cost; } -enum Heuristic { DEFAULT, DIAGONAL_DISTANCE, MANHATTAN_DISTANCE, ORTHOGONAL_X, ORTHOGONAL_Y }; +enum Heuristic { DEFAULT, ORTHOGONAL_X, ORTHOGONAL_Y }; // See for various grid heuristics: // http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#S7 @@ -38,6 +38,7 @@ inline float l1_norm(int i0, int j0, int i1, int j1, int, int) { return std::abs(i0 - i1) + std::abs(j0 - j1); } +// Please note below heuristic is experimental and only for pretty lines // Orthogonal x (moves by x first, then half way by y) inline float orthogonal_x(int i0, int j0, int i1, int j1, int i2, int j2) { int di = std::abs(i0 - i1); @@ -50,6 +51,7 @@ inline float orthogonal_x(int i0, int j0, int i1, int j1, int i2, int j2) { } } +// Please note below heuristic is experimental and only for pretty lines // Orthogonal y (moves by y first, then half way by x) inline float orthogonal_y(int i0, int j0, int i1, int j1, int i2, int j2) { int dj = std::abs(j0 - j1); @@ -66,10 +68,6 @@ typedef float (*heuristic_ptr)(int, int, int, int, int, int); inline heuristic_ptr select_heuristic(int h) { switch (h) { - case DIAGONAL_DISTANCE: - return linf_norm; - case MANHATTAN_DISTANCE: - return l1_norm; case ORTHOGONAL_X: return orthogonal_x; case ORTHOGONAL_Y: diff --git a/src/pyastar2d/astar_wrapper.py b/src/pyastar2d/astar_wrapper.py index 0b9047c..0d36f95 100644 --- a/src/pyastar2d/astar_wrapper.py +++ b/src/pyastar2d/astar_wrapper.py @@ -27,10 +27,8 @@ class Heuristic(IntEnum): """The supported heuristics.""" DEFAULT = 0 - DIAGONAL_DISTANCE = 1 - MANHATTAN_DISTANCE = 2 - ORTHOGONAL_X = 3 - ORTHOGONAL_Y = 4 + ORTHOGONAL_X = 1 + ORTHOGONAL_Y = 2 def astar_path( weights: np.ndarray, @@ -42,8 +40,8 @@ def astar_path( Run astar algorithm on 2d weights. param np.ndarray weights: A grid of weights e.g. np.ones((10, 10), dtype=np.float32) - param Tuple[int, int] start: (x, y) point to start - param Tuple[int, int] goal: (x, y) point to end + param Tuple[int, int] start: (i, j) + param Tuple[int, int] goal: (i, j) param bool allow_diagonal: Whether to allow diagonal moves param Heuristic heuristic_override: Override heuristic, see Heuristic(IntEnum) From 894db9f582605127798bd876138713b72886eb19 Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Mon, 9 May 2022 00:28:12 +1000 Subject: [PATCH 14/16] performance fixes --- src/cpp/astar.cpp | 41 +++++++++++++++++++++++------------------ 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index 76a136f..f82d476 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -29,12 +29,12 @@ enum Heuristic { DEFAULT, ORTHOGONAL_X, ORTHOGONAL_Y }; // See for various grid heuristics: // http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#S7 // L_\inf norm (diagonal distance) -inline float linf_norm(int i0, int j0, int i1, int j1, int, int) { +inline float linf_norm(int i0, int j0, int i1, int j1) { return std::max(std::abs(i0 - i1), std::abs(j0 - j1)); } // L_1 norm (manhattan distance) -inline float l1_norm(int i0, int j0, int i1, int j1, int, int) { +inline float l1_norm(int i0, int j0, int i1, int j1) { return std::abs(i0 - i1) + std::abs(j0 - j1); } @@ -73,7 +73,7 @@ inline heuristic_ptr select_heuristic(int h) { case ORTHOGONAL_Y: return orthogonal_y; default: - return l1_norm; + return NULL; } } @@ -116,18 +116,12 @@ static PyObject *astar(PyObject *self, PyObject *args) { int* nbrs = new int[8]; - heuristic_ptr heuristic_func; - - // Get the heuristic method to use - if (heuristic_override == DEFAULT) { - if (diag_ok) { - heuristic_func = linf_norm; - } else { - heuristic_func = l1_norm; - } - } else { - heuristic_func = select_heuristic(heuristic_override); - } + int goal_i = goal / w; + int goal_j = goal % w; + int start_i = start / w; + int start_j = start % w; + + heuristic_ptr heuristic_func = select_heuristic(heuristic_override); while (!nodes_to_visit.empty()) { // .top() doesn't actually remove the node @@ -159,9 +153,20 @@ static PyObject *astar(PyObject *self, PyObject *args) { float new_cost = costs[cur.idx] + weights[nbrs[i]]; if (new_cost < costs[nbrs[i]]) { // estimate the cost to the goal based on legal moves - heuristic_cost = heuristic_func(nbrs[i] / w, nbrs[i] % w, - goal / w, goal % w, - start / w, start % w); + // Get the heuristic method to use + if (heuristic_override == DEFAULT) { + if (diag_ok) { + heuristic_cost = linf_norm(nbrs[i] / w, nbrs[i] % w, + goal_i, goal_j); + } else { + heuristic_cost = l1_norm(nbrs[i] / w, nbrs[i] % w, + goal_i, goal_j); + } + } else { + heuristic_cost = heuristic_func(nbrs[i] / w, nbrs[i] % w, + goal_i, goal_j, + start_i, start_j); + } // paths with lower expected cost are explored first float priority = new_cost + heuristic_cost; From 56cf2a8eb872505ef1c502bc76eb64850b3b97b6 Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Mon, 9 May 2022 09:48:10 +1000 Subject: [PATCH 15/16] moved experimental heuristics to its own file --- setup.py | 7 ++-- src/cpp/astar.cpp | 52 +++-------------------------- src/cpp/experimental_heuristics.cpp | 41 +++++++++++++++++++++++ src/cpp/experimental_heuristics.h | 20 +++++++++++ 4 files changed, 71 insertions(+), 49 deletions(-) create mode 100644 src/cpp/experimental_heuristics.cpp create mode 100644 src/cpp/experimental_heuristics.h diff --git a/setup.py b/setup.py index b041648..8fe5916 100644 --- a/setup.py +++ b/setup.py @@ -5,8 +5,11 @@ import numpy astar_module = Extension( - 'pyastar2d.astar', sources=['src/cpp/astar.cpp'], - include_dirs=[numpy.get_include()], # for numpy/arrayobject.h + 'pyastar2d.astar', sources=['src/cpp/astar.cpp', 'src/cpp/experimental_heuristics.cpp'], + include_dirs=[ + numpy.get_include(), # for numpy/arrayobject.h + 'src/cpp' # for experimental_heuristics.h + ], extra_compile_args=["-O3", "-Wall", "-shared", "-fpic"], ) diff --git a/src/cpp/astar.cpp b/src/cpp/astar.cpp index f82d476..94ab3a1 100644 --- a/src/cpp/astar.cpp +++ b/src/cpp/astar.cpp @@ -4,6 +4,7 @@ #include #include #include +#include const float INF = std::numeric_limits::infinity(); @@ -24,8 +25,6 @@ bool operator<(const Node &n1, const Node &n2) { return n1.cost > n2.cost; } -enum Heuristic { DEFAULT, ORTHOGONAL_X, ORTHOGONAL_Y }; - // See for various grid heuristics: // http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#S7 // L_\inf norm (diagonal distance) @@ -38,44 +37,6 @@ inline float l1_norm(int i0, int j0, int i1, int j1) { return std::abs(i0 - i1) + std::abs(j0 - j1); } -// Please note below heuristic is experimental and only for pretty lines -// Orthogonal x (moves by x first, then half way by y) -inline float orthogonal_x(int i0, int j0, int i1, int j1, int i2, int j2) { - int di = std::abs(i0 - i1); - int dim = std::abs(i1 - i2); - int djm = std::abs(j1 - j2); - if (di > (dim * 0.5)) { - return di + djm; - } else { - return std::abs(j0 - j1); - } -} - -// Please note below heuristic is experimental and only for pretty lines -// Orthogonal y (moves by y first, then half way by x) -inline float orthogonal_y(int i0, int j0, int i1, int j1, int i2, int j2) { - int dj = std::abs(j0 - j1); - int djm = std::abs(j1 - j2); - int dim = std::abs(i1 - i2); - if (dj > (djm * 0.5)) { - return dj + dim; - } else { - return std::abs(i0 - i1); - } -} - -typedef float (*heuristic_ptr)(int, int, int, int, int, int); - -inline heuristic_ptr select_heuristic(int h) { - switch (h) { - case ORTHOGONAL_X: - return orthogonal_x; - case ORTHOGONAL_Y: - return orthogonal_y; - default: - return NULL; - } -} // weights: flattened h x w grid of costs // h, w: height and width of grid @@ -156,16 +117,13 @@ static PyObject *astar(PyObject *self, PyObject *args) { // Get the heuristic method to use if (heuristic_override == DEFAULT) { if (diag_ok) { - heuristic_cost = linf_norm(nbrs[i] / w, nbrs[i] % w, - goal_i, goal_j); + heuristic_cost = linf_norm(nbrs[i] / w, nbrs[i] % w, goal_i, goal_j); } else { - heuristic_cost = l1_norm(nbrs[i] / w, nbrs[i] % w, - goal_i, goal_j); + heuristic_cost = l1_norm(nbrs[i] / w, nbrs[i] % w, goal_i, goal_j); } } else { - heuristic_cost = heuristic_func(nbrs[i] / w, nbrs[i] % w, - goal_i, goal_j, - start_i, start_j); + heuristic_cost = heuristic_func( + nbrs[i] / w, nbrs[i] % w, goal_i, goal_j, start_i, start_j); } // paths with lower expected cost are explored first diff --git a/src/cpp/experimental_heuristics.cpp b/src/cpp/experimental_heuristics.cpp new file mode 100644 index 0000000..54ad6d0 --- /dev/null +++ b/src/cpp/experimental_heuristics.cpp @@ -0,0 +1,41 @@ +// Please note below heuristics are experimental and only for pretty lines. +// They may not take the shortest path and require additional cpu cycles. + +#include +#include + + +heuristic_ptr select_heuristic(int h) { + switch (h) { + case ORTHOGONAL_X: + return orthogonal_x; + case ORTHOGONAL_Y: + return orthogonal_y; + default: + return NULL; + } +} + +// Orthogonal x (moves by x first, then half way by y) +float orthogonal_x(int i0, int j0, int i1, int j1, int i2, int j2) { + int di = std::abs(i0 - i1); + int dim = std::abs(i1 - i2); + int djm = std::abs(j1 - j2); + if (di > (dim * 0.5)) { + return di + djm; + } else { + return std::abs(j0 - j1); + } +} + +// Orthogonal y (moves by y first, then half way by x) +float orthogonal_y(int i0, int j0, int i1, int j1, int i2, int j2) { + int dj = std::abs(j0 - j1); + int djm = std::abs(j1 - j2); + int dim = std::abs(i1 - i2); + if (dj > (djm * 0.5)) { + return dj + dim; + } else { + return std::abs(i0 - i1); + } +} diff --git a/src/cpp/experimental_heuristics.h b/src/cpp/experimental_heuristics.h new file mode 100644 index 0000000..98c0562 --- /dev/null +++ b/src/cpp/experimental_heuristics.h @@ -0,0 +1,20 @@ +// Please note below heuristics are experimental and only for pretty lines. +// They may not take the shortest path and require additional cpu cycles. + +#ifndef EXPERIMENTAL_HEURISTICS_H_ +#define EXPERIMENTAL_HEURISTICS_H_ + + +enum Heuristic { DEFAULT, ORTHOGONAL_X, ORTHOGONAL_Y }; + +typedef float (*heuristic_ptr)(int, int, int, int, int, int); + +heuristic_ptr select_heuristic(int); + +// Orthogonal x (moves by x first, then half way by y) +float orthogonal_x(int, int, int, int, int, int); + +// Orthogonal y (moves by y first, then half way by x) +float orthogonal_y(int, int, int, int, int, int); + +#endif From 6de6373185277b8e617b6cbdae5bf82777e22853 Mon Sep 17 00:00:00 2001 From: "Chen, Peter" Date: Mon, 9 May 2022 09:54:29 +1000 Subject: [PATCH 16/16] added missing include for linux --- src/cpp/experimental_heuristics.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/cpp/experimental_heuristics.cpp b/src/cpp/experimental_heuristics.cpp index 54ad6d0..c6a5a17 100644 --- a/src/cpp/experimental_heuristics.cpp +++ b/src/cpp/experimental_heuristics.cpp @@ -2,6 +2,7 @@ // They may not take the shortest path and require additional cpu cycles. #include +#include #include