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 }