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/>.
22 float BicycleCar::length()
27 float BicycleCar::wheelbase()
29 return this->wheel_base_;
32 float BicycleCar::width()
37 float BicycleCar::dr()
39 return (this->length_ - this->wheel_base_) / 2;
42 float BicycleCar::df()
44 return this->length_ - this->dr();
47 float BicycleCar::lfx()
50 float lfx = this->x();
51 lfx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
52 lfx += this->df() * cos(this->h());
53 lfx += this->safety_dist_ * cos(this->h());
57 float BicycleCar::lrx()
59 float lrx = this->x();
60 lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
61 lrx += -this->dr() * cos(this->h());
62 lrx += -this->safety_dist_ * cos(this->h());
66 float BicycleCar::rrx()
68 float rrx = this->x();
69 rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
70 rrx += -this->dr() * cos(this->h());
71 rrx += -this->safety_dist_ * cos(this->h());
75 float BicycleCar::rfx()
77 float rfx = this->x();
78 rfx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
79 rfx += this->df() * cos(this->h());
80 rfx += this->safety_dist_ * cos(this->h());
84 float BicycleCar::lfy()
86 float lfy = this->y();
87 lfy += (this->width_ / 2) * sin(this->h() + M_PI / 2);
88 lfy += this->df() * sin(this->h());
89 lfy += this->safety_dist_ * sin(this->h());
93 float BicycleCar::lry()
95 float lry = this->y();
96 lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
97 lry += -this->dr() * sin(this->h());
98 lry += -this->safety_dist_ * sin(this->h());
102 float BicycleCar::rry()
104 float rry = this->y();
105 rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
106 rry += -this->dr() * sin(this->h());
107 rry += -this->safety_dist_ * sin(this->h());
111 float BicycleCar::rfy()
113 float rfy = this->y();
114 rfy += (this->width_ / 2) * sin(this->h() - M_PI / 2);
115 rfy += this->df() * sin(this->h());
116 rfy += this->safety_dist_ * sin(this->h());
120 float BicycleCar::lwbx()
122 float lrx = this->x();
123 lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
126 float BicycleCar::lwby()
128 float lry = this->y();
129 lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
133 float BicycleCar::rwbx()
135 float rrx = this->x();
136 rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
140 float BicycleCar::rwby()
142 float rry = this->y();
143 rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
147 RRTNode *BicycleCar::ccl()
149 RRTEdge *l1 = new RRTEdge(
150 new RRTNode(this->lwbx(), this->lwby()),
151 new RRTNode(this->rwbx(), this->rwby()));
152 float d = this->width_ * tan(this->alfa());
153 float x = this->lfx() + d * cos(this->h() + M_PI);
154 float y = this->lfy() + d * sin(this->h() + M_PI);
155 RRTEdge *l2 = new RRTEdge(
156 new RRTNode(x, y, 0),
157 new RRTNode(this->rfx(), this->rfy(), 0));
158 return l1->intersect(l2, false);
161 RRTNode *BicycleCar::ccr()
163 RRTEdge *l1 = new RRTEdge(
164 new RRTNode(this->lwbx(), this->lwby()),
165 new RRTNode(this->rwbx(), this->rwby()));
166 float d = this->width_ * tan(this->alfa());
167 float x = this->rfx() + d * cos(this->h() - M_PI);
168 float y = this->rfy() + d * sin(this->h() - M_PI);
169 RRTEdge *l2 = new RRTEdge(
170 new RRTNode(this->lfx(), this->lfy(), 0),
171 new RRTNode(x, y, 0));
172 return l1->intersect(l2, false);
175 float BicycleCar::speed()
180 float BicycleCar::steer()
185 bool BicycleCar::speed(float s)
191 bool BicycleCar::steer(float s)
197 std::vector<RRTEdge *> BicycleCar::frame()
199 std::vector<RRTEdge *> frame;
200 frame.push_back(new RRTEdge(
201 new RRTNode(this->lfx(), this->lfy()),
202 new RRTNode(this->lrx(), this->lry())));
203 frame.push_back(new RRTEdge(
204 new RRTNode(this->lrx(), this->lry()),
205 new RRTNode(this->rrx(), this->rry())));
206 frame.push_back(new RRTEdge(
207 new RRTNode(this->rrx(), this->rry()),
208 new RRTNode(this->rfx(), this->rfy())));
209 frame.push_back(new RRTEdge(
210 new RRTNode(this->rfx(), this->rfy()),
211 new RRTNode(this->lfx(), this->rfy())));
215 bool BicycleCar::next()
217 if (this->steer_ > MAXSTEER(this->wheel_base_, this->turning_radius_))
218 this->steer_ = MAXSTEER(
220 this->turning_radius_);
221 if (this->steer_ < -MAXSTEER(this->wheel_base_, this->turning_radius_))
222 this->steer_ = -MAXSTEER(
224 this->turning_radius_);
225 this->h_ += (this->speed_ / this->wheel_base_) * tan(this->steer_);
226 this->x_ += this->speed_ * cos(this->h_);
227 this->y_ += this->speed_ * sin(this->h_);
231 bool BicycleCar::next(float speed, float steer)
233 this->speed_ = speed;
234 this->steer_ = steer;
238 float BicycleCar::diag_radi()
240 float xx = pow((this->length_ + this->wheel_base_) / 2, 2);
241 float yy = pow(this->width_ / 2, 2);
242 return pow(xx + yy, 0.5);
245 float BicycleCar::out_radi()
247 return pow((pow(this->turning_radius_ + this->width_ / 2, 2) +
248 pow((this->length_ + this->wheel_base_) / 2, 2)), 0.5);
251 float BicycleCar::alfa()
253 return asin((this->length_ + this->wheel_base_) /
254 (2 * this->out_radi()));
257 bool BicycleCar::drivable(RRTNode *n)
259 bool drivable = true;
260 RRTNode *ccl = this->ccl();
261 RRTNode *ccr = this->ccr();
262 if (pow(ccl->x() - n->x(), 2) + pow(ccl->y() - n->y(), 2) <=
263 pow(this->turning_radius_, 2))
265 if (pow(ccr->x() - n->x(), 2) + pow(ccr->y() - n->y(), 2) <=
266 pow(this->turning_radius_, 2))
271 BicycleCar *BicycleCar::move(RRTNode *c, float dh)
275 float px = this->x();
276 float py = this->y();
279 float nx = px * cos(dh) - py * sin(dh);
280 float ny = px * sin(dh) + py * cos(dh);
283 float ph = this->h();
285 return new BicycleCar(px, py, ph);