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 false;
}
+bool PolygonObstacle::collide(std::vector<RRTEdge *> edges)
+{
+ for (auto &e: edges) {
+ if (this->collide(e))
+ return true;
+ }
+ return false;
+}
+
float PolygonObstacle::dist_to(RRTNode *n)
{
return 0;
bool SegmentObstacle::collide(RRTEdge *e)
{
- // 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
- }
- //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;
- }
- float t = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
- t *= -1;
- t /= deno;
- if (t < 0 || t > 1) {
- return false;
- }
- return true;
+ if (this->intersect(e, true))
+ return true;
+ return false;
}
bool SegmentObstacle::collide(std::vector<RRTEdge *> &edges)