ut/bcar.t.cc

   1 #include <cmath>
   2 #include "bcar.h"
   3 #include "wvtest.h"
   4
   5 WVTEST_MAIN("bcar basic geometry")
   6 {
   7         BicycleCar *bc = new BicycleCar();
   8         bc->x(1);
   9         bc->y(1);
  10         bc->h(M_PI / 2);
  11         bc->mtr(10);
  12         bc->wb(2);
  13         bc->w(1);
  14         bc->l(3);
  15         bc->he(1.5);
  16         bc->df(2 + 0.5);
  17         bc->dr(0.5);
  18
  19         // car frame
  20         WVPASSEQ_DOUBLE(bc->l(), bc->df() + bc->dr(), 0.00001);
  21         WVPASSEQ_DOUBLE(0.5, bc->lfx(), 0.00001);
  22         WVPASSEQ_DOUBLE(0.5, bc->lrx(), 0.00001);
  23         WVPASSEQ_DOUBLE(1.5, bc->rrx(), 0.00001);
  24         WVPASSEQ_DOUBLE(1.5, bc->rfx(), 0.00001);
  25         WVPASSEQ_DOUBLE(3.5, bc->lfy(), 0.00001);
  26         WVPASSEQ_DOUBLE(0.5, bc->lry(), 0.00001);
  27         WVPASSEQ_DOUBLE(0.5, bc->rry(), 0.00001);
  28         WVPASSEQ_DOUBLE(3.5, bc->rfy(), 0.00001);
  29         WVPASSEQ_DOUBLE(0.5, bc->ralx(), 0.00001);
  30         WVPASSEQ_DOUBLE(1.5, bc->rarx(), 0.00001);
  31         WVPASSEQ_DOUBLE(1, bc->raly(), 0.00001);
  32         WVPASSEQ_DOUBLE(1, bc->rary(), 0.00001);
  33
  34         // min. turning radius circle centers
  35         WVPASSEQ_DOUBLE(bc->h(), bc->ccl()->h(), 0.00001);
  36         WVPASSEQ_DOUBLE(M_PI / 2, bc->ccl()->h(), 0.00001);
  37         WVPASSEQ_DOUBLE(-9, bc->ccl()->x(), 0.00001);
  38         WVPASSEQ_DOUBLE(1, bc->ccl()->y(), 0.00001);
  39         WVPASSEQ_DOUBLE(bc->h(), bc->ccr()->h(), 0.00001);
  40         WVPASSEQ_DOUBLE(M_PI / 2, bc->ccr()->h(), 0.00001);
  41         WVPASSEQ_DOUBLE(11, bc->ccr()->x(), 0.00001);
  42         WVPASSEQ_DOUBLE(1, bc->ccr()->y(), 0.00001);
  43
  44         // drivable
  45         WVPASS(!bc->drivable(bc->ccl()));
  46         WVPASS(!bc->drivable(bc->ccr()));
  47         BicycleCar *bc2 = new BicycleCar();
  48         bc2->x(1);
  49         bc2->y(2);
  50         bc2->h(M_PI / 2);
  51         bc2->mtr(10);
  52         bc2->wb(2);
  53         bc2->w(1);
  54         bc2->l(3);
  55         bc2->he(1.5);
  56         bc2->df(2 + 0.5);
  57         bc2->dr(0.5);
  58         WVPASS(bc->drivable(bc2));
  59         bc2->y(-2);
  60         WVPASS(bc->drivable(bc2));
  61
  62         // moving
  63         bc->sp(1);
  64         bc->st(0);
  65         bc->next();
  66         WVPASSEQ_DOUBLE(1, bc->x(), 0.00001);
  67         WVPASSEQ_DOUBLE(2, bc->y(), 0.00001);
  68
  69         bc->st(M_PI);
  70         bc->next();
  71         WVPASSEQ_DOUBLE(0.2, bc->st(), 0.00001);
  72         bc->st(-M_PI);
  73         bc->next();
  74         WVPASSEQ_DOUBLE(-0.2, bc->st(), 0.00001);
  75 }