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 }
 111
 112 WVTEST_MAIN("drivable")
 113 {
 114         double tmp_double_1 = 0;
 115         double tmp_double_2 = 0;
 116         BicycleCar g;
 117         // TODO set g.x, g.y to different values
 118         // TODO set g.h to cover all 4 quadrants
 119         BicycleCar n;
 120         n.x(g.x());
 121         n.y(g.y());
 122         n.h(g.h());
 123         WVPASS(g.drivable(n)); // pass the same pose
 124
 125         n = BicycleCar(g);
 126         n.rotate(g.ccr().x(), g.ccr().y(), -M_PI/2);
 127         WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
 128         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 129         tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
 130         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 131         WVPASS(g.drivable(n)); // pass right corner case
 132
 133         n = BicycleCar(g);
 134         n.rotate(g.ccl().x(), g.ccl().y(), M_PI/2);
 135         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
 136         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 137         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
 138         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 139         WVPASS(g.drivable(n)); // pass left corner case
 140         n.rotate(g.ccl().x(), g.ccl().y(), 0.01);
 141         WVPASS(!g.drivable(n)); // fail left corner case
 142
 143         n = BicycleCar(g);
 144         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 145         n.st(0);
 146         n.next();
 147         WVPASS(g.drivable(n)); // pass forward corner case
 148
 149         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 150                 n = BicycleCar(g);
 151                 n.rotate(g.ccr().x(), g.ccr().y(), a);
 152                 WVPASS(g.drivable(n)); // pass drivable border
 153         }
 154         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 155                 // + 0.1 -- compensate for Euclid. dist. check
 156                 n = BicycleCar(g);
 157                 n.x(n.x() + 0.1*cos(n.h()));
 158                 n.y(n.y() + 0.1*sin(n.h()));
 159                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 160                 WVPASS(g.drivable(n)); // pass near drivable border
 161         }
 162         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 163                 // = -0.1 -- compensate for near goal
 164                 n = BicycleCar(g);
 165                 n.x(n.x() - 0.1*cos(n.h()));
 166                 n.y(n.y() - 0.1*sin(n.h()));
 167                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 168                 WVPASS(!g.drivable(n)); // fail near drivable border
 169         }
 170         for (double a = 0; a < M_PI / 2; a += 0.01) {
 171                 n = BicycleCar(g);
 172                 n.rotate(g.ccl().x(), g.ccl().y(), a);
 173                 WVPASS(g.drivable(n)); // pass drivable border
 174         }
 175         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 176                 // - 0.1 -- compensate for Euclid. dist. check
 177                 n = BicycleCar(g);
 178                 n.x(n.x() + 0.1*cos(n.h()));
 179                 n.y(n.y() + 0.1*sin(n.h()));
 180                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 181                 WVPASS(g.drivable(n)); // pass near drivable border
 182         }
 183         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 184                 // = 0.1 -- compensate for near goal
 185                 n = BicycleCar(g);
 186                 n.x(n.x() - 0.1*cos(n.h()));
 187                 n.y(n.y() - 0.1*sin(n.h()));
 188                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 189                 WVPASS(!g.drivable(n)); // fail near drivable border
 190         }
 191
 192         n = BicycleCar(g);
 193         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 194         n.sp(n.sp() * -1);
 195         n.st(0);
 196         n.next();
 197         WVPASS(g.drivable(n)); // pass backward corner case
 198
 199         n = BicycleCar(g);
 200         n.rotate(g.ccr().x(), g.ccr().y(), M_PI/2);
 201         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
 202         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 203         tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
 204         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 205         WVPASS(g.drivable(n)); // pass right corner case
 206
 207         n = BicycleCar(g);
 208         n.rotate(g.ccl().x(), g.ccl().y(), -M_PI/2);
 209         WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
 210         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 211         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
 212         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 213         WVPASS(g.drivable(n)); // pass left corner case
 214
 215         for (double a = 0; a < M_PI / 2; a += 0.01) {
 216                 n = BicycleCar(g);
 217                 n.rotate(g.ccr().x(), g.ccr().y(), a);
 218                 WVPASS(g.drivable(n)); // pass drivable border
 219         }
 220         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 221                 // - 0.1 -- compensate for Euclid. dist. check
 222                 n = BicycleCar(g);
 223                 n.x(n.x() - 0.1*cos(n.h()));
 224                 n.y(n.y() - 0.1*sin(n.h()));
 225                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 226                 WVPASS(g.drivable(n)); // pass near drivable border
 227         }
 228         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 229                 // = 0.1 -- compensate for near goal
 230                 n = BicycleCar(g);
 231                 n.x(n.x() + 0.1*cos(n.h()));
 232                 n.y(n.y() + 0.1*sin(n.h()));
 233                 n.rotate(n.ccr().x(), n.ccr().y(), a);
 234                 WVPASS(!g.drivable(n)); // fail near drivable border
 235         }
 236         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 237                 n = BicycleCar(g);
 238                 n.rotate(g.ccl().x(), g.ccl().y(), a);
 239                 WVPASS(g.drivable(n)); // pass drivable border
 240         }
 241         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 242                 // + 0.1 -- compensate for Euclid. dist. check
 243                 n = BicycleCar(g);
 244                 n.x(n.x() - 0.1*cos(n.h()));
 245                 n.y(n.y() - 0.1*sin(n.h()));
 246                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 247                 WVPASS(g.drivable(n)); // pass near drivable border
 248         }
 249         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 250                 // = -0.1 -- compensate for near goal
 251                 n = BicycleCar(g);
 252                 n.x(n.x() + 0.1*cos(n.h()));
 253                 n.y(n.y() + 0.1*sin(n.h()));
 254                 n.rotate(n.ccl().x(), n.ccl().y(), a);
 255                 WVPASS(!g.drivable(n)); // fail near drivable border
 256         }
 257 }