Rewrite circle with line segment collide method

diff --git a/perception/obstacle.cc b/perception/obstacle.cc
index 03a0d8b3984ecb9dce40771f7cf27a885b3ccd8c..43ae78e0ec06224bf797e2f85769a8466b82b768 100644 (file)
--- a/perception/obstacle.cc
+++ b/perception/obstacle.cc
@@ -38,9 +38,45 @@ bool CircleObstacle::collide(RRTNode *n)
 
 bool CircleObstacle::collide(RRTEdge *e)
 {
-        std::vector<RRTEdge *> edges;
-        edges.push_back(e);
-        return this->collide(edges);
+        // see http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
+        // Find the closest point on seq.
+        float seg_v[] = {
+                e->goal()->x() - e->init()->x(),
+                e->goal()->y() - e->init()->y()
+        };
+        float pt_v[] = {
+                this->x() - e->init()->x(),
+                this->y() - e->init()->y()
+        };
+        float seg_vl = sqrt(pow(seg_v[0], 2) + pow(seg_v[1], 2));
+        // seg_vl must be > 0 otherwise it is invalid segment length.
+        if (seg_vl <= 0)
+                return false;
+        float seg_v_unit[] = {seg_v[0] / seg_vl, seg_v[1] / seg_vl};
+        float proj = pt_v[0]*seg_v_unit[0] + pt_v[1]*seg_v_unit[1];
+        float closest[] = {0, 0};
+        if (proj <= 0) {
+                closest[0] = e->init()->x();
+                closest[1] = e->init()->y();
+        } else if (proj >= seg_vl) {
+                closest[0] = e->goal()->x();
+                closest[1] = e->goal()->y();
+        } else {
+                float proj_v[] = {seg_v_unit[0] * proj, seg_v_unit[1] * proj};
+                closest[0] = proj_v[0] + e->init()->x();
+                closest[1] = proj_v[1] + e->init()->y();
+        }
+        // Find the segment circle.
+        float dist_v[] = {this->x() - closest[0], this->y() - closest[1]};
+        float dist = sqrt(pow(dist_v[0], 2) + pow(dist_v[1], 2));
+        if (dist <= this->r())
+                return true;
+        return false;
+        // Offset computation.
+        // float offset[] = {
+        //        dist_v[0] / dist * (BCAR_TURNING_RADIUS - dist),
+        //        dist_v[1] / dist * (BCAR_TURNING_RADIUS - dist)
+        // };
 }
 
 bool CircleObstacle::collide(std::vector<RRTEdge *> &edges)