8 WVTEST_MAIN("bcar basic geometry")
21 WVPASSEQ_DOUBLE(bc.len(), bc.df() + bc.dr(), 0.00001);
22 WVPASSEQ_DOUBLE(0.5, bc.lfx(), 0.00001);
23 WVPASSEQ_DOUBLE(0.5, bc.lrx(), 0.00001);
24 WVPASSEQ_DOUBLE(1.5, bc.rrx(), 0.00001);
25 WVPASSEQ_DOUBLE(1.5, bc.rfx(), 0.00001);
26 WVPASSEQ_DOUBLE(3.5, bc.lfy(), 0.00001);
27 WVPASSEQ_DOUBLE(0.5, bc.lry(), 0.00001);
28 WVPASSEQ_DOUBLE(0.5, bc.rry(), 0.00001);
29 WVPASSEQ_DOUBLE(3.5, bc.rfy(), 0.00001);
30 WVPASSEQ_DOUBLE(0.5, bc.ralx(), 0.00001);
31 WVPASSEQ_DOUBLE(1.5, bc.rarx(), 0.00001);
32 WVPASSEQ_DOUBLE(1.0, bc.raly(), 0.00001);
33 WVPASSEQ_DOUBLE(1.0, bc.rary(), 0.00001);
35 // min. turning radius circle centers
36 WVPASSEQ_DOUBLE(bc.h(), M_PI / 2.0, 0.00001);
37 WVPASSEQ_DOUBLE(-3.08257569495584, bc.ccl().x(), 0.00001);
38 WVPASSEQ_DOUBLE(1.0000000000000004, bc.ccl().y(), 0.00001);
39 WVPASSEQ_DOUBLE(5.08257569495584, bc.ccr().x(), 0.00001);
40 WVPASSEQ_DOUBLE(1.0, bc.ccr().y(), 0.00001);
42 // car radiuses (inner radius, outer front radius, outer rear radius)
44 WVPASSEQ_DOUBLE(bc.iradi(), 3.58257569495584, 0.00001);
45 WVPASSEQ_DOUBLE(bc.ofradi(), 5.220153254455275, 0.00001);
46 WVPASSEQ_DOUBLE(bc.orradi(), 4.6097722286464435, 0.00001);
53 WVPASSEQ_DOUBLE(1.0, bc.x(), 0.00001);
54 WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
57 WVPASSEQ_DOUBLE(0.45552437743483687, bc.st(), 0.00001);
63 bc.rotate(Point(-1.0, 1.0), M_PI);
64 WVPASSEQ_DOUBLE(-1.0, bc.x(), 0.00001);
65 WVPASSEQ_DOUBLE(1.0, bc.y(), 0.00001);
66 WVPASSEQ_DOUBLE(M_PI, bc.h(), 0.00001);
67 bc.rotate(Point(0.0, 1.0), -M_PI / 2.0);
68 WVPASSEQ_DOUBLE(0.0, bc.x(), 0.00001);
69 WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
70 WVPASSEQ_DOUBLE(M_PI / 2.0, bc.h(), 0.00001);
73 WVTEST_MAIN("test collide functions")
75 std::vector<Point> p1;
76 p1.push_back(Point(1.0, 1.0));
77 p1.push_back(Point(1.0, 3.0));
78 p1.push_back(Point(3.0, 3.0));
79 p1.push_back(Point(3.0, 1.0));
80 WVPASS(Point(2.0, 2.0).inside_of(p1));
81 WVPASS(!Point(4.0, 4.0).inside_of(p1));
83 auto li1 = Line(Point(1.0, 1.0), Point(3.0, 3.0));
84 auto li2 = Line(Point(1.0, 3.0), Point(3.0, 1.0));
85 WVPASS(li1.intersects_with(li2));
86 WVPASSEQ_DOUBLE(li1.i1().x(), 2.0, 0.00001);
87 WVPASSEQ_DOUBLE(li1.i1().y(), 2.0, 0.00001);
90 auto li1 = Line(Point(1.0, 1.0), Point(1.0, 3.0));
91 auto li2 = Line(Point(3.0, 1.0), Point(3.0, 3.0));
92 WVPASS(!li1.intersects_with(li2));
94 std::vector<Point> p2;
95 p2.push_back(Point(2.5, 1.0));
96 p2.push_back(Point(3.5, 3.0));
97 p2.push_back(Point(2.0, 4.0));
98 p2.push_back(Point(1.0, 2.0));
100 // TODO polygon-polygon collision detection was removed
103 std::vector<Point> p3;
104 p3.push_back(Point(2.0, 2.0));
105 p3.push_back(Point(2.0, 0.0));
106 p3.push_back(Point(4.0, 0.0));
107 p3.push_back(Point(4.0, 2.0));
109 // TODO polygon-polygon collision detection was removed
112 auto c = Point(1.0, 1.0);
113 auto zz = Point(0.0, 0.0);
114 auto pp = Point(5.0, 5.0);
115 auto mp = Point(-5.0, 5.0);
116 auto mm = Point(-5.0, -5.0);
117 auto pm = Point(5.0, -5.0);
120 WVPASS(Line(zz, pp).intersects_with(c, r3));
121 WVPASS(Line(zz, mp).intersects_with(c, r3));
122 WVPASS(Line(zz, mm).intersects_with(c, r3));
123 WVPASS(Line(zz, pm).intersects_with(c, r3));
124 WVPASS(!Line(mp, pp).intersects_with(c, r1));
125 WVPASS(!Line(mm, pm).intersects_with(c, r1));
126 WVPASS(!Line(mm, mp).intersects_with(c, r1));
127 WVPASS(!Line(pm, pp).intersects_with(c, r1));
130 WVTEST_MAIN("auxiliary angle between three points")
132 auto zz = Point(0.0, 0.0);
133 auto oo = Point(1.0, 1.0);
134 auto tt = Point(2.0, 2.0);
135 auto oz = Point(1.0, 0.0);
136 auto zo = Point(0.0, 1.0);
137 auto mtt = Point(-2.0, 2.0);
138 auto moo = Point(-1.0, 1.0);
139 auto mot = Point(-1.0, 2.0);
140 WVPASSEQ_DOUBLE(oz.min_angle_between(zz, zo), M_PI / 2.0, 0.00001);
141 WVPASSEQ_DOUBLE(zo.min_angle_between(zz, oz), M_PI / 2.0, 0.00001);
142 WVPASSEQ_DOUBLE(oz.min_angle_between(zo, zz), M_PI / 4.0, 0.00001);
143 WVPASSEQ_DOUBLE(tt.min_angle_between(oo, zz), 0.0, 0.00001);
144 WVPASSEQ_DOUBLE(mtt.min_angle_between(moo, mot), M_PI / 4.0, 0.00001);
145 WVPASSEQ_DOUBLE(mot.min_angle_between(moo, mtt), M_PI / 4.0, 0.00001);
146 auto momo = Point(-1.0, -1.0);
147 auto to = Point(2.0, 1.0);
148 auto ot = Point(1.0, 2.0);
149 auto omo = Point(1.0, -1.0);
150 auto tmo = Point(2.0, -1.0);
151 auto mto = Point(-2.0, 1.0);
152 WVPASS(to.on_right_side_of(Line(momo, oo)));
153 WVPASS(!ot.on_right_side_of(Line(momo, oo)));
154 WVPASS(!to.on_right_side_of(Line(moo, omo)));
155 WVPASS(!ot.on_right_side_of(Line(moo, omo)));
156 WVPASS(!tmo.on_right_side_of(Line(moo, omo)));
157 WVPASS(!mot.on_right_side_of(Line(moo, omo)));
158 WVPASS(mto.on_right_side_of(Line(moo, omo)));
161 WVTEST_MAIN("drivable")
163 double tmp_double_1 = 0;
164 double tmp_double_2 = 0;
166 // TODO set g.x, g.y to different values
167 // TODO set g.h to cover all 4 quadrants
172 WVPASS(g.drivable(n)); // pass the same pose
175 n.rotate(g.ccr(), -M_PI/2);
176 WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
177 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
178 tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
179 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
180 WVPASS(g.drivable(n)); // pass right corner case
183 n.rotate(g.ccl(), 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 left corner case
189 n.rotate(g.ccl(), 0.01);
190 WVPASS(!g.drivable(n)); // fail left corner case
193 n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
196 WVPASS(g.drivable(n)); // pass forward corner case
198 for (double a = 0; a > -M_PI/2; a -= 0.01) {
200 n.rotate(g.ccr(), a);
201 WVPASS(g.drivable(n)); // pass drivable border
203 for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
204 // + 0.1 -- compensate for Euclid. dist. check
206 n.x(n.x() + 0.1*cos(n.h()));
207 n.y(n.y() + 0.1*sin(n.h()));
208 n.rotate(n.ccr(), a);
209 WVPASS(g.drivable(n)); // pass near drivable border
211 for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
212 // = -0.1 -- compensate for near goal
214 n.x(n.x() - 0.1*cos(n.h()));
215 n.y(n.y() - 0.1*sin(n.h()));
216 n.rotate(n.ccr(), a);
217 WVPASS(!g.drivable(n)); // fail near drivable border
219 for (double a = 0; a < M_PI / 2; a += 0.01) {
221 n.rotate(g.ccl(), a);
222 WVPASS(g.drivable(n)); // pass drivable border
224 for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
225 // - 0.1 -- compensate for Euclid. dist. check
227 n.x(n.x() + 0.1*cos(n.h()));
228 n.y(n.y() + 0.1*sin(n.h()));
229 n.rotate(n.ccl(), a);
230 WVPASS(g.drivable(n)); // pass near drivable border
232 for (double a = 0.1; a < M_PI / 2; a += 0.01) {
233 // = 0.1 -- compensate for near goal
235 n.x(n.x() - 0.1*cos(n.h()));
236 n.y(n.y() - 0.1*sin(n.h()));
237 n.rotate(n.ccl(), a);
238 WVPASS(!g.drivable(n)); // fail near drivable border
242 n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
246 WVPASS(g.drivable(n)); // pass backward corner case
249 n.rotate(g.ccr(), M_PI/2);
250 WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
251 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
252 tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
253 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
254 WVPASS(g.drivable(n)); // pass right corner case
257 n.rotate(g.ccl(), -M_PI/2);
258 WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
259 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
260 tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
261 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
262 WVPASS(g.drivable(n)); // pass left corner case
264 for (double a = 0; a < M_PI / 2; a += 0.01) {
266 n.rotate(g.ccr(), a);
267 WVPASS(g.drivable(n)); // pass drivable border
269 for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
270 // - 0.1 -- compensate for Euclid. dist. check
272 n.x(n.x() - 0.1*cos(n.h()));
273 n.y(n.y() - 0.1*sin(n.h()));
274 n.rotate(n.ccr(), a);
275 WVPASS(g.drivable(n)); // pass near drivable border
277 for (double a = 0.1; a < M_PI / 2; a += 0.01) {
278 // = 0.1 -- compensate for near goal
280 n.x(n.x() + 0.1*cos(n.h()));
281 n.y(n.y() + 0.1*sin(n.h()));
282 n.rotate(n.ccr(), a);
283 WVPASS(!g.drivable(n)); // fail near drivable border
285 for (double a = 0; a > -M_PI/2; a -= 0.01) {
287 n.rotate(g.ccl(), a);
288 WVPASS(g.drivable(n)); // pass drivable border
290 for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
291 // + 0.1 -- compensate for Euclid. dist. check
293 n.x(n.x() - 0.1*cos(n.h()));
294 n.y(n.y() - 0.1*sin(n.h()));
295 n.rotate(n.ccl(), a);
296 WVPASS(g.drivable(n)); // pass near drivable border
298 for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
299 // = -0.1 -- compensate for near goal
301 n.x(n.x() + 0.1*cos(n.h()));
302 n.y(n.y() + 0.1*sin(n.h()));
303 n.rotate(n.ccl(), a);
304 WVPASS(!g.drivable(n)); // fail near drivable border