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 BicycleCar::dr()
23 return (this->length_ - this->wheel_base_) / 2;
26 float BicycleCar::df()
28 return this->length_ - this->dr();
31 float BicycleCar::lfx()
34 float lfx = this->x();
35 lfx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
36 lfx += this->df() * cos(this->h());
37 lfx += this->safety_dist_ * cos(this->h());
41 float BicycleCar::lrx()
43 float lrx = this->x();
44 lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
45 lrx += -this->dr() * cos(this->h());
46 lrx += -this->safety_dist_ * cos(this->h());
50 float BicycleCar::rrx()
52 float rrx = this->x();
53 rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
54 rrx += -this->dr() * cos(this->h());
55 rrx += -this->safety_dist_ * cos(this->h());
59 float BicycleCar::rfx()
61 float rfx = this->x();
62 rfx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
63 rfx += this->df() * cos(this->h());
64 rfx += this->safety_dist_ * cos(this->h());
68 float BicycleCar::lfy()
70 float lfy = this->y();
71 lfy += (this->width_ / 2) * sin(this->h() + M_PI / 2);
72 lfy += this->df() * sin(this->h());
73 lfy += this->safety_dist_ * sin(this->h());
77 float BicycleCar::lry()
79 float lry = this->y();
80 lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
81 lry += -this->dr() * sin(this->h());
82 lry += -this->safety_dist_ * sin(this->h());
86 float BicycleCar::rry()
88 float rry = this->y();
89 rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
90 rry += -this->dr() * sin(this->h());
91 rry += -this->safety_dist_ * sin(this->h());
95 float BicycleCar::rfy()
97 float rfy = this->y();
98 rfy += (this->width_ / 2) * sin(this->h() - M_PI / 2);
99 rfy += this->df() * sin(this->h());
100 rfy += this->safety_dist_ * sin(this->h());
104 float BicycleCar::lwbx()
106 float lrx = this->x();
107 lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
110 float BicycleCar::lwby()
112 float lry = this->y();
113 lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
117 float BicycleCar::rwbx()
119 float rrx = this->x();
120 rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
124 float BicycleCar::rwby()
126 float rry = this->y();
127 rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
131 RRTNode *BicycleCar::ccl()
133 RRTEdge *l1 = new RRTEdge(
134 new RRTNode(this->lwbx(), this->lwby()),
135 new RRTNode(this->rwbx(), this->rwby()));
136 float d = this->width_ * tan(this->alfa());
137 float x = this->lfx() + d * cos(this->h() + M_PI);
138 float y = this->lfy() + d * sin(this->h() + M_PI);
139 RRTEdge *l2 = new RRTEdge(
140 new RRTNode(x, y, 0),
141 new RRTNode(this->rfx(), this->rfy(), 0));
142 return l1->intersect(l2, false);
145 RRTNode *BicycleCar::ccr()
147 RRTEdge *l1 = new RRTEdge(
148 new RRTNode(this->lwbx(), this->lwby()),
149 new RRTNode(this->rwbx(), this->rwby()));
150 float d = this->width_ * tan(this->alfa());
151 float x = this->rfx() + d * cos(this->h() - M_PI);
152 float y = this->rfy() + d * sin(this->h() - M_PI);
153 RRTEdge *l2 = new RRTEdge(
154 new RRTNode(this->lfx(), this->lfy(), 0),
155 new RRTNode(x, y, 0));
156 return l1->intersect(l2, false);
159 float BicycleCar::speed()
164 float BicycleCar::steer()
169 bool BicycleCar::speed(float s)
175 bool BicycleCar::steer(float s)
181 std::vector<RRTEdge *> BicycleCar::frame()
183 std::vector<RRTEdge *> frame;
184 frame.push_back(new RRTEdge(
185 new RRTNode(this->lfx(), this->lfy()),
186 new RRTNode(this->lrx(), this->lry())));
187 frame.push_back(new RRTEdge(
188 new RRTNode(this->lrx(), this->lry()),
189 new RRTNode(this->rrx(), this->rry())));
190 frame.push_back(new RRTEdge(
191 new RRTNode(this->rrx(), this->rry()),
192 new RRTNode(this->rfx(), this->rfy())));
193 frame.push_back(new RRTEdge(
194 new RRTNode(this->rfx(), this->rfy()),
195 new RRTNode(this->lfx(), this->rfy())));
199 bool BicycleCar::next()
201 if (this->steer_ > MAXSTEER(this->wheel_base_, this->turning_radius_))
202 this->steer_ = MAXSTEER(
204 this->turning_radius_);
205 if (this->steer_ < -MAXSTEER(this->wheel_base_, this->turning_radius_))
206 this->steer_ = -MAXSTEER(
208 this->turning_radius_);
209 this->h_ += (this->speed_ / this->wheel_base_) * tan(this->steer_);
210 this->x_ += this->speed_ * cos(this->h_);
211 this->y_ += this->speed_ * sin(this->h_);
215 bool BicycleCar::next(float speed, float steer)
217 this->speed_ = speed;
218 this->steer_ = steer;
222 float BicycleCar::diag_radi()
224 float xx = pow((this->length_ + this->wheel_base_) / 2, 2);
225 float yy = pow(this->width_ / 2, 2);
226 return pow(xx + yy, 0.5);
229 float BicycleCar::out_radi()
231 return pow((pow(this->turning_radius_ + this->width_ / 2, 2) +
232 pow((this->length_ + this->wheel_base_) / 2, 2)), 0.5);
235 float BicycleCar::alfa()
237 return asin((this->length_ + this->wheel_base_) /
238 (2 * this->out_radi()));
241 bool BicycleCar::drivable(RRTNode *n)
243 bool drivable = true;
244 RRTNode *ccl = this->ccl();
245 RRTNode *ccr = this->ccr();
246 if (pow(ccl->x() - n->x(), 2) + pow(ccl->y() - n->y(), 2) <=
247 pow(this->turning_radius_, 2))
249 if (pow(ccr->x() - n->x(), 2) + pow(ccr->y() - n->y(), 2) <=
250 pow(this->turning_radius_, 2))
255 BicycleCar *BicycleCar::move(RRTNode *c, float dh)
259 float px = this->x();
260 float py = this->y();
263 float nx = px * cos(dh) - py * sin(dh);
264 float ny = px * sin(dh) + py * cos(dh);
267 float ph = this->h();
269 return new BicycleCar(px, py, ph);