ut/bcar.t.cc

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