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