6 WVTEST_MAIN("bcar basic geometry")
21 WVPASSEQ_DOUBLE(bc.l(), 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, bc.raly(), 0.00001);
33 WVPASSEQ_DOUBLE(1, bc.rary(), 0.00001);
35 // min. turning radius circle centers
36 WVPASSEQ_DOUBLE(bc.h(), bc.ccl().h(), 0.00001);
37 WVPASSEQ_DOUBLE(M_PI / 2, bc.ccl().h(), 0.00001);
38 WVPASSEQ_DOUBLE(-9, bc.ccl().x(), 0.00001);
39 WVPASSEQ_DOUBLE(1, bc.ccl().y(), 0.00001);
40 WVPASSEQ_DOUBLE(bc.h(), bc.ccr().h(), 0.00001);
41 WVPASSEQ_DOUBLE(M_PI / 2, bc.ccr().h(), 0.00001);
42 WVPASSEQ_DOUBLE(11, bc.ccr().x(), 0.00001);
43 WVPASSEQ_DOUBLE(1, bc.ccr().y(), 0.00001);
45 // car radiuses (inner radius, outer front radius, outer rear radius)
47 WVPASSEQ_DOUBLE(bc.iradi(), 9.5, 0.00001);
48 WVPASSEQ_DOUBLE(bc.ofradi(), 10.793516572461451, 0.00001);
49 WVPASSEQ_DOUBLE(bc.orradi(), 10.51189802081432, 0.00001);
56 WVPASSEQ_DOUBLE(1, bc.x(), 0.00001);
57 WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
59 bc.set_max_steer();//bc.st(M_PI);
61 WVPASSEQ_DOUBLE(0.2, bc.st(), 0.01);
64 WVPASSEQ_DOUBLE(-0.2, bc.st(), 0.01);
70 bc.rotate(-1, 1, M_PI);
71 WVPASSEQ_DOUBLE(-1, bc.x(), 0.00001);
72 WVPASSEQ_DOUBLE(1, bc.y(), 0.00001);
73 WVPASSEQ_DOUBLE(M_PI, bc.h(), 0.00001);
74 bc.rotate(0, 1, -M_PI / 2);
75 WVPASSEQ_DOUBLE(0, bc.x(), 0.00001);
76 WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
77 WVPASSEQ_DOUBLE(M_PI / 2, bc.h(), 0.00001);
80 WVTEST_MAIN("test collide functions")
82 std::vector<std::tuple<double, double>> p1;
83 p1.push_back(std::make_tuple(1, 1));
84 p1.push_back(std::make_tuple(1, 3));
85 p1.push_back(std::make_tuple(3, 3));
86 p1.push_back(std::make_tuple(3, 1));
87 WVPASS(inside(2, 2, p1));
88 WVPASS(!inside(4, 4, p1));
89 auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
90 WVPASS(std::get<0>(tmpi1));
91 WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
92 WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
93 auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
94 WVPASS(!std::get<0>(tmpi2));
95 std::vector<std::tuple<double, double>> p2;
96 p2.push_back(std::make_tuple(2.5, 1));
97 p2.push_back(std::make_tuple(3.5, 3));
98 p2.push_back(std::make_tuple(2, 4));
99 p2.push_back(std::make_tuple(1, 2));
100 auto col1 = collide(p1, p2);
101 WVPASS(std::get<0>(col1));
102 WVPASSEQ(std::get<1>(col1), 0); // first segment (indexing from 0)
103 WVPASSEQ(std::get<2>(col1), 2); // the last segment
104 std::vector<std::tuple<double, double>> p3;
105 p3.push_back(std::make_tuple(2, 2));
106 p3.push_back(std::make_tuple(2, 0));
107 p3.push_back(std::make_tuple(4, 0));
108 p3.push_back(std::make_tuple(4, 2));
109 WVPASS(!std::get<0>(collide(p1, p3)));
110 auto tmpi3 = intersect(1, 1, 3, 0, 0, 5, 5);
111 WVPASS(std::get<0>(tmpi3));
112 auto tmpi4 = intersect(1, 1, 3, 0, 0, -5, 5);
113 WVPASS(std::get<0>(tmpi4));
114 auto tmpi5 = intersect(1, 1, 3, 0, 0, -5, -5);
115 WVPASS(std::get<0>(tmpi5));
116 auto tmpi6 = intersect(1, 1, 3, 0, 0, 5, -5);
117 WVPASS(std::get<0>(tmpi6));
118 auto tmpi7 = intersect(1, 1, 1, -5, 5, 5, 5);
119 WVPASS(!std::get<0>(tmpi7));
120 auto tmpi8 = intersect(1, 1, 1, -5, -5, 5, -5);
121 WVPASS(!std::get<0>(tmpi8));
122 auto tmpi9 = intersect(1, 1, 1, -5, -5, -5, 5);
123 WVPASS(!std::get<0>(tmpi9));
124 auto tmpi10 = intersect(1, 1, 1, 5, -5, 5, 5);
125 WVPASS(!std::get<0>(tmpi10));
128 WVTEST_MAIN("auxiliary angle between three points")
131 a = angle_between_three_points(1, 0, 0, 0, 0, 1);
132 WVPASSEQ_DOUBLE(a, M_PI/2, 0.00001);
133 a = angle_between_three_points(0, 1, 0, 0, 1, 0);
134 WVPASSEQ_DOUBLE(a, M_PI/2, 0.00001);
135 a = angle_between_three_points(2, 2, 1, 1, 0, 0);
136 WVPASSEQ_DOUBLE(a, 0, 0.00001);
137 a = angle_between_three_points(-2, 2, -1, 1, -1, 2);
138 WVPASSEQ_DOUBLE(a, M_PI/4, 0.00001);
139 a = angle_between_three_points(-1, 2, -1, 1, -2, 2);
140 WVPASSEQ_DOUBLE(a, M_PI/4, 0.00001);
143 r = right_side_of_line(-1, -1, 1, 1, 2, 1);
145 r = right_side_of_line(-1, -1, 1, 1, 1, 2);
147 r = right_side_of_line(-1, 1, 1, -1, 2, 1);
149 r = right_side_of_line(-1, 1, 1, -1, 1, 2);
151 r = right_side_of_line(-1, 1, 1, -1, 2, -1);
153 r = right_side_of_line(-1, 1, 1, -1, -1, 2);
155 r = right_side_of_line(-1, 1, 1, -1, -2, 1);
159 WVTEST_MAIN("drivable")
161 double tmp_double_1 = 0;
162 double tmp_double_2 = 0;
164 // TODO set g.x, g.y to different values
165 // TODO set g.h to cover all 4 quadrants
170 WVPASS(g.drivable(n)); // pass the same pose
173 n.rotate(g.ccr().x(), g.ccr().y(), -M_PI/2);
174 WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
175 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
176 tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
177 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
178 WVPASS(g.drivable(n)); // pass right corner case
181 n.rotate(g.ccl().x(), g.ccl().y(), 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 left corner case
187 n.rotate(g.ccl().x(), g.ccl().y(), 0.01);
188 WVPASS(!g.drivable(n)); // fail left corner case
191 n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
194 WVPASS(g.drivable(n)); // pass forward corner case
196 for (double a = 0; a > -M_PI/2; a -= 0.01) {
198 n.rotate(g.ccr().x(), g.ccr().y(), a);
199 WVPASS(g.drivable(n)); // pass drivable border
201 for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
202 // + 0.1 -- compensate for Euclid. dist. check
204 n.x(n.x() + 0.1*cos(n.h()));
205 n.y(n.y() + 0.1*sin(n.h()));
206 n.rotate(n.ccr().x(), n.ccr().y(), a);
207 WVPASS(g.drivable(n)); // pass near drivable border
209 for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
210 // = -0.1 -- compensate for near goal
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().x(), n.ccr().y(), a);
215 WVPASS(!g.drivable(n)); // fail near drivable border
217 for (double a = 0; a < M_PI / 2; a += 0.01) {
219 n.rotate(g.ccl().x(), g.ccl().y(), a);
220 WVPASS(g.drivable(n)); // pass drivable border
222 for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
223 // - 0.1 -- compensate for Euclid. dist. check
225 n.x(n.x() + 0.1*cos(n.h()));
226 n.y(n.y() + 0.1*sin(n.h()));
227 n.rotate(n.ccl().x(), n.ccl().y(), a);
228 WVPASS(g.drivable(n)); // pass near drivable border
230 for (double a = 0.1; a < M_PI / 2; a += 0.01) {
231 // = 0.1 -- compensate for near goal
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().x(), n.ccl().y(), a);
236 WVPASS(!g.drivable(n)); // fail near drivable border
240 n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
244 WVPASS(g.drivable(n)); // pass backward corner case
247 n.rotate(g.ccr().x(), g.ccr().y(), M_PI/2);
248 WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
249 tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
250 tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
251 WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
252 WVPASS(g.drivable(n)); // pass right corner case
255 n.rotate(g.ccl().x(), g.ccl().y(), -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 left corner case
262 for (double a = 0; a < M_PI / 2; a += 0.01) {
264 n.rotate(g.ccr().x(), g.ccr().y(), a);
265 WVPASS(g.drivable(n)); // pass drivable border
267 for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
268 // - 0.1 -- compensate for Euclid. dist. check
270 n.x(n.x() - 0.1*cos(n.h()));
271 n.y(n.y() - 0.1*sin(n.h()));
272 n.rotate(n.ccr().x(), n.ccr().y(), a);
273 WVPASS(g.drivable(n)); // pass near drivable border
275 for (double a = 0.1; a < M_PI / 2; a += 0.01) {
276 // = 0.1 -- compensate for near goal
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().x(), n.ccr().y(), a);
281 WVPASS(!g.drivable(n)); // fail near drivable border
283 for (double a = 0; a > -M_PI/2; a -= 0.01) {
285 n.rotate(g.ccl().x(), g.ccl().y(), a);
286 WVPASS(g.drivable(n)); // pass drivable border
288 for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
289 // + 0.1 -- compensate for Euclid. dist. check
291 n.x(n.x() - 0.1*cos(n.h()));
292 n.y(n.y() - 0.1*sin(n.h()));
293 n.rotate(n.ccl().x(), n.ccl().y(), a);
294 WVPASS(g.drivable(n)); // pass near drivable border
296 for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
297 // = -0.1 -- compensate for near goal
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().x(), n.ccl().y(), a);
302 WVPASS(!g.drivable(n)); // fail near drivable border