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)
{
- std::vector<RRTEdge *> bedges;
- float radi = this->r() / cos(M_PI / 4); // TODO 4 is square
- float angl = 2 * M_PI / 4;
- float x1;
- float y1;
- float x2;
- float y2;
- int i;
- for (i = 0; i < 4; i++) {
- x1 = radi * cos(i * angl);
- y1 = radi * sin(i * angl);
- x2 = radi * cos((i + 1) * angl);
- y2 = radi * sin((i + 1) * angl);
- x1 += this->x();
- y1 += this->y();
- x2 += this->x();
- y2 += this->y();
- bedges.push_back(new RRTEdge(
- new RRTNode(x1, y1, 0),
- new RRTNode(x2, y2, 0)));
- }
- for (auto &be: bedges) {
- for (auto &e: edges) {
- if (SegmentObstacle(
- be->init(),
- be->goal())
- .collide(e)) {
- for (auto e: bedges) {
- delete e->init();
- delete e->goal();
- delete e;
- }
- return true;
- }
- }
- }
- for (auto e: bedges) {
- delete e->init();
- delete e->goal();
- delete e;
- }
+ for (auto e: edges)
+ if (this->collide(e))
+ return true;
return false;
}
return (float) (sqrt(xx + yy) - this->r());
}
-bool SegmentObstacle::collide(RRTNode *n)
+void PolygonObstacle::add_bnode(RRTNode *n)
+{
+ this->bnodes_.push_back(n);
+}
+
+bool PolygonObstacle::collide(RRTNode *n)
+{
+ // From substack/point-in-polygon, see
+ // https://github.com/substack/point-in-polygon/blob/master/index.js
+ bool inside = false;
+ unsigned int i;
+ int j;
+ for (i = 0, j = this->bnodes_.size() - 1;
+ i < this->bnodes_.size();
+ j = i++) {
+ float xi = this->bnodes_[i]->x();
+ float yi = this->bnodes_[i]->y();
+ float xj = this->bnodes_[j]->x();
+ float yj = this->bnodes_[j]->y();
+ bool intersect = ((yi > n->y()) != (yj > n->y())) &&
+ (n->x() < (xj - xi) * (n->y() - yi) / (yj - yi) + xi);
+ if (intersect)
+ inside = !inside;
+ }
+ return inside;
+}
+
+bool PolygonObstacle::collide(RRTEdge *e)
{
+ for (auto &f: this->frame()) {
+ if (SegmentObstacle(f->init(), f->goal()).collide(e))
+ return true;
+ }
return false;
}
-bool SegmentObstacle::collide(RRTEdge *e)
+bool PolygonObstacle::collide(std::vector<RRTEdge *> &edges)
{
- // 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 false; // parallel
+ for (auto &e: edges) {
+ if (this->collide(e))
+ return true;
}
- //return true; // colliding lines, not line segments
- //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;
- float s = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
- s /= deno;
- if (s < 0 || s > 1) {
- return false;
+ return false;
+}
+
+bool PolygonObstacle::collide(std::vector<RRTEdge *> edges)
+{
+ for (auto &e: edges) {
+ if (this->collide(e))
+ return true;
}
- float t = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
- t *= -1;
- t /= deno;
- if (t < 0 || t > 1) {
- return false;
+ return false;
+}
+
+float PolygonObstacle::dist_to(RRTNode *n)
+{
+ return 0;
+}
+
+std::vector<RRTEdge *> PolygonObstacle::frame()
+{
+ std::vector<RRTEdge *> frame;
+ unsigned int i;
+ int j;
+ for (i = 1, j = 0; i < this->bnodes_.size(); j = i++) {
+ frame.push_back(new RRTEdge(
+ new RRTNode(
+ this->bnodes_[j]->x(),
+ this->bnodes_[j]->y(),
+ this->bnodes_[j]->h()
+ ),
+ new RRTNode(
+ this->bnodes_[i]->x(),
+ this->bnodes_[i]->y(),
+ this->bnodes_[i]->h()
+ )
+ ));
}
- return true;
+ return frame;
+}
+
+std::vector<RRTNode *> &PolygonObstacle::bnodes()
+{
+ return this->bnodes_;
+}
+
+bool SegmentObstacle::collide(RRTNode *n)
+{
+ return false;
+}
+
+bool SegmentObstacle::collide(RRTEdge *e)
+{
+ if (this->intersect(e, true))
+ return true;
+ return false;
}
bool SegmentObstacle::collide(std::vector<RRTEdge *> &edges)