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
 143 WVTEST_MAIN("drivable")
 144 {
 145         double tmp_double_1 = 0;
 146         double tmp_double_2 = 0;
 147         BicycleCar g;
 148         // TODO set g.x, g.y to different values
 149         // TODO set g.h to cover all 4 quadrants
 150         BicycleCar n;
 151         n.x(g.x());
 152         n.y(g.y());
 153         n.h(g.h());
 154         WVPASS(g.drivable(n)); // pass the same pose
 155
 156         n = BicycleCar(g);
 157         n.rotate(g.ccr().x(), g.ccr().y(), -M_PI/2);
 158         WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
 159         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 160         tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
 161         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 162         WVPASS(g.drivable(n)); // pass right corner case
 163
 164         n = BicycleCar(g);
 165         n.rotate(g.ccl().x(), g.ccl().y(), M_PI/2);
 166         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
 167         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 168         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
 169         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 170         WVPASS(g.drivable(n)); // pass left corner case
 171         n.rotate(g.ccl().x(), g.ccl().y(), 0.01);
 172         WVPASS(!g.drivable(n)); // fail left corner case
 173
 174         n = BicycleCar(g);
 175         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 176         n.st(0);
 177         n.next();
 178         WVPASS(g.drivable(n)); // pass forward corner case
 179
 180         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 181                 n = BicycleCar(g);
 182                 n.rotate(g.ccr().x(), g.ccr().y(), a);
 183                 WVPASS(g.drivable(n)); // pass drivable border
 184         }
 185         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 186                 // + 0.1 -- compensate for Euclid. dist. check
 187                 n = BicycleCar(g);
 188                 n.x(n.x() + 0.1*cos(n.h()));
 189                 n.y(n.y() + 0.1*sin(n.h()));
 190                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 191                 WVPASS(g.drivable(n)); // pass near drivable border
 192         }
 193         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 194                 // = -0.1 -- compensate for near goal
 195                 n = BicycleCar(g);
 196                 n.x(n.x() - 0.1*cos(n.h()));
 197                 n.y(n.y() - 0.1*sin(n.h()));
 198                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 199                 WVPASS(!g.drivable(n)); // fail near drivable border
 200         }
 201         for (double a = 0; a < M_PI / 2; a += 0.01) {
 202                 n = BicycleCar(g);
 203                 n.rotate(g.ccl().x(), g.ccl().y(), a);
 204                 WVPASS(g.drivable(n)); // pass drivable border
 205         }
 206         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 207                 // - 0.1 -- compensate for Euclid. dist. check
 208                 n = BicycleCar(g);
 209                 n.x(n.x() + 0.1*cos(n.h()));
 210                 n.y(n.y() + 0.1*sin(n.h()));
 211                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 212                 WVPASS(g.drivable(n)); // pass near drivable border
 213         }
 214         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 215                 // = 0.1 -- compensate for near goal
 216                 n = BicycleCar(g);
 217                 n.x(n.x() - 0.1*cos(n.h()));
 218                 n.y(n.y() - 0.1*sin(n.h()));
 219                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 220                 WVPASS(!g.drivable(n)); // fail near drivable border
 221         }
 222
 223         n = BicycleCar(g);
 224         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 225         n.sp(n.sp() * -1);
 226         n.st(0);
 227         n.next();
 228         WVPASS(g.drivable(n)); // pass backward corner case
 229
 230         n = BicycleCar(g);
 231         n.rotate(g.ccr().x(), g.ccr().y(), M_PI/2);
 232         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
 233         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 234         tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
 235         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 236         WVPASS(g.drivable(n)); // pass right corner case
 237
 238         n = BicycleCar(g);
 239         n.rotate(g.ccl().x(), g.ccl().y(), -M_PI/2);
 240         WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
 241         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 242         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
 243         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 244         WVPASS(g.drivable(n)); // pass left corner case
 245
 246         for (double a = 0; a < M_PI / 2; a += 0.01) {
 247                 n = BicycleCar(g);
 248                 n.rotate(g.ccr().x(), g.ccr().y(), a);
 249                 WVPASS(g.drivable(n)); // pass drivable border
 250         }
 251         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 252                 // - 0.1 -- compensate for Euclid. dist. check
 253                 n = BicycleCar(g);
 254                 n.x(n.x() - 0.1*cos(n.h()));
 255                 n.y(n.y() - 0.1*sin(n.h()));
 256                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 257                 WVPASS(g.drivable(n)); // pass near drivable border
 258         }
 259         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 260                 // = 0.1 -- compensate for near goal
 261                 n = BicycleCar(g);
 262                 n.x(n.x() + 0.1*cos(n.h()));
 263                 n.y(n.y() + 0.1*sin(n.h()));
 264                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 265                 WVPASS(!g.drivable(n)); // fail near drivable border
 266         }
 267         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 268                 n = BicycleCar(g);
 269                 n.rotate(g.ccl().x(), g.ccl().y(), a);
 270                 WVPASS(g.drivable(n)); // pass drivable border
 271         }
 272         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 273                 // + 0.1 -- compensate for Euclid. dist. check
 274                 n = BicycleCar(g);
 275                 n.x(n.x() - 0.1*cos(n.h()));
 276                 n.y(n.y() - 0.1*sin(n.h()));
 277                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 278                 WVPASS(g.drivable(n)); // pass near drivable border
 279         }
 280         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 281                 // = -0.1 -- compensate for near goal
 282                 n = BicycleCar(g);
 283                 n.x(n.x() + 0.1*cos(n.h()));
 284                 n.y(n.y() + 0.1*sin(n.h()));
 285                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 286                 WVPASS(!g.drivable(n)); // fail near drivable border
 287         }
 288 }