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 std::vector<RRTEdge *> edges;
43 return this->collide(edges);
46 bool CircleObstacle::collide(std::vector<RRTEdge *> &edges)
48 std::vector<RRTEdge *> bedges;
49 float radi = this->r() / cos(M_PI / 4); // TODO 4 is square
50 float angl = 2 * M_PI / 4;
56 for (i = 0; i < 4; i++) {
57 x1 = radi * cos(i * angl);
58 y1 = radi * sin(i * angl);
59 x2 = radi * cos((i + 1) * angl);
60 y2 = radi * sin((i + 1) * angl);
65 bedges.push_back(new RRTEdge(
66 new RRTNode(x1, y1, 0),
67 new RRTNode(x2, y2, 0)));
69 for (auto &be: bedges) {
70 for (auto &e: edges) {
75 for (auto e: bedges) {
84 for (auto e: bedges) {
92 float CircleObstacle::dist_to(RRTNode *n)
94 float xx = n->x() - this->x();
96 float yy = n->y() - this->y();
98 return (float) (sqrt(xx + yy) - this->r());
101 void PolygonObstacle::add_bnode(RRTNode *n)
103 this->bnodes_.push_back(n);
106 bool PolygonObstacle::collide(RRTNode *n)
108 // From substack/point-in-polygon, see
109 // https://github.com/substack/point-in-polygon/blob/master/index.js
113 for (i = 0, j = this->bnodes_.size() - 1;
114 i < this->bnodes_.size();
116 float xi = this->bnodes_[i]->x();
117 float yi = this->bnodes_[i]->y();
118 float xj = this->bnodes_[j]->x();
119 float yj = this->bnodes_[j]->y();
120 bool intersect = ((yi > n->y()) != (yj > n->y())) &&
121 (n->x() < (xj - xi) * (n->y() - yi) / (yj - yi) + xi);
128 bool PolygonObstacle::collide(RRTEdge *e)
133 bool PolygonObstacle::collide(std::vector<RRTEdge *> &edges)
138 float PolygonObstacle::dist_to(RRTNode *n)
143 std::vector<RRTEdge *> PolygonObstacle::frame()
145 std::vector<RRTEdge *> frame;
148 for (i = 1, j = 0; i < this->bnodes_.size(); j = i++) {
149 frame.push_back(new RRTEdge(
151 this->bnodes_[j]->x(),
152 this->bnodes_[j]->y(),
153 this->bnodes_[j]->h()
156 this->bnodes_[i]->x(),
157 this->bnodes_[i]->y(),
158 this->bnodes_[i]->h()
165 std::vector<RRTNode *> &PolygonObstacle::bnodes()
167 return this->bnodes_;
170 bool SegmentObstacle::collide(RRTNode *n)
175 bool SegmentObstacle::collide(RRTEdge *e)
177 // see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
178 float x1 = this->init()->x();
179 float y1 = this->init()->y();
180 float x2 = this->goal()->x();
181 float y2 = this->goal()->y();
182 float x3 = e->init()->x();
183 float y3 = e->init()->y();
184 float x4 = e->goal()->x();
185 float y4 = e->goal()->y();
186 float deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
188 return false; // parallel
190 //return true; // colliding lines, not line segments
191 //float px = (x1 * y2 - y1 * x2) * (x3 - x4) -
192 // (x1 - x2) * (x3 * y4 - y3 * x4);
194 //float py = (x1 * y2 - y1 * x2) * (y3 - y4) -
195 // (y1 - y2) * (x3 * y4 - y3 * x4);
197 float s = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
199 if (s < 0 || s > 1) {
202 float t = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
205 if (t < 0 || t > 1) {
211 bool SegmentObstacle::collide(std::vector<RRTEdge *> &edges)
213 for (auto &e: edges) {
214 if (this->collide(e)) {
221 float SegmentObstacle::dist_to(RRTNode *n)
223 // see https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
224 float x1 = this->init()->x();
225 float y1 = this->init()->y();
226 float x2 = this->goal()->x();
227 float y2 = this->goal()->y();
230 float nume = (y2 - y1) * x0 - (x2 - x1) * y0 + x2 * y1 - y2 * x1;
231 nume = std::abs(nume);
232 float deno = sqrt(pow(y2 - y1, 2) + pow(x2 - x1, 2));