2 This file is part of I am car.
4 I am car is free software: you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation, either version 3 of the License, or
7 (at your option) any later version.
9 I am car is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
14 You should have received a copy of the GNU General Public License
15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
21 float CircleObstacle::r()
26 bool CircleObstacle::collide(RRTNode *n)
28 float xx = n->x() - this->x();
30 float yy = n->y() - this->y();
32 float rr = this->r() * this->r();
39 bool CircleObstacle::collide(RRTEdge *e)
41 // see http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
42 // Find the closest point on seq.
44 e->goal()->x() - e->init()->x(),
45 e->goal()->y() - e->init()->y()
48 this->x() - e->init()->x(),
49 this->y() - e->init()->y()
51 float seg_vl = sqrt(pow(seg_v[0], 2) + pow(seg_v[1], 2));
52 // seg_vl must be > 0 otherwise it is invalid segment length.
55 float seg_v_unit[] = {seg_v[0] / seg_vl, seg_v[1] / seg_vl};
56 float proj = pt_v[0]*seg_v_unit[0] + pt_v[1]*seg_v_unit[1];
57 float closest[] = {0, 0};
59 closest[0] = e->init()->x();
60 closest[1] = e->init()->y();
61 } else if (proj >= seg_vl) {
62 closest[0] = e->goal()->x();
63 closest[1] = e->goal()->y();
65 float proj_v[] = {seg_v_unit[0] * proj, seg_v_unit[1] * proj};
66 closest[0] = proj_v[0] + e->init()->x();
67 closest[1] = proj_v[1] + e->init()->y();
69 // Find the segment circle.
70 float dist_v[] = {this->x() - closest[0], this->y() - closest[1]};
71 float dist = sqrt(pow(dist_v[0], 2) + pow(dist_v[1], 2));
72 if (dist <= this->r())
75 // Offset computation.
77 // dist_v[0] / dist * (BCAR_TURNING_RADIUS - dist),
78 // dist_v[1] / dist * (BCAR_TURNING_RADIUS - dist)
82 bool CircleObstacle::collide(std::vector<RRTEdge *> &edges)
90 float CircleObstacle::dist_to(RRTNode *n)
92 float xx = n->x() - this->x();
94 float yy = n->y() - this->y();
96 return (float) (sqrt(xx + yy) - this->r());
99 void PolygonObstacle::add_bnode(RRTNode *n)
101 this->bnodes_.push_back(n);
104 bool PolygonObstacle::collide(RRTNode *n)
106 // From substack/point-in-polygon, see
107 // https://github.com/substack/point-in-polygon/blob/master/index.js
111 for (i = 0, j = this->bnodes_.size() - 1;
112 i < this->bnodes_.size();
114 float xi = this->bnodes_[i]->x();
115 float yi = this->bnodes_[i]->y();
116 float xj = this->bnodes_[j]->x();
117 float yj = this->bnodes_[j]->y();
118 bool intersect = ((yi > n->y()) != (yj > n->y())) &&
119 (n->x() < (xj - xi) * (n->y() - yi) / (yj - yi) + xi);
126 bool PolygonObstacle::collide(RRTEdge *e)
128 for (auto &f: this->frame()) {
129 if (SegmentObstacle(f->init(), f->goal()).collide(e))
135 bool PolygonObstacle::collide(std::vector<RRTEdge *> &edges)
137 for (auto &e: edges) {
138 if (this->collide(e))
144 bool PolygonObstacle::collide(std::vector<RRTEdge *> edges)
146 for (auto &e: edges) {
147 if (this->collide(e))
153 float PolygonObstacle::dist_to(RRTNode *n)
158 std::vector<RRTEdge *> PolygonObstacle::frame()
160 std::vector<RRTEdge *> frame;
163 for (i = 1, j = 0; i < this->bnodes_.size(); j = i++) {
164 frame.push_back(new RRTEdge(
166 this->bnodes_[j]->x(),
167 this->bnodes_[j]->y(),
168 this->bnodes_[j]->h()
171 this->bnodes_[i]->x(),
172 this->bnodes_[i]->y(),
173 this->bnodes_[i]->h()
180 std::vector<RRTNode *> &PolygonObstacle::bnodes()
182 return this->bnodes_;
185 bool SegmentObstacle::collide(RRTNode *n)
190 bool SegmentObstacle::collide(RRTEdge *e)
192 if (this->intersect(e, true))
197 bool SegmentObstacle::collide(std::vector<RRTEdge *> &edges)
199 for (auto &e: edges) {
200 if (this->collide(e)) {
207 float SegmentObstacle::dist_to(RRTNode *n)
209 // see https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
210 float x1 = this->init()->x();
211 float y1 = this->init()->y();
212 float x2 = this->goal()->x();
213 float y2 = this->goal()->y();
216 float nume = (y2 - y1) * x0 - (x2 - x1) * y0 + x2 * y1 - y2 * x1;
217 nume = std::abs(nume);
218 float deno = sqrt(pow(y2 - y1, 2) + pow(x2 - x1, 2));