2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
14 WVTEST_MAIN("bcar basic geometry")
27 WVPASSEQ_DOUBLE(bc.len(), bc.df() + bc.dr(), 0.00001);
28 WVPASSEQ_DOUBLE(0.5, bc.lfx(), 0.00001);
29 WVPASSEQ_DOUBLE(0.5, bc.lrx(), 0.00001);
30 WVPASSEQ_DOUBLE(1.5, bc.rrx(), 0.00001);
31 WVPASSEQ_DOUBLE(1.5, bc.rfx(), 0.00001);
32 WVPASSEQ_DOUBLE(3.5, bc.lfy(), 0.00001);
33 WVPASSEQ_DOUBLE(0.5, bc.lry(), 0.00001);
34 WVPASSEQ_DOUBLE(0.5, bc.rry(), 0.00001);
35 WVPASSEQ_DOUBLE(3.5, bc.rfy(), 0.00001);
36 WVPASSEQ_DOUBLE(0.5, bc.lrax(), 0.00001);
37 WVPASSEQ_DOUBLE(1.5, bc.rrax(), 0.00001);
38 WVPASSEQ_DOUBLE(1.0, bc.lray(), 0.00001);
39 WVPASSEQ_DOUBLE(1.0, bc.rray(), 0.00001);
41 // min. turning radius circle centers
42 WVPASSEQ_DOUBLE(bc.h(), M_PI / 2.0, 0.00001);
43 WVPASSEQ_DOUBLE(-2.827076, bc.ccl().x(), 0.00001);
44 WVPASSEQ_DOUBLE(1.0000000000000004, bc.ccl().y(), 0.00001);
45 WVPASSEQ_DOUBLE(4.827076, bc.ccr().x(), 0.00001);
46 WVPASSEQ_DOUBLE(1.0, bc.ccr().y(), 0.00001);
48 // car radiuses (inner radius, outer front radius, outer rear radius)
50 WVPASSEQ_DOUBLE(3.327076, bc.iradi(), 0.00001);
51 WVPASSEQ_DOUBLE(4.997358, bc.ofradi(), 0.00001);
52 WVPASSEQ_DOUBLE(4.355868, bc.orradi(), 0.00001);
53 WVPASSEQ_DOUBLE(3.485485, bc.imradi(), 0.00001);
54 WVPASSEQ_DOUBLE(5.199608, bc.omradi(), 0.00001);
61 WVPASSEQ_DOUBLE(1.0, bc.x(), 0.00001);
62 WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
65 WVPASSEQ_DOUBLE(0.481558, bc.st(), 0.00001);
71 bc.rotate(Point(-1.0, 1.0), M_PI);
72 WVPASSEQ_DOUBLE(-1.0, bc.x(), 0.00001);
73 WVPASSEQ_DOUBLE(1.0, bc.y(), 0.00001);
74 WVPASSEQ_DOUBLE(M_PI, bc.h(), 0.00001);
75 bc.rotate(Point(0.0, 1.0), -M_PI / 2.0);
76 WVPASSEQ_DOUBLE(0.0, bc.x(), 0.00001);
77 WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
78 WVPASSEQ_DOUBLE(M_PI / 2.0, bc.h(), 0.00001);
81 WVTEST_MAIN("test collide functions")
83 std::vector<Point> p1;
84 p1.push_back(Point(1.0, 1.0));
85 p1.push_back(Point(1.0, 3.0));
86 p1.push_back(Point(3.0, 3.0));
87 p1.push_back(Point(3.0, 1.0));
88 WVPASS(Point(2.0, 2.0).inside_of(p1));
89 WVPASS(!Point(4.0, 4.0).inside_of(p1));
91 auto li1 = Line(Point(1.0, 1.0), Point(3.0, 3.0));
92 auto li2 = Line(Point(1.0, 3.0), Point(3.0, 1.0));
93 WVPASS(li1.intersects_with(li2));
94 WVPASSEQ_DOUBLE(li1.i1().x(), 2.0, 0.00001);
95 WVPASSEQ_DOUBLE(li1.i1().y(), 2.0, 0.00001);
98 auto li1 = Line(Point(1.0, 1.0), Point(1.0, 3.0));
99 auto li2 = Line(Point(3.0, 1.0), Point(3.0, 3.0));
100 WVPASS(!li1.intersects_with(li2));
102 std::vector<Point> p2;
103 p2.push_back(Point(2.5, 1.0));
104 p2.push_back(Point(3.5, 3.0));
105 p2.push_back(Point(2.0, 4.0));
106 p2.push_back(Point(1.0, 2.0));
108 // TODO polygon-polygon collision detection was removed
111 std::vector<Point> p3;
112 p3.push_back(Point(2.0, 2.0));
113 p3.push_back(Point(2.0, 0.0));
114 p3.push_back(Point(4.0, 0.0));
115 p3.push_back(Point(4.0, 2.0));
117 // TODO polygon-polygon collision detection was removed
120 auto c = Point(1.0, 1.0);
121 auto zz = Point(0.0, 0.0);
122 auto pp = Point(5.0, 5.0);
123 auto mp = Point(-5.0, 5.0);
124 auto mm = Point(-5.0, -5.0);
125 auto pm = Point(5.0, -5.0);
128 WVPASS(Line(zz, pp).intersects_with(c, r3));
129 WVPASS(Line(zz, mp).intersects_with(c, r3));
130 WVPASS(Line(zz, mm).intersects_with(c, r3));
131 WVPASS(Line(zz, pm).intersects_with(c, r3));
132 WVPASS(!Line(mp, pp).intersects_with(c, r1));
133 WVPASS(!Line(mm, pm).intersects_with(c, r1));
134 WVPASS(!Line(mm, mp).intersects_with(c, r1));
135 WVPASS(!Line(pm, pp).intersects_with(c, r1));
138 WVTEST_MAIN("auxiliary angle between three points")
140 auto zz = Point(0.0, 0.0);
141 auto oo = Point(1.0, 1.0);
142 auto tt = Point(2.0, 2.0);
143 auto oz = Point(1.0, 0.0);
144 auto zo = Point(0.0, 1.0);
145 auto mtt = Point(-2.0, 2.0);
146 auto moo = Point(-1.0, 1.0);
147 auto mot = Point(-1.0, 2.0);
148 WVPASSEQ_DOUBLE(oz.min_angle_between(zz, zo), M_PI / 2.0, 0.00001);
149 WVPASSEQ_DOUBLE(zo.min_angle_between(zz, oz), M_PI / 2.0, 0.00001);
150 WVPASSEQ_DOUBLE(oz.min_angle_between(zo, zz), M_PI / 4.0, 0.00001);
151 WVPASSEQ_DOUBLE(tt.min_angle_between(oo, zz), 0.0, 0.00001);
152 WVPASSEQ_DOUBLE(mtt.min_angle_between(moo, mot), M_PI / 4.0, 0.00001);
153 WVPASSEQ_DOUBLE(mot.min_angle_between(moo, mtt), M_PI / 4.0, 0.00001);
154 auto momo = Point(-1.0, -1.0);
155 auto to = Point(2.0, 1.0);
156 auto ot = Point(1.0, 2.0);
157 auto omo = Point(1.0, -1.0);
158 auto tmo = Point(2.0, -1.0);
159 auto mto = Point(-2.0, 1.0);
160 WVPASS(to.on_right_side_of(Line(momo, oo)));
161 WVPASS(!ot.on_right_side_of(Line(momo, oo)));
162 WVPASS(!to.on_right_side_of(Line(moo, omo)));
163 WVPASS(!ot.on_right_side_of(Line(moo, omo)));
164 WVPASS(!tmo.on_right_side_of(Line(moo, omo)));
165 WVPASS(!mot.on_right_side_of(Line(moo, omo)));
166 WVPASS(mto.on_right_side_of(Line(moo, omo)));
169 WVTEST_MAIN("drivable")
171 double tmp_double_1 = 0;
172 double tmp_double_2 = 0;
174 // TODO set g.x, g.y to different values
175 // TODO set g.h to cover all 4 quadrants
180 WVPASS(g.drivable(n)); // pass the same pose
183 n.rotate(g.ccr(), -M_PI/2);
184 WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
185 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
186 tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
187 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
188 WVPASS(g.drivable(n)); // pass right corner case
189 n.rotate(g.ccr(), -0.1);
190 WVPASS(!g.drivable(n)); // fail right corner case
193 n.rotate(g.ccl(), M_PI/2);
194 WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
195 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
196 tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
197 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
198 WVPASS(g.drivable(n)); // pass left corner case
199 n.rotate(g.ccl(), 0.1);
200 WVPASS(!g.drivable(n)); // fail left corner case
203 n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
206 WVPASS(g.drivable(n)); // pass forward corner case
208 for (double a = 0; a > -M_PI/2; a -= 0.01) {
210 n.rotate(g.ccr(), a);
211 WVPASS(g.drivable(n)); // pass drivable border
213 for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
214 // + 0.1 -- compensate for Euclid. dist. check
216 n.x(n.x() + 0.1*cos(n.h()));
217 n.y(n.y() + 0.1*sin(n.h()));
218 n.rotate(n.ccr(), a);
219 WVPASS(g.drivable(n)); // pass near drivable border
221 for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
222 // = -0.1 -- compensate for near goal
224 n.x(n.x() - 0.1*cos(n.h()));
225 n.y(n.y() - 0.1*sin(n.h()));
226 n.rotate(n.ccr(), a);
227 WVPASS(!g.drivable(n)); // fail near drivable border
229 for (double a = 0; a < M_PI / 2; a += 0.01) {
231 n.rotate(g.ccl(), a);
232 WVPASS(g.drivable(n)); // pass drivable border
234 for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
235 // - 0.1 -- compensate for Euclid. dist. check
237 n.x(n.x() + 0.1*cos(n.h()));
238 n.y(n.y() + 0.1*sin(n.h()));
239 n.rotate(n.ccl(), a);
240 WVPASS(g.drivable(n)); // pass near drivable border
242 for (double a = 0.1; a < M_PI / 2; a += 0.01) {
243 // = 0.1 -- compensate for near goal
245 n.x(n.x() - 0.1*cos(n.h()));
246 n.y(n.y() - 0.1*sin(n.h()));
247 n.rotate(n.ccl(), a);
248 WVPASS(!g.drivable(n)); // fail near drivable border
252 n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
256 WVPASS(g.drivable(n)); // pass backward corner case
259 n.rotate(g.ccr(), M_PI/2);
260 WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
261 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
262 tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
263 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
264 WVPASS(g.drivable(n)); // pass right corner case
267 n.rotate(g.ccl(), -M_PI/2);
268 WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
269 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
270 tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
271 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
272 WVPASS(g.drivable(n)); // pass left corner case
274 for (double a = 0; a < M_PI / 2; a += 0.01) {
276 n.rotate(g.ccr(), a);
277 WVPASS(g.drivable(n)); // pass drivable border
279 for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
280 // - 0.1 -- compensate for Euclid. dist. check
282 n.x(n.x() - 0.1*cos(n.h()));
283 n.y(n.y() - 0.1*sin(n.h()));
284 n.rotate(n.ccr(), a);
285 WVPASS(g.drivable(n)); // pass near drivable border
287 for (double a = 0.1; a < M_PI / 2; a += 0.01) {
288 // = 0.1 -- compensate for near goal
290 n.x(n.x() + 0.1*cos(n.h()));
291 n.y(n.y() + 0.1*sin(n.h()));
292 n.rotate(n.ccr(), a);
293 WVPASS(!g.drivable(n)); // fail near drivable border
295 for (double a = 0; a > -M_PI/2; a -= 0.01) {
297 n.rotate(g.ccl(), a);
298 WVPASS(g.drivable(n)); // pass drivable border
300 for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
301 // + 0.1 -- compensate for Euclid. dist. check
303 n.x(n.x() - 0.1*cos(n.h()));
304 n.y(n.y() - 0.1*sin(n.h()));
305 n.rotate(n.ccl(), a);
306 WVPASS(g.drivable(n)); // pass near drivable border
308 for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
309 // = -0.1 -- compensate for near goal
311 n.x(n.x() + 0.1*cos(n.h()));
312 n.y(n.y() + 0.1*sin(n.h()));
313 n.rotate(n.ccl(), a);
314 WVPASS(!g.drivable(n)); // fail near drivable border