summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-07 13:28:06 +0200
committerNicolas Schodet2012-05-11 00:47:24 +0200
commitfc29c40de5516d5b57e7d07f882655610b711fd2 (patch)
tree8ef5c79807a4b0bc01bb7ceba76570c77d812712
parent268ed66583490f95faa7b80496474df44767353a (diff)
digital/io-hub/src/guybrush: optimize path earlier
-rw-r--r--digital/io-hub/src/guybrush/path.c61
1 files changed, 39 insertions, 22 deletions
diff --git a/digital/io-hub/src/guybrush/path.c b/digital/io-hub/src/guybrush/path.c
index 735982ba..a1e0c2be 100644
--- a/digital/io-hub/src/guybrush/path.c
+++ b/digital/io-hub/src/guybrush/path.c
@@ -333,6 +333,42 @@ path_decay (void)
}
}
+/** Remove useless nodes (colinear nodes). */
+static void
+path_optimize (void)
+{
+ uint8_t cur = path.get, next, next2;
+ vect_t curp, nextp, next2p;
+ path_pos (cur, &curp);
+ next = path.astar_nodes[cur].prev;
+ if (next == 0xff || next == PATH_DST_NODE_INDEX)
+ return;
+ path_pos (next, &nextp);
+ /* Look at three points, if they are aligned, remove the center point. */
+ while (1)
+ {
+ /* Nodes: cur ---> next ---> next2. Do not be fooled by the "prev"
+ * name, astar is feed backward. */
+ next2 = path.astar_nodes[next].prev;
+ if (next2 == 0xff || next2 == PATH_DST_NODE_INDEX)
+ break;
+ path_pos (next2, &next2p);
+ vect_t vnp = nextp; vect_sub (&vnp, &curp);
+ vect_t vn2p = next2p; vect_sub (&vn2p, &curp);
+ if (vect_normal_dot_product (&vnp, &vn2p) == 0)
+ {
+ path.astar_nodes[cur].prev = path.astar_nodes[next].prev;
+ }
+ else
+ {
+ cur = next;
+ curp = nextp;
+ }
+ next = next2;
+ nextp = next2p;
+ }
+}
+
void
path_update (void)
{
@@ -340,6 +376,8 @@ path_update (void)
path.found = astar (path.astar_nodes, PATH_NODES_NB, PATH_DST_NODE_INDEX,
PATH_SRC_NODE_INDEX);
path.get = PATH_SRC_NODE_INDEX;
+ if (path.found)
+ path_optimize ();
#if AC_PATH_REPORT
if (path.found)
{
@@ -359,31 +397,10 @@ path_get_next (vect_t *p)
{
if (path.found)
{
- assert (path.get != PATH_DST_NODE_INDEX);
- uint8_t prev = path.get;
- vect_t pp;
- path_pos (prev, &pp);
+ assert (path.get != PATH_DST_NODE_INDEX && path.get < PATH_NODES_NB);
uint8_t next = path.astar_nodes[path.get].prev;
path.get = next;
path_pos (next, p);
- while (next != 0xff)
- {
- /* Try to remove useless points. */
- uint8_t next = path.astar_nodes[path.get].prev;
- if (next == 0xff || next == PATH_DST_NODE_INDEX)
- break;
- vect_t np;
- path_pos (next, &np);
- vect_t vnp = np; vect_sub (&vnp, &pp);
- vect_t vp = *p; vect_sub (&vp, &pp);
- if (vect_normal_dot_product (&vp, &vnp) == 0)
- {
- path.get = next;
- *p = np;
- }
- else
- break;
- }
return 1;
}
else