summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-15 08:44:47 +0200
committerNicolas Schodet2010-05-15 08:44:47 +0200
commit9503ce32ceb5b3b00b924db94f4b340b00399e46 (patch)
treeaaba410176eb07e84254f37e65280f6d83faf0fb
parentf9a1a8d5553f0b954e5be31001f282ffbbc7fc4a (diff)
digital/io/src: remove useless path nodeseurobot-2010-5
-rw-r--r--digital/io/src/path.c22
1 files changed, 21 insertions, 1 deletions
diff --git a/digital/io/src/path.c b/digital/io/src/path.c
index 2381a7ce..01a82d11 100644
--- a/digital/io/src/path.c
+++ b/digital/io/src/path.c
@@ -381,11 +381,31 @@ path_get_next (vect_t *p)
{
if (path.found)
{
- /* TODO: remove useless nodes. */
assert (path.get != PATH_DST_NODE_INDEX);
+ uint8_t prev = path.get;
+ vect_t pp;
+ path_pos (prev, &pp);
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