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 WVPASSEQ_DOUBLE(0.70710677, bc.edist_to_rr(), 10e-5);
82 WVPASSEQ_DOUBLE(bc.edist(bc.rr()), bc.edist_to_rr(), 10e-5);
83 WVPASSEQ_DOUBLE(2.5495098, bc.edist_to_lf(), 10e-5);
84 WVPASSEQ_DOUBLE(bc.edist(bc.lf()), bc.edist_to_lf(), 10e-5);
87 WVTEST_MAIN("test collide functions")
89 std::vector<Point> p1;
90 p1.push_back(Point(1.0, 1.0));
91 p1.push_back(Point(1.0, 3.0));
92 p1.push_back(Point(3.0, 3.0));
93 p1.push_back(Point(3.0, 1.0));
94 WVPASS(Point(2.0, 2.0).inside_of(p1));
95 WVPASS(!Point(4.0, 4.0).inside_of(p1));
97 auto li1 = Line(Point(1.0, 1.0), Point(3.0, 3.0));
98 auto li2 = Line(Point(1.0, 3.0), Point(3.0, 1.0));
99 WVPASS(li1.intersects_with(li2));
100 WVPASSEQ_DOUBLE(li1.i1().x(), 2.0, 0.00001);
101 WVPASSEQ_DOUBLE(li1.i1().y(), 2.0, 0.00001);
104 auto li1 = Line(Point(1.0, 1.0), Point(1.0, 3.0));
105 auto li2 = Line(Point(3.0, 1.0), Point(3.0, 3.0));
106 WVPASS(!li1.intersects_with(li2));
108 std::vector<Point> p2;
109 p2.push_back(Point(2.5, 1.0));
110 p2.push_back(Point(3.5, 3.0));
111 p2.push_back(Point(2.0, 4.0));
112 p2.push_back(Point(1.0, 2.0));
114 // TODO polygon-polygon collision detection was removed
117 std::vector<Point> p3;
118 p3.push_back(Point(2.0, 2.0));
119 p3.push_back(Point(2.0, 0.0));
120 p3.push_back(Point(4.0, 0.0));
121 p3.push_back(Point(4.0, 2.0));
123 // TODO polygon-polygon collision detection was removed
126 auto c = Point(1.0, 1.0);
127 auto zz = Point(0.0, 0.0);
128 auto pp = Point(5.0, 5.0);
129 auto mp = Point(-5.0, 5.0);
130 auto mm = Point(-5.0, -5.0);
131 auto pm = Point(5.0, -5.0);
134 WVPASS(Line(zz, pp).intersects_with(c, r3));
135 WVPASS(Line(zz, mp).intersects_with(c, r3));
136 WVPASS(Line(zz, mm).intersects_with(c, r3));
137 WVPASS(Line(zz, pm).intersects_with(c, r3));
138 WVPASS(!Line(mp, pp).intersects_with(c, r1));
139 WVPASS(!Line(mm, pm).intersects_with(c, r1));
140 WVPASS(!Line(mm, mp).intersects_with(c, r1));
141 WVPASS(!Line(pm, pp).intersects_with(c, r1));
144 WVTEST_MAIN("auxiliary angle between three points")
146 auto zz = Point(0.0, 0.0);
147 auto oo = Point(1.0, 1.0);
148 auto tt = Point(2.0, 2.0);
149 auto oz = Point(1.0, 0.0);
150 auto zo = Point(0.0, 1.0);
151 auto mtt = Point(-2.0, 2.0);
152 auto moo = Point(-1.0, 1.0);
153 auto mot = Point(-1.0, 2.0);
154 WVPASSEQ_DOUBLE(oz.min_angle_between(zz, zo), M_PI / 2.0, 0.00001);
155 WVPASSEQ_DOUBLE(zo.min_angle_between(zz, oz), M_PI / 2.0, 0.00001);
156 WVPASSEQ_DOUBLE(oz.min_angle_between(zo, zz), M_PI / 4.0, 0.00001);
157 WVPASSEQ_DOUBLE(tt.min_angle_between(oo, zz), 0.0, 0.00001);
158 WVPASSEQ_DOUBLE(mtt.min_angle_between(moo, mot), M_PI / 4.0, 0.00001);
159 WVPASSEQ_DOUBLE(mot.min_angle_between(moo, mtt), M_PI / 4.0, 0.00001);
160 auto momo = Point(-1.0, -1.0);
161 auto to = Point(2.0, 1.0);
162 auto ot = Point(1.0, 2.0);
163 auto omo = Point(1.0, -1.0);
164 auto tmo = Point(2.0, -1.0);
165 auto mto = Point(-2.0, 1.0);
166 WVPASS(to.on_right_side_of(Line(momo, oo)));
167 WVPASS(!ot.on_right_side_of(Line(momo, oo)));
168 WVPASS(!to.on_right_side_of(Line(moo, omo)));
169 WVPASS(!ot.on_right_side_of(Line(moo, omo)));
170 WVPASS(!tmo.on_right_side_of(Line(moo, omo)));
171 WVPASS(!mot.on_right_side_of(Line(moo, omo)));
172 WVPASS(mto.on_right_side_of(Line(moo, omo)));
175 WVTEST_MAIN("drivable")
177 double tmp_double_1 = 0;
178 double tmp_double_2 = 0;
180 // TODO set g.x, g.y to different values
181 // TODO set g.h to cover all 4 quadrants
186 WVPASS(g.drivable(n)); // pass the same pose
189 n.rotate(g.ccr(), -M_PI/2);
190 WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
191 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
192 tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
193 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
194 WVPASS(g.drivable(n)); // pass right corner case
195 n.rotate(g.ccr(), -0.1);
196 WVPASS(!g.drivable(n)); // fail right corner case
199 n.rotate(g.ccl(), M_PI/2);
200 WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
201 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
202 tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
203 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
204 WVPASS(g.drivable(n)); // pass left corner case
205 n.rotate(g.ccl(), 0.1);
206 WVPASS(!g.drivable(n)); // fail left corner case
209 n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
212 WVPASS(g.drivable(n)); // pass forward corner case
214 for (double a = 0; a > -M_PI/2; a -= 0.01) {
216 n.rotate(g.ccr(), a);
217 WVPASS(g.drivable(n)); // pass drivable border
219 for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
220 // + 0.1 -- compensate for Euclid. dist. check
222 n.x(n.x() + 0.1*cos(n.h()));
223 n.y(n.y() + 0.1*sin(n.h()));
224 n.rotate(n.ccr(), a);
225 WVPASS(g.drivable(n)); // pass near drivable border
227 for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
228 // = -0.1 -- compensate for near goal
230 n.x(n.x() - 0.1*cos(n.h()));
231 n.y(n.y() - 0.1*sin(n.h()));
232 n.rotate(n.ccr(), a);
233 WVPASS(!g.drivable(n)); // fail near drivable border
235 for (double a = 0; a < M_PI / 2; a += 0.01) {
237 n.rotate(g.ccl(), a);
238 WVPASS(g.drivable(n)); // pass drivable border
240 for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
241 // - 0.1 -- compensate for Euclid. dist. check
243 n.x(n.x() + 0.1*cos(n.h()));
244 n.y(n.y() + 0.1*sin(n.h()));
245 n.rotate(n.ccl(), a);
246 WVPASS(g.drivable(n)); // pass near drivable border
248 for (double a = 0.1; a < M_PI / 2; a += 0.01) {
249 // = 0.1 -- compensate for near goal
251 n.x(n.x() - 0.1*cos(n.h()));
252 n.y(n.y() - 0.1*sin(n.h()));
253 n.rotate(n.ccl(), a);
254 WVPASS(!g.drivable(n)); // fail near drivable border
258 n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
262 WVPASS(g.drivable(n)); // pass backward corner case
265 n.rotate(g.ccr(), M_PI/2);
266 WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
267 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
268 tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
269 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
270 WVPASS(g.drivable(n)); // pass right corner case
273 n.rotate(g.ccl(), -M_PI/2);
274 WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
275 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
276 tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
277 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
278 WVPASS(g.drivable(n)); // pass left corner case
280 for (double a = 0; a < M_PI / 2; a += 0.01) {
282 n.rotate(g.ccr(), a);
283 WVPASS(g.drivable(n)); // pass drivable border
285 for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
286 // - 0.1 -- compensate for Euclid. dist. check
288 n.x(n.x() - 0.1*cos(n.h()));
289 n.y(n.y() - 0.1*sin(n.h()));
290 n.rotate(n.ccr(), a);
291 WVPASS(g.drivable(n)); // pass near drivable border
293 for (double a = 0.1; a < M_PI / 2; a += 0.01) {
294 // = 0.1 -- compensate for near goal
296 n.x(n.x() + 0.1*cos(n.h()));
297 n.y(n.y() + 0.1*sin(n.h()));
298 n.rotate(n.ccr(), a);
299 WVPASS(!g.drivable(n)); // fail near drivable border
301 for (double a = 0; a > -M_PI/2; a -= 0.01) {
303 n.rotate(g.ccl(), a);
304 WVPASS(g.drivable(n)); // pass drivable border
306 for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
307 // + 0.1 -- compensate for Euclid. dist. check
309 n.x(n.x() - 0.1*cos(n.h()));
310 n.y(n.y() - 0.1*sin(n.h()));
311 n.rotate(n.ccl(), a);
312 WVPASS(g.drivable(n)); // pass near drivable border
314 for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
315 // = -0.1 -- compensate for near goal
317 n.x(n.x() + 0.1*cos(n.h()));
318 n.y(n.y() + 0.1*sin(n.h()));
319 n.rotate(n.ccl(), a);
320 WVPASS(!g.drivable(n)); // fail near drivable border