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 BicycleCar *BicycleCar::bcparent() const
187 return this->bcparent_;
191 bool BicycleCar::speed(float s)
197 bool BicycleCar::steer(float s)
203 void BicycleCar::bcparent(BicycleCar *bcp)
205 this->bcparent_ = bcp;
209 std::vector<RRTEdge *> BicycleCar::frame()
211 std::vector<RRTEdge *> frame;
212 frame.push_back(new RRTEdge(
213 new RRTNode(this->lfx(), this->lfy()),
214 new RRTNode(this->lrx(), this->lry())));
215 frame.push_back(new RRTEdge(
216 new RRTNode(this->lrx(), this->lry()),
217 new RRTNode(this->rrx(), this->rry())));
218 frame.push_back(new RRTEdge(
219 new RRTNode(this->rrx(), this->rry()),
220 new RRTNode(this->rfx(), this->rfy())));
221 frame.push_back(new RRTEdge(
222 new RRTNode(this->rfx(), this->rfy()),
223 new RRTNode(this->lfx(), this->lfy())));
227 bool BicycleCar::next()
229 if (this->steer_ > MAXSTEER(this->wheel_base_, this->turning_radius_))
230 this->steer_ = MAXSTEER(
232 this->turning_radius_);
233 if (this->steer_ < -MAXSTEER(this->wheel_base_, this->turning_radius_))
234 this->steer_ = -MAXSTEER(
236 this->turning_radius_);
237 this->h_ += (this->speed_ / this->wheel_base_) * tan(this->steer_);
238 this->x_ += this->speed_ * cos(this->h_);
239 this->y_ += this->speed_ * sin(this->h_);
243 bool BicycleCar::next(float speed, float steer)
245 this->speed_ = speed;
246 this->steer_ = steer;
250 float BicycleCar::diag_radi()
252 float xx = pow((this->length_ + this->wheel_base_) / 2, 2);
253 float yy = pow(this->width_ / 2, 2);
254 return pow(xx + yy, 0.5);
257 float BicycleCar::diag_rradi()
259 float xx = pow(this->dr(), 2);
260 float yy = pow(this->width_ / 2, 2);
261 return pow(xx + yy, 0.5);
264 float BicycleCar::out_radi()
266 return pow((pow(this->turning_radius_ + this->width_ / 2, 2) +
267 pow((this->length_ + this->wheel_base_) / 2, 2)), 0.5);
270 float BicycleCar::alfa()
272 return asin((this->length_ + this->wheel_base_) /
273 (2 * this->out_radi()));
276 bool BicycleCar::drivable(RRTNode *n)
278 bool drivable = true;
279 RRTNode *ccl = this->ccl();
280 RRTNode *ccr = this->ccr();
281 if (pow(ccl->x() - n->x(), 2) + pow(ccl->y() - n->y(), 2) <=
282 pow(this->turning_radius_, 2))
284 if (pow(ccr->x() - n->x(), 2) + pow(ccr->y() - n->y(), 2) <=
285 pow(this->turning_radius_, 2))
290 BicycleCar *BicycleCar::move(RRTNode *c, float dh)
294 float px = this->x();
295 float py = this->y();
298 float nx = px * cos(dh) - py * sin(dh);
299 float ny = px * sin(dh) + py * cos(dh);
302 float ph = this->h();
304 return new BicycleCar(px, py, ph);