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.ralx(), 0.00001);
37 WVPASSEQ_DOUBLE(1.5, bc.rarx(), 0.00001);
38 WVPASSEQ_DOUBLE(1.0, bc.raly(), 0.00001);
39 WVPASSEQ_DOUBLE(1.0, bc.rary(), 0.00001);
41 // min. turning radius circle centers
42 WVPASSEQ_DOUBLE(bc.h(), M_PI / 2.0, 0.00001);
43 WVPASSEQ_DOUBLE(-3.08257569495584, bc.ccl().x(), 0.00001);
44 WVPASSEQ_DOUBLE(1.0000000000000004, bc.ccl().y(), 0.00001);
45 WVPASSEQ_DOUBLE(5.08257569495584, 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(bc.iradi(), 3.58257569495584, 0.00001);
51 WVPASSEQ_DOUBLE(bc.ofradi(), 5.220153254455275, 0.00001);
52 WVPASSEQ_DOUBLE(bc.orradi(), 4.6097722286464435, 0.00001);
59 WVPASSEQ_DOUBLE(1.0, bc.x(), 0.00001);
60 WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
63 WVPASSEQ_DOUBLE(0.45552437743483687, bc.st(), 0.00001);
69 bc.rotate(Point(-1.0, 1.0), M_PI);
70 WVPASSEQ_DOUBLE(-1.0, bc.x(), 0.00001);
71 WVPASSEQ_DOUBLE(1.0, bc.y(), 0.00001);
72 WVPASSEQ_DOUBLE(M_PI, bc.h(), 0.00001);
73 bc.rotate(Point(0.0, 1.0), -M_PI / 2.0);
74 WVPASSEQ_DOUBLE(0.0, bc.x(), 0.00001);
75 WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
76 WVPASSEQ_DOUBLE(M_PI / 2.0, bc.h(), 0.00001);
79 WVTEST_MAIN("test collide functions")
81 std::vector<Point> p1;
82 p1.push_back(Point(1.0, 1.0));
83 p1.push_back(Point(1.0, 3.0));
84 p1.push_back(Point(3.0, 3.0));
85 p1.push_back(Point(3.0, 1.0));
86 WVPASS(Point(2.0, 2.0).inside_of(p1));
87 WVPASS(!Point(4.0, 4.0).inside_of(p1));
89 auto li1 = Line(Point(1.0, 1.0), Point(3.0, 3.0));
90 auto li2 = Line(Point(1.0, 3.0), Point(3.0, 1.0));
91 WVPASS(li1.intersects_with(li2));
92 WVPASSEQ_DOUBLE(li1.i1().x(), 2.0, 0.00001);
93 WVPASSEQ_DOUBLE(li1.i1().y(), 2.0, 0.00001);
96 auto li1 = Line(Point(1.0, 1.0), Point(1.0, 3.0));
97 auto li2 = Line(Point(3.0, 1.0), Point(3.0, 3.0));
98 WVPASS(!li1.intersects_with(li2));
100 std::vector<Point> p2;
101 p2.push_back(Point(2.5, 1.0));
102 p2.push_back(Point(3.5, 3.0));
103 p2.push_back(Point(2.0, 4.0));
104 p2.push_back(Point(1.0, 2.0));
106 // TODO polygon-polygon collision detection was removed
109 std::vector<Point> p3;
110 p3.push_back(Point(2.0, 2.0));
111 p3.push_back(Point(2.0, 0.0));
112 p3.push_back(Point(4.0, 0.0));
113 p3.push_back(Point(4.0, 2.0));
115 // TODO polygon-polygon collision detection was removed
118 auto c = Point(1.0, 1.0);
119 auto zz = Point(0.0, 0.0);
120 auto pp = Point(5.0, 5.0);
121 auto mp = Point(-5.0, 5.0);
122 auto mm = Point(-5.0, -5.0);
123 auto pm = Point(5.0, -5.0);
126 WVPASS(Line(zz, pp).intersects_with(c, r3));
127 WVPASS(Line(zz, mp).intersects_with(c, r3));
128 WVPASS(Line(zz, mm).intersects_with(c, r3));
129 WVPASS(Line(zz, pm).intersects_with(c, r3));
130 WVPASS(!Line(mp, pp).intersects_with(c, r1));
131 WVPASS(!Line(mm, pm).intersects_with(c, r1));
132 WVPASS(!Line(mm, mp).intersects_with(c, r1));
133 WVPASS(!Line(pm, pp).intersects_with(c, r1));
136 WVTEST_MAIN("auxiliary angle between three points")
138 auto zz = Point(0.0, 0.0);
139 auto oo = Point(1.0, 1.0);
140 auto tt = Point(2.0, 2.0);
141 auto oz = Point(1.0, 0.0);
142 auto zo = Point(0.0, 1.0);
143 auto mtt = Point(-2.0, 2.0);
144 auto moo = Point(-1.0, 1.0);
145 auto mot = Point(-1.0, 2.0);
146 WVPASSEQ_DOUBLE(oz.min_angle_between(zz, zo), M_PI / 2.0, 0.00001);
147 WVPASSEQ_DOUBLE(zo.min_angle_between(zz, oz), M_PI / 2.0, 0.00001);
148 WVPASSEQ_DOUBLE(oz.min_angle_between(zo, zz), M_PI / 4.0, 0.00001);
149 WVPASSEQ_DOUBLE(tt.min_angle_between(oo, zz), 0.0, 0.00001);
150 WVPASSEQ_DOUBLE(mtt.min_angle_between(moo, mot), M_PI / 4.0, 0.00001);
151 WVPASSEQ_DOUBLE(mot.min_angle_between(moo, mtt), M_PI / 4.0, 0.00001);
152 auto momo = Point(-1.0, -1.0);
153 auto to = Point(2.0, 1.0);
154 auto ot = Point(1.0, 2.0);
155 auto omo = Point(1.0, -1.0);
156 auto tmo = Point(2.0, -1.0);
157 auto mto = Point(-2.0, 1.0);
158 WVPASS(to.on_right_side_of(Line(momo, oo)));
159 WVPASS(!ot.on_right_side_of(Line(momo, oo)));
160 WVPASS(!to.on_right_side_of(Line(moo, omo)));
161 WVPASS(!ot.on_right_side_of(Line(moo, omo)));
162 WVPASS(!tmo.on_right_side_of(Line(moo, omo)));
163 WVPASS(!mot.on_right_side_of(Line(moo, omo)));
164 WVPASS(mto.on_right_side_of(Line(moo, omo)));
167 WVTEST_MAIN("drivable")
169 double tmp_double_1 = 0;
170 double tmp_double_2 = 0;
172 // TODO set g.x, g.y to different values
173 // TODO set g.h to cover all 4 quadrants
178 WVPASS(g.drivable(n)); // pass the same pose
181 n.rotate(g.ccr(), -M_PI/2);
182 WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
183 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
184 tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
185 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
186 WVPASS(g.drivable(n)); // pass right corner case
189 n.rotate(g.ccl(), 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 left corner case
195 n.rotate(g.ccl(), 0.01);
196 WVPASS(!g.drivable(n)); // fail left corner case
199 n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
202 WVPASS(g.drivable(n)); // pass forward corner case
204 for (double a = 0; a > -M_PI/2; a -= 0.01) {
206 n.rotate(g.ccr(), a);
207 WVPASS(g.drivable(n)); // pass drivable border
209 for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
210 // + 0.1 -- compensate for Euclid. dist. check
212 n.x(n.x() + 0.1*cos(n.h()));
213 n.y(n.y() + 0.1*sin(n.h()));
214 n.rotate(n.ccr(), a);
215 WVPASS(g.drivable(n)); // pass near drivable border
217 for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
218 // = -0.1 -- compensate for near goal
220 n.x(n.x() - 0.1*cos(n.h()));
221 n.y(n.y() - 0.1*sin(n.h()));
222 n.rotate(n.ccr(), a);
223 WVPASS(!g.drivable(n)); // fail near drivable border
225 for (double a = 0; a < M_PI / 2; a += 0.01) {
227 n.rotate(g.ccl(), a);
228 WVPASS(g.drivable(n)); // pass drivable border
230 for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
231 // - 0.1 -- compensate for Euclid. dist. check
233 n.x(n.x() + 0.1*cos(n.h()));
234 n.y(n.y() + 0.1*sin(n.h()));
235 n.rotate(n.ccl(), a);
236 WVPASS(g.drivable(n)); // pass near drivable border
238 for (double a = 0.1; a < M_PI / 2; a += 0.01) {
239 // = 0.1 -- compensate for near goal
241 n.x(n.x() - 0.1*cos(n.h()));
242 n.y(n.y() - 0.1*sin(n.h()));
243 n.rotate(n.ccl(), a);
244 WVPASS(!g.drivable(n)); // fail near drivable border
248 n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
252 WVPASS(g.drivable(n)); // pass backward corner case
255 n.rotate(g.ccr(), M_PI/2);
256 WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
257 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
258 tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
259 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
260 WVPASS(g.drivable(n)); // pass right corner case
263 n.rotate(g.ccl(), -M_PI/2);
264 WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
265 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
266 tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
267 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
268 WVPASS(g.drivable(n)); // pass left corner case
270 for (double a = 0; a < M_PI / 2; a += 0.01) {
272 n.rotate(g.ccr(), a);
273 WVPASS(g.drivable(n)); // pass drivable border
275 for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
276 // - 0.1 -- compensate for Euclid. dist. check
278 n.x(n.x() - 0.1*cos(n.h()));
279 n.y(n.y() - 0.1*sin(n.h()));
280 n.rotate(n.ccr(), a);
281 WVPASS(g.drivable(n)); // pass near drivable border
283 for (double a = 0.1; a < M_PI / 2; a += 0.01) {
284 // = 0.1 -- compensate for near goal
286 n.x(n.x() + 0.1*cos(n.h()));
287 n.y(n.y() + 0.1*sin(n.h()));
288 n.rotate(n.ccr(), a);
289 WVPASS(!g.drivable(n)); // fail near drivable border
291 for (double a = 0; a > -M_PI/2; a -= 0.01) {
293 n.rotate(g.ccl(), a);
294 WVPASS(g.drivable(n)); // pass drivable border
296 for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
297 // + 0.1 -- compensate for Euclid. dist. check
299 n.x(n.x() - 0.1*cos(n.h()));
300 n.y(n.y() - 0.1*sin(n.h()));
301 n.rotate(n.ccl(), a);
302 WVPASS(g.drivable(n)); // pass near drivable border
304 for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
305 // = -0.1 -- compensate for near goal
307 n.x(n.x() + 0.1*cos(n.h()));
308 n.y(n.y() + 0.1*sin(n.h()));
309 n.rotate(n.ccl(), a);
310 WVPASS(!g.drivable(n)); // fail near drivable border