ut/bcar.t.cc

   1 #include <cmath>
   2 #include "wvtest.h"
   3
   4 #include "bcar.h"
   5
   6 WVTEST_MAIN("bcar basic geometry")
   7 {
   8         BicycleCar bc;
   9         bc.x(1);
  10         bc.y(1);
  11         bc.h(M_PI / 2);
  12         bc.mtr(10);
  13         bc.wb(2);
  14         bc.w(1);
  15         bc.l(3);
  16         bc.he(1.5);
  17         bc.df(2 + 0.5);
  18         bc.dr(0.5);
  19
  20         // car frame
  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);
  34
  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);
  44
  45         // car radiuses (inner radius, outer front radius, outer rear radius)
  46         bc.h(1.2345);
  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);
  50         bc.h(M_PI / 2);
  51
  52         // moving
  53         bc.sp(1);
  54         bc.st(0);
  55         bc.next();
  56         WVPASSEQ_DOUBLE(1, bc.x(), 0.00001);
  57         WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
  58
  59         bc.set_max_steer();//bc.st(M_PI);
  60         bc.next();
  61         WVPASSEQ_DOUBLE(0.2, bc.st(), 0.01);
  62         bc.st(bc.st() * -1);
  63         bc.next();
  64         WVPASSEQ_DOUBLE(-0.2, bc.st(), 0.01);
  65
  66         // rotate
  67         bc.x(-1);
  68         bc.y(1);
  69         bc.h(0);
  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);
  78 }
  79
  80 WVTEST_MAIN("test collide functions")
  81 {
  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));
 126 }
 127
 128 WVTEST_MAIN("auxiliary angle between three points")
 129 {
 130         double a;
 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);
 141
 142         bool r;
 143         r = right_side_of_line(-1, -1, 1, 1, 2, 1);
 144         WVPASS(r);
 145         r = right_side_of_line(-1, -1, 1, 1, 1, 2);
 146         WVPASS(!r);
 147         r = right_side_of_line(-1, 1, 1, -1, 2, 1);
 148         WVPASS(!r);
 149         r = right_side_of_line(-1, 1, 1, -1, 1, 2);
 150         WVPASS(!r);
 151         r = right_side_of_line(-1, 1, 1, -1, 2, -1);
 152         WVPASS(!r);
 153         r = right_side_of_line(-1, 1, 1, -1, -1, 2);
 154         WVPASS(!r);
 155         r = right_side_of_line(-1, 1, 1, -1, -2, 1);
 156         WVPASS(r);
 157 }
 158
 159 WVTEST_MAIN("drivable")
 160 {
 161         double tmp_double_1 = 0;
 162         double tmp_double_2 = 0;
 163         BicycleCar g;
 164         // TODO set g.x, g.y to different values
 165         // TODO set g.h to cover all 4 quadrants
 166         BicycleCar n;
 167         n.x(g.x());
 168         n.y(g.y());
 169         n.h(g.h());
 170         WVPASS(g.drivable(n)); // pass the same pose
 171
 172         n = BicycleCar(g);
 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
 179
 180         n = BicycleCar(g);
 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
 189
 190         n = BicycleCar(g);
 191         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 192         n.st(0);
 193         n.next();
 194         WVPASS(g.drivable(n)); // pass forward corner case
 195
 196         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 197                 n = BicycleCar(g);
 198                 n.rotate(g.ccr().x(), g.ccr().y(), a);
 199                 WVPASS(g.drivable(n)); // pass drivable border
 200         }
 201         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 202                 // + 0.1 -- compensate for Euclid. dist. check
 203                 n = BicycleCar(g);
 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
 208         }
 209         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 210                 // = -0.1 -- compensate for near goal
 211                 n = BicycleCar(g);
 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
 216         }
 217         for (double a = 0; a < M_PI / 2; a += 0.01) {
 218                 n = BicycleCar(g);
 219                 n.rotate(g.ccl().x(), g.ccl().y(), a);
 220                 WVPASS(g.drivable(n)); // pass drivable border
 221         }
 222         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 223                 // - 0.1 -- compensate for Euclid. dist. check
 224                 n = BicycleCar(g);
 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
 229         }
 230         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 231                 // = 0.1 -- compensate for near goal
 232                 n = BicycleCar(g);
 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
 237         }
 238
 239         n = BicycleCar(g);
 240         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 241         n.sp(n.sp() * -1);
 242         n.st(0);
 243         n.next();
 244         WVPASS(g.drivable(n)); // pass backward corner case
 245
 246         n = BicycleCar(g);
 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
 253
 254         n = BicycleCar(g);
 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
 261
 262         for (double a = 0; a < M_PI / 2; a += 0.01) {
 263                 n = BicycleCar(g);
 264                 n.rotate(g.ccr().x(), g.ccr().y(), a);
 265                 WVPASS(g.drivable(n)); // pass drivable border
 266         }
 267         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 268                 // - 0.1 -- compensate for Euclid. dist. check
 269                 n = BicycleCar(g);
 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
 274         }
 275         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 276                 // = 0.1 -- compensate for near goal
 277                 n = BicycleCar(g);
 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
 282         }
 283         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 284                 n = BicycleCar(g);
 285                 n.rotate(g.ccl().x(), g.ccl().y(), a);
 286                 WVPASS(g.drivable(n)); // pass drivable border
 287         }
 288         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 289                 // + 0.1 -- compensate for Euclid. dist. check
 290                 n = BicycleCar(g);
 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
 295         }
 296         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 297                 // = -0.1 -- compensate for near goal
 298                 n = BicycleCar(g);
 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
 303         }
 304 }