ut/bcar.t.cc

   1 #include <cmath>
   2 #include "wvtest.h"
   3
   4 #include "bcar.hh"
   5
   6 using namespace bcar;
   7
   8 WVTEST_MAIN("bcar basic geometry")
   9 {
  10         BicycleCar bc;
  11         bc.x(1.0);
  12         bc.y(1.0);
  13         bc.h(M_PI / 2.0);
  14         bc.ctc(10.0);
  15         bc.wb(2.0);
  16         bc.w(1.0);
  17         bc.len(3.0);
  18         bc.df(2.0 + 0.5);
  19
  20         // car frame
  21         WVPASSEQ_DOUBLE(bc.len(), 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.0, bc.raly(), 0.00001);
  33         WVPASSEQ_DOUBLE(1.0, bc.rary(), 0.00001);
  34
  35         // min. turning radius circle centers
  36         WVPASSEQ_DOUBLE(bc.h(), M_PI / 2.0, 0.00001);
  37         WVPASSEQ_DOUBLE(-3.08257569495584, bc.ccl().x(), 0.00001);
  38         WVPASSEQ_DOUBLE(1.0000000000000004, bc.ccl().y(), 0.00001);
  39         WVPASSEQ_DOUBLE(5.08257569495584, bc.ccr().x(), 0.00001);
  40         WVPASSEQ_DOUBLE(1.0, bc.ccr().y(), 0.00001);
  41
  42         // car radiuses (inner radius, outer front radius, outer rear radius)
  43         bc.h(1.2345);
  44         WVPASSEQ_DOUBLE(bc.iradi(), 3.58257569495584, 0.00001);
  45         WVPASSEQ_DOUBLE(bc.ofradi(), 5.220153254455275, 0.00001);
  46         WVPASSEQ_DOUBLE(bc.orradi(), 4.6097722286464435, 0.00001);
  47         bc.h(M_PI / 2.0);
  48
  49         // moving
  50         bc.sp(1.0);
  51         bc.st(0.0);
  52         bc.next();
  53         WVPASSEQ_DOUBLE(1.0, bc.x(), 0.00001);
  54         WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
  55
  56         bc.set_max_steer();
  57         WVPASSEQ_DOUBLE(0.45552437743483687, bc.st(), 0.00001);
  58
  59         // rotate
  60         bc.x(-1.0);
  61         bc.y(1.0);
  62         bc.h(0.0);
  63         bc.rotate(Point(-1.0, 1.0), M_PI);
  64         WVPASSEQ_DOUBLE(-1.0, bc.x(), 0.00001);
  65         WVPASSEQ_DOUBLE(1.0, bc.y(), 0.00001);
  66         WVPASSEQ_DOUBLE(M_PI, bc.h(), 0.00001);
  67         bc.rotate(Point(0.0, 1.0), -M_PI / 2.0);
  68         WVPASSEQ_DOUBLE(0.0, bc.x(), 0.00001);
  69         WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
  70         WVPASSEQ_DOUBLE(M_PI / 2.0, bc.h(), 0.00001);
  71 }
  72
  73 WVTEST_MAIN("test collide functions")
  74 {
  75         std::vector<Point> p1;
  76         p1.push_back(Point(1.0, 1.0));
  77         p1.push_back(Point(1.0, 3.0));
  78         p1.push_back(Point(3.0, 3.0));
  79         p1.push_back(Point(3.0, 1.0));
  80         WVPASS(Point(2.0, 2.0).inside_of(p1));
  81         WVPASS(!Point(4.0, 4.0).inside_of(p1));
  82         {
  83                 auto li1 = Line(Point(1.0, 1.0), Point(3.0, 3.0));
  84                 auto li2 = Line(Point(1.0, 3.0), Point(3.0, 1.0));
  85                 WVPASS(li1.intersects_with(li2));
  86                 WVPASSEQ_DOUBLE(li1.in1().x(), 2.0, 0.00001);
  87                 WVPASSEQ_DOUBLE(li1.in1().y(), 2.0, 0.00001);
  88         }
  89         {
  90                 auto li1 = Line(Point(1.0, 1.0), Point(1.0, 3.0));
  91                 auto li2 = Line(Point(3.0, 1.0), Point(3.0, 3.0));
  92                 WVPASS(!li1.intersects_with(li2));
  93         }
  94         std::vector<Point> p2;
  95         p2.push_back(Point(2.5, 1.0));
  96         p2.push_back(Point(3.5, 3.0));
  97         p2.push_back(Point(2.0, 4.0));
  98         p2.push_back(Point(1.0, 2.0));
  99         bool col1 = false;
 100         // TODO polygon-polygon collision detection was removed
 101         col1 = true;
 102         WVPASS(col1);
 103         std::vector<Point> p3;
 104         p3.push_back(Point(2.0, 2.0));
 105         p3.push_back(Point(2.0, 0.0));
 106         p3.push_back(Point(4.0, 0.0));
 107         p3.push_back(Point(4.0, 2.0));
 108         bool col2 = false;
 109         // TODO polygon-polygon collision detection was removed
 110         col2 = false;
 111         WVPASS(!col2);
 112         auto c = Point(1.0, 1.0);
 113         auto zz = Point(0.0, 0.0);
 114         auto pp = Point(5.0, 5.0);
 115         auto mp = Point(-5.0, 5.0);
 116         auto mm = Point(-5.0, -5.0);
 117         auto pm = Point(5.0, -5.0);
 118         double r3 = 3.0;
 119         double r1 = 1.0;
 120         WVPASS(Line(zz, pp).intersects_with(c, r3));
 121         WVPASS(Line(zz, mp).intersects_with(c, r3));
 122         WVPASS(Line(zz, mm).intersects_with(c, r3));
 123         WVPASS(Line(zz, pm).intersects_with(c, r3));
 124         WVPASS(!Line(mp, pp).intersects_with(c, r1));
 125         WVPASS(!Line(mm, pm).intersects_with(c, r1));
 126         WVPASS(!Line(mm, mp).intersects_with(c, r1));
 127         WVPASS(!Line(pm, pp).intersects_with(c, r1));
 128 }
 129
 130 WVTEST_MAIN("auxiliary angle between three points")
 131 {
 132         auto zz = Point(0.0, 0.0);
 133         auto oo = Point(1.0, 1.0);
 134         auto tt = Point(2.0, 2.0);
 135         auto oz = Point(1.0, 0.0);
 136         auto zo = Point(0.0, 1.0);
 137         auto mtt = Point(-2.0, 2.0);
 138         auto moo = Point(-1.0, 1.0);
 139         auto mot = Point(-1.0, 2.0);
 140         WVPASSEQ_DOUBLE(oz.min_angle_between(zz, zo), M_PI / 2.0, 0.00001);
 141         WVPASSEQ_DOUBLE(zo.min_angle_between(zz, oz), M_PI / 2.0, 0.00001);
 142         WVPASSEQ_DOUBLE(oz.min_angle_between(zo, zz), M_PI / 4.0, 0.00001);
 143         WVPASSEQ_DOUBLE(tt.min_angle_between(oo, zz), 0.0, 0.00001);
 144         WVPASSEQ_DOUBLE(mtt.min_angle_between(moo, mot), M_PI / 4.0, 0.00001);
 145         WVPASSEQ_DOUBLE(mot.min_angle_between(moo, mtt), M_PI / 4.0, 0.00001);
 146         auto momo = Point(-1.0, -1.0);
 147         auto to = Point(2.0, 1.0);
 148         auto ot = Point(1.0, 2.0);
 149         auto omo = Point(1.0, -1.0);
 150         auto tmo = Point(2.0, -1.0);
 151         auto mto = Point(-2.0, 1.0);
 152         WVPASS(to.on_right_side_of(Line(momo, oo)));
 153         WVPASS(!ot.on_right_side_of(Line(momo, oo)));
 154         WVPASS(!to.on_right_side_of(Line(moo, omo)));
 155         WVPASS(!ot.on_right_side_of(Line(moo, omo)));
 156         WVPASS(!tmo.on_right_side_of(Line(moo, omo)));
 157         WVPASS(!mot.on_right_side_of(Line(moo, omo)));
 158         WVPASS(mto.on_right_side_of(Line(moo, omo)));
 159 }
 160
 161 WVTEST_MAIN("drivable")
 162 {
 163         double tmp_double_1 = 0;
 164         double tmp_double_2 = 0;
 165         BicycleCar g;
 166         // TODO set g.x, g.y to different values
 167         // TODO set g.h to cover all 4 quadrants
 168         BicycleCar n;
 169         n.x(g.x());
 170         n.y(g.y());
 171         n.h(g.h());
 172         WVPASS(g.drivable(n)); // pass the same pose
 173
 174         n = BicycleCar(g);
 175         n.rotate(g.ccr(), -M_PI/2);
 176         WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
 177         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 178         tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
 179         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 180         WVPASS(g.drivable(n)); // pass right corner case
 181
 182         n = BicycleCar(g);
 183         n.rotate(g.ccl(), M_PI/2);
 184         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
 185         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 186         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
 187         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 188         WVPASS(g.drivable(n)); // pass left corner case
 189         n.rotate(g.ccl(), 0.01);
 190         WVPASS(!g.drivable(n)); // fail left corner case
 191
 192         n = BicycleCar(g);
 193         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 194         n.st(0);
 195         n.next();
 196         WVPASS(g.drivable(n)); // pass forward corner case
 197
 198         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 199                 n = BicycleCar(g);
 200                 n.rotate(g.ccr(), a);
 201                 WVPASS(g.drivable(n)); // pass drivable border
 202         }
 203         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 204                 // + 0.1 -- compensate for Euclid. dist. check
 205                 n = BicycleCar(g);
 206                 n.x(n.x() + 0.1*cos(n.h()));
 207                 n.y(n.y() + 0.1*sin(n.h()));
 208                 n.rotate(n.ccr(), a);
 209                 WVPASS(g.drivable(n)); // pass near drivable border
 210         }
 211         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 212                 // = -0.1 -- compensate for near goal
 213                 n = BicycleCar(g);
 214                 n.x(n.x() - 0.1*cos(n.h()));
 215                 n.y(n.y() - 0.1*sin(n.h()));
 216                 n.rotate(n.ccr(), a);
 217                 WVPASS(!g.drivable(n)); // fail near drivable border
 218         }
 219         for (double a = 0; a < M_PI / 2; a += 0.01) {
 220                 n = BicycleCar(g);
 221                 n.rotate(g.ccl(), a);
 222                 WVPASS(g.drivable(n)); // pass drivable border
 223         }
 224         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 225                 // - 0.1 -- compensate for Euclid. dist. check
 226                 n = BicycleCar(g);
 227                 n.x(n.x() + 0.1*cos(n.h()));
 228                 n.y(n.y() + 0.1*sin(n.h()));
 229                 n.rotate(n.ccl(), a);
 230                 WVPASS(g.drivable(n)); // pass near drivable border
 231         }
 232         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 233                 // = 0.1 -- compensate for near goal
 234                 n = BicycleCar(g);
 235                 n.x(n.x() - 0.1*cos(n.h()));
 236                 n.y(n.y() - 0.1*sin(n.h()));
 237                 n.rotate(n.ccl(), a);
 238                 WVPASS(!g.drivable(n)); // fail near drivable border
 239         }
 240
 241         n = BicycleCar(g);
 242         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 243         n.sp(n.sp() * -1);
 244         n.st(0);
 245         n.next();
 246         WVPASS(g.drivable(n)); // pass backward corner case
 247
 248         n = BicycleCar(g);
 249         n.rotate(g.ccr(), M_PI/2);
 250         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
 251         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 252         tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
 253         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 254         WVPASS(g.drivable(n)); // pass right corner case
 255
 256         n = BicycleCar(g);
 257         n.rotate(g.ccl(), -M_PI/2);
 258         WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
 259         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 260         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
 261         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 262         WVPASS(g.drivable(n)); // pass left corner case
 263
 264         for (double a = 0; a < M_PI / 2; a += 0.01) {
 265                 n = BicycleCar(g);
 266                 n.rotate(g.ccr(), a);
 267                 WVPASS(g.drivable(n)); // pass drivable border
 268         }
 269         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 270                 // - 0.1 -- compensate for Euclid. dist. check
 271                 n = BicycleCar(g);
 272                 n.x(n.x() - 0.1*cos(n.h()));
 273                 n.y(n.y() - 0.1*sin(n.h()));
 274                 n.rotate(n.ccr(), a);
 275                 WVPASS(g.drivable(n)); // pass near drivable border
 276         }
 277         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 278                 // = 0.1 -- compensate for near goal
 279                 n = BicycleCar(g);
 280                 n.x(n.x() + 0.1*cos(n.h()));
 281                 n.y(n.y() + 0.1*sin(n.h()));
 282                 n.rotate(n.ccr(), a);
 283                 WVPASS(!g.drivable(n)); // fail near drivable border
 284         }
 285         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 286                 n = BicycleCar(g);
 287                 n.rotate(g.ccl(), a);
 288                 WVPASS(g.drivable(n)); // pass drivable border
 289         }
 290         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 291                 // + 0.1 -- compensate for Euclid. dist. check
 292                 n = BicycleCar(g);
 293                 n.x(n.x() - 0.1*cos(n.h()));
 294                 n.y(n.y() - 0.1*sin(n.h()));
 295                 n.rotate(n.ccl(), a);
 296                 WVPASS(g.drivable(n)); // pass near drivable border
 297         }
 298         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 299                 // = -0.1 -- compensate for near goal
 300                 n = BicycleCar(g);
 301                 n.x(n.x() + 0.1*cos(n.h()));
 302                 n.y(n.y() + 0.1*sin(n.h()));
 303                 n.rotate(n.ccl(), a);
 304                 WVPASS(!g.drivable(n)); // fail near drivable border
 305         }
 306 }