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         // drivable
  53         WVPASS(!bc.drivable(bc.ccl()));
  54         WVPASS(!bc.drivable(bc.ccr()));
  55         BicycleCar bc2;
  56         bc2.x(1);
  57         bc2.y(2);
  58         bc2.h(M_PI / 2);
  59         bc2.mtr(10);
  60         bc2.wb(2);
  61         bc2.w(1);
  62         bc2.l(3);
  63         bc2.he(1.5);
  64         bc2.df(2 + 0.5);
  65         bc2.dr(0.5);
  66         WVPASS(bc.drivable(bc2));
  67         bc2.y(-2);
  68         WVPASS(bc.drivable(bc2));
  69
  70         // moving
  71         bc.sp(1);
  72         bc.st(0);
  73         bc.next();
  74         WVPASSEQ_DOUBLE(1, bc.x(), 0.00001);
  75         WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
  76
  77         bc.st(M_PI);
  78         bc.next();
  79         WVPASSEQ_DOUBLE(0.2, bc.st(), 0.00001);
  80         bc.st(-M_PI);
  81         bc.next();
  82         WVPASSEQ_DOUBLE(-0.2, bc.st(), 0.00001);
  83 }
  84
  85 WVTEST_MAIN("test collide functions")
  86 {
  87         std::vector<std::tuple<double, double>> p1;
  88         p1.push_back(std::make_tuple(1, 1));
  89         p1.push_back(std::make_tuple(1, 3));
  90         p1.push_back(std::make_tuple(3, 3));
  91         p1.push_back(std::make_tuple(3, 1));
  92         WVPASS(inside(2, 2, p1));
  93         WVPASS(!inside(4, 4, p1));
  94         auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
  95         WVPASS(std::get<0>(tmpi1));
  96         WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
  97         WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
  98         auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
  99         WVPASS(!std::get<0>(tmpi2));
 100         std::vector<std::tuple<double, double>> p2;
 101         p2.push_back(std::make_tuple(2.5, 1));
 102         p2.push_back(std::make_tuple(3.5, 3));
 103         p2.push_back(std::make_tuple(2, 4));
 104         p2.push_back(std::make_tuple(1, 2));
 105         auto col1 = collide(p1, p2);
 106         WVPASS(std::get<0>(col1));
 107         WVPASSEQ(std::get<1>(col1), 0); // first segment (indexing from 0)
 108         WVPASSEQ(std::get<2>(col1), 2); // the last segment
 109         std::vector<std::tuple<double, double>> p3;
 110         p3.push_back(std::make_tuple(2, 2));
 111         p3.push_back(std::make_tuple(2, 0));
 112         p3.push_back(std::make_tuple(4, 0));
 113         p3.push_back(std::make_tuple(4, 2));
 114         WVPASS(!std::get<0>(collide(p1, p3)));
 115 }