]> rtime.felk.cvut.cz Git - hubacji1/bcar.git/blob - src/bcar.cc
Merge branch 'feature/collide-function'
[hubacji1/bcar.git] / src / bcar.cc
1 #include <cmath>
2 #include "bcar.h"
3
4 // kinematic constraints
5 bool BicycleCar::drivable(const BicycleCar &bc) const
6 {
7         if (
8                 pow(this->ccl().x() - bc.x(), 2)
9                 + pow(this->ccl().y() - bc.y(), 2)
10                 <= pow(this->mtr(), 2)
11         )
12                 return false;
13         if (
14                 pow(this->ccr().x() - bc.x(), 2)
15                 + pow(this->ccr().y() - bc.y(), 2)
16                 <= pow(this->mtr(), 2)
17         )
18                 return false;
19         return true;
20 }
21
22 double BicycleCar::iradi() const
23 {
24         return this->mtr() - this->w() / 2;
25 }
26
27 double BicycleCar::ofradi() const
28 {
29         return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->df(), 2));
30 }
31
32 double BicycleCar::orradi() const
33 {
34         return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->dr(), 2));
35 }
36
37 // car frame
38 double BicycleCar::lfx() const
39 {
40         double lfx = this->x();
41         lfx += (this->w() / 2) * cos(this->h() + M_PI / 2);
42         lfx += this->df() * cos(this->h());
43         lfx += this->sd() * cos(this->h());
44         return lfx;
45 }
46
47 double BicycleCar::lfy() const
48 {
49         double lfy = this->y();
50         lfy += (this->w() / 2) * sin(this->h() + M_PI / 2);
51         lfy += this->df() * sin(this->h());
52         lfy += this->sd() * sin(this->h());
53         return lfy;
54 }
55
56 double BicycleCar::lrx() const
57 {
58         double lrx = this->x();
59         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
60         lrx += -this->dr() * cos(this->h());
61         lrx += -this->sd() * cos(this->h());
62         return lrx;
63 }
64
65 double BicycleCar::lry() const
66 {
67         double lry = this->y();
68         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
69         lry += -this->dr() * sin(this->h());
70         lry += -this->sd() * sin(this->h());
71         return lry;
72 }
73
74 double BicycleCar::rrx() const
75 {
76         double rrx = this->x();
77         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
78         rrx += -this->dr() * cos(this->h());
79         rrx += -this->sd() * cos(this->h());
80         return rrx;
81 }
82
83 double BicycleCar::rry() const
84 {
85         double rry = this->y();
86         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
87         rry += -this->dr() * sin(this->h());
88         rry += -this->sd() * sin(this->h());
89         return rry;
90 }
91
92 double BicycleCar::rfx() const
93 {
94         double rfx = this->x();
95         rfx += (this->w() / 2) * cos(this->h() - M_PI / 2);
96         rfx += this->df() * cos(this->h());
97         rfx += this->sd() * cos(this->h());
98         return rfx;
99 }
100
101 double BicycleCar::rfy() const
102 {
103         double rfy = this->y();
104         rfy += (this->w() / 2) * sin(this->h() - M_PI / 2);
105         rfy += this->df() * sin(this->h());
106         rfy += this->sd() * sin(this->h());
107         return rfy;
108 }
109
110 double BicycleCar::ralx() const
111 {
112         double lrx = this->x();
113         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
114         return lrx;
115 }
116 double BicycleCar::raly() const
117 {
118         double lry = this->y();
119         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
120         return lry;
121 }
122
123 double BicycleCar::rarx() const
124 {
125         double rrx = this->x();
126         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
127         return rrx;
128 }
129
130 double BicycleCar::rary() const
131 {
132         double rry = this->y();
133         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
134         return rry;
135 }
136
137 BicycleCar BicycleCar::ccl() const
138 {
139         BicycleCar bc;
140         bc.x(this->x() + this->mtr() * cos(this->h() + M_PI / 2));
141         bc.y(this->y() + this->mtr() * sin(this->h() + M_PI / 2));
142         bc.h(this->h());
143         return bc;
144 }
145
146 BicycleCar BicycleCar::ccr() const
147 {
148         BicycleCar bc;
149         bc.x(this->x() + this->mtr() * cos(this->h() - M_PI / 2));
150         bc.y(this->y() + this->mtr() * sin(this->h() - M_PI / 2));
151         bc.h(this->h());
152         return bc;
153 }
154
155 // moving
156 void BicycleCar::next()
157 {
158         if (this->st() > this->wb() / this->mtr())
159                 this->st(this->wb() / this->mtr());
160         if (this->st() < -this->wb() / this->mtr())
161                 this->st(-this->wb() / this->mtr());
162         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
163         this->x(this->x() + this->sp() * cos(this->h()));
164         this->y(this->y() + this->sp() * sin(this->h()));
165 }
166
167 BicycleCar::BicycleCar()
168 {
169 }
170
171 std::tuple<bool, unsigned int, unsigned int> collide(
172         std::vector<std::tuple<double, double>> p1,
173         std::vector<std::tuple<double, double>> p2
174 )
175 {
176         for (unsigned int i = 0; i < p1.size() - 1; i++) {
177                 for (unsigned int j = 0; j < p2.size() - 1; j++) {
178                         auto x = intersect(
179                                 std::get<0>(p1[i]),
180                                 std::get<1>(p1[i]),
181                                 std::get<0>(p1[i + 1]),
182                                 std::get<1>(p1[i + 1]),
183                                 std::get<0>(p2[j]),
184                                 std::get<1>(p2[j]),
185                                 std::get<0>(p2[j + 1]),
186                                 std::get<1>(p2[j + 1])
187                         );
188                         if (std::get<0>(x))
189                                 return std::make_tuple(true, i, j);
190                 }
191         }
192         return std::make_tuple(false, 0, 0);
193 }
194
195 bool inside(double x, double y, std::vector<std::tuple<double, double>> poly)
196 {
197         unsigned int i = 0;
198         unsigned int j = 3;
199         bool inside = false;
200         for (i = 0; i < 4; i++) {
201                 if (
202                         (std::get<1>(poly[i]) > y) != (std::get<1>(poly[j]) > y)
203                         && (
204                                 x < std::get<0>(poly[i])
205                                 + (std::get<0>(poly[j]) - std::get<0>(poly[i]))
206                                 * (y - std::get<1>(poly[i]))
207                                 / (std::get<1>(poly[j]) - std::get<1>(poly[i]))
208                         )
209                 )
210                         inside = !inside;
211                 j = i;
212         }
213         return inside;
214 }
215
216 std::tuple<bool, double, double> intersect(
217         double x1, double y1,
218         double x2, double y2,
219         double x3, double y3,
220         double x4, double y4
221 )
222 {
223         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
224         if (deno == 0)
225                 return std::make_tuple(false, 0, 0);
226         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
227         t /= deno;
228         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
229         u *= -1;
230         u /= deno;
231         if (t < 0 || t > 1 || u < 0 || u > 1)
232                 return std::make_tuple(false, 0, 0);
233         return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
234 }