Add RRTEdge intersection method

diff --git a/base/rrtnode.cc b/base/rrtnode.cc
index f77e543837fab1910946436bf28dc98665304225..cba4c5514abfe14aa51b3a43349cdc6665ae66f9 100644 (file)
--- a/base/rrtnode.cc
+++ b/base/rrtnode.cc
@@ -258,3 +258,38 @@ RRTNode *RRTEdge::goal() const
 {
         return this->goal_;
 }
+
+RRTNode *RRTEdge::intersect(RRTEdge *e, bool segment)
+{
+        // see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
+        float x1 = this->init()->x();
+        float y1 = this->init()->y();
+        float x2 = this->goal()->x();
+        float y2 = this->goal()->y();
+        float x3 = e->init()->x();
+        float y3 = e->init()->y();
+        float x4 = e->goal()->x();
+        float y4 = e->goal()->y();
+        float deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
+        if (deno == 0)
+                return nullptr;
+        if (!segment) {
+                float px = (x1 * y2 - y1 * x2) * (x3 - x4) -
+                                (x1 - x2) * (x3 * y4 - y3 * x4);
+                px /= deno;
+                float py = (x1 * y2 - y1 * x2) * (y3 - y4) -
+                                (y1 - y2) * (x3 * y4 - y3 * x4);
+                py /= deno;
+                return new RRTNode(px, py, 0);
+        } else {
+                float t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
+                t /= deno;
+                float u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
+                u *= -1;
+                u /= deno;
+                if (t < 0 || t > 1 || u < 0 || u > 1)
+                        return nullptr;
+                return new RRTNode(x1 + t * (x2 - x1), y1 + t * (y2 - y1), 0);
+        }
+        return nullptr;
+}

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index 6acec4a9f0fd27aacbf4fa340be9e07f25b3825f..fcb9d344049acbb5a94fa8dbde61e14988ce45ea 100644 (file)
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -104,6 +104,8 @@ class RRTEdge {
 
                 RRTNode *init() const;
                 RRTNode *goal() const;
+
+                RRTNode *intersect(RRTEdge *e, bool segment);
 };
 
 #endif