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         WVPASSEQ_DOUBLE(3.485485, bc.imradi(), 0.00001);
  54         WVPASSEQ_DOUBLE(5.199608, bc.omradi(), 0.00001);
  55         bc.h(M_PI / 2.0);
  56
  57         // moving
  58         bc.sp(1.0);
  59         bc.st(0.0);
  60         bc.next();
  61         WVPASSEQ_DOUBLE(1.0, bc.x(), 0.00001);
  62         WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
  63
  64         bc.set_max_steer();
  65         WVPASSEQ_DOUBLE(0.481558, bc.st(), 0.00001);
  66
  67         // rotate
  68         bc.x(-1.0);
  69         bc.y(1.0);
  70         bc.h(0.0);
  71         bc.rotate(Point(-1.0, 1.0), M_PI);
  72         WVPASSEQ_DOUBLE(-1.0, bc.x(), 0.00001);
  73         WVPASSEQ_DOUBLE(1.0, bc.y(), 0.00001);
  74         WVPASSEQ_DOUBLE(M_PI, bc.h(), 0.00001);
  75         bc.rotate(Point(0.0, 1.0), -M_PI / 2.0);
  76         WVPASSEQ_DOUBLE(0.0, bc.x(), 0.00001);
  77         WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
  78         WVPASSEQ_DOUBLE(M_PI / 2.0, bc.h(), 0.00001);
  79 }
  80
  81 WVTEST_MAIN("test collide functions")
  82 {
  83         std::vector<Point> p1;
  84         p1.push_back(Point(1.0, 1.0));
  85         p1.push_back(Point(1.0, 3.0));
  86         p1.push_back(Point(3.0, 3.0));
  87         p1.push_back(Point(3.0, 1.0));
  88         WVPASS(Point(2.0, 2.0).inside_of(p1));
  89         WVPASS(!Point(4.0, 4.0).inside_of(p1));
  90         {
  91                 auto li1 = Line(Point(1.0, 1.0), Point(3.0, 3.0));
  92                 auto li2 = Line(Point(1.0, 3.0), Point(3.0, 1.0));
  93                 WVPASS(li1.intersects_with(li2));
  94                 WVPASSEQ_DOUBLE(li1.i1().x(), 2.0, 0.00001);
  95                 WVPASSEQ_DOUBLE(li1.i1().y(), 2.0, 0.00001);
  96         }
  97         {
  98                 auto li1 = Line(Point(1.0, 1.0), Point(1.0, 3.0));
  99                 auto li2 = Line(Point(3.0, 1.0), Point(3.0, 3.0));
 100                 WVPASS(!li1.intersects_with(li2));
 101         }
 102         std::vector<Point> p2;
 103         p2.push_back(Point(2.5, 1.0));
 104         p2.push_back(Point(3.5, 3.0));
 105         p2.push_back(Point(2.0, 4.0));
 106         p2.push_back(Point(1.0, 2.0));
 107         bool col1 = false;
 108         // TODO polygon-polygon collision detection was removed
 109         col1 = true;
 110         WVPASS(col1);
 111         std::vector<Point> p3;
 112         p3.push_back(Point(2.0, 2.0));
 113         p3.push_back(Point(2.0, 0.0));
 114         p3.push_back(Point(4.0, 0.0));
 115         p3.push_back(Point(4.0, 2.0));
 116         bool col2 = false;
 117         // TODO polygon-polygon collision detection was removed
 118         col2 = false;
 119         WVPASS(!col2);
 120         auto c = Point(1.0, 1.0);
 121         auto zz = Point(0.0, 0.0);
 122         auto pp = Point(5.0, 5.0);
 123         auto mp = Point(-5.0, 5.0);
 124         auto mm = Point(-5.0, -5.0);
 125         auto pm = Point(5.0, -5.0);
 126         double r3 = 3.0;
 127         double r1 = 1.0;
 128         WVPASS(Line(zz, pp).intersects_with(c, r3));
 129         WVPASS(Line(zz, mp).intersects_with(c, r3));
 130         WVPASS(Line(zz, mm).intersects_with(c, r3));
 131         WVPASS(Line(zz, pm).intersects_with(c, r3));
 132         WVPASS(!Line(mp, pp).intersects_with(c, r1));
 133         WVPASS(!Line(mm, pm).intersects_with(c, r1));
 134         WVPASS(!Line(mm, mp).intersects_with(c, r1));
 135         WVPASS(!Line(pm, pp).intersects_with(c, r1));
 136 }
 137
 138 WVTEST_MAIN("auxiliary angle between three points")
 139 {
 140         auto zz = Point(0.0, 0.0);
 141         auto oo = Point(1.0, 1.0);
 142         auto tt = Point(2.0, 2.0);
 143         auto oz = Point(1.0, 0.0);
 144         auto zo = Point(0.0, 1.0);
 145         auto mtt = Point(-2.0, 2.0);
 146         auto moo = Point(-1.0, 1.0);
 147         auto mot = Point(-1.0, 2.0);
 148         WVPASSEQ_DOUBLE(oz.min_angle_between(zz, zo), M_PI / 2.0, 0.00001);
 149         WVPASSEQ_DOUBLE(zo.min_angle_between(zz, oz), M_PI / 2.0, 0.00001);
 150         WVPASSEQ_DOUBLE(oz.min_angle_between(zo, zz), M_PI / 4.0, 0.00001);
 151         WVPASSEQ_DOUBLE(tt.min_angle_between(oo, zz), 0.0, 0.00001);
 152         WVPASSEQ_DOUBLE(mtt.min_angle_between(moo, mot), M_PI / 4.0, 0.00001);
 153         WVPASSEQ_DOUBLE(mot.min_angle_between(moo, mtt), M_PI / 4.0, 0.00001);
 154         auto momo = Point(-1.0, -1.0);
 155         auto to = Point(2.0, 1.0);
 156         auto ot = Point(1.0, 2.0);
 157         auto omo = Point(1.0, -1.0);
 158         auto tmo = Point(2.0, -1.0);
 159         auto mto = Point(-2.0, 1.0);
 160         WVPASS(to.on_right_side_of(Line(momo, oo)));
 161         WVPASS(!ot.on_right_side_of(Line(momo, oo)));
 162         WVPASS(!to.on_right_side_of(Line(moo, omo)));
 163         WVPASS(!ot.on_right_side_of(Line(moo, omo)));
 164         WVPASS(!tmo.on_right_side_of(Line(moo, omo)));
 165         WVPASS(!mot.on_right_side_of(Line(moo, omo)));
 166         WVPASS(mto.on_right_side_of(Line(moo, omo)));
 167 }
 168
 169 WVTEST_MAIN("drivable")
 170 {
 171         double tmp_double_1 = 0;
 172         double tmp_double_2 = 0;
 173         BicycleCar g;
 174         // TODO set g.x, g.y to different values
 175         // TODO set g.h to cover all 4 quadrants
 176         BicycleCar n;
 177         n.x(g.x());
 178         n.y(g.y());
 179         n.h(g.h());
 180         WVPASS(g.drivable(n)); // pass the same pose
 181
 182         n = BicycleCar(g);
 183         n.rotate(g.ccr(), -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 right corner case
 189         n.rotate(g.ccr(), -0.1);
 190         WVPASS(!g.drivable(n)); // fail right corner case
 191
 192         n = BicycleCar(g);
 193         n.rotate(g.ccl(), M_PI/2);
 194         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
 195         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 196         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
 197         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 198         WVPASS(g.drivable(n)); // pass left corner case
 199         n.rotate(g.ccl(), 0.1);
 200         WVPASS(!g.drivable(n)); // fail left corner case
 201
 202         n = BicycleCar(g);
 203         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 204         n.st(0);
 205         n.next();
 206         WVPASS(g.drivable(n)); // pass forward corner case
 207
 208         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 209                 n = BicycleCar(g);
 210                 n.rotate(g.ccr(), a);
 211                 WVPASS(g.drivable(n)); // pass drivable border
 212         }
 213         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 214                 // + 0.1 -- compensate for Euclid. dist. check
 215                 n = BicycleCar(g);
 216                 n.x(n.x() + 0.1*cos(n.h()));
 217                 n.y(n.y() + 0.1*sin(n.h()));
 218                 n.rotate(n.ccr(), a);
 219                 WVPASS(g.drivable(n)); // pass near drivable border
 220         }
 221         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 222                 // = -0.1 -- compensate for near goal
 223                 n = BicycleCar(g);
 224                 n.x(n.x() - 0.1*cos(n.h()));
 225                 n.y(n.y() - 0.1*sin(n.h()));
 226                 n.rotate(n.ccr(), a);
 227                 WVPASS(!g.drivable(n)); // fail near drivable border
 228         }
 229         for (double a = 0; a < M_PI / 2; a += 0.01) {
 230                 n = BicycleCar(g);
 231                 n.rotate(g.ccl(), a);
 232                 WVPASS(g.drivable(n)); // pass drivable border
 233         }
 234         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 235                 // - 0.1 -- compensate for Euclid. dist. check
 236                 n = BicycleCar(g);
 237                 n.x(n.x() + 0.1*cos(n.h()));
 238                 n.y(n.y() + 0.1*sin(n.h()));
 239                 n.rotate(n.ccl(), a);
 240                 WVPASS(g.drivable(n)); // pass near drivable border
 241         }
 242         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 243                 // = 0.1 -- compensate for near goal
 244                 n = BicycleCar(g);
 245                 n.x(n.x() - 0.1*cos(n.h()));
 246                 n.y(n.y() - 0.1*sin(n.h()));
 247                 n.rotate(n.ccl(), a);
 248                 WVPASS(!g.drivable(n)); // fail near drivable border
 249         }
 250
 251         n = BicycleCar(g);
 252         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 253         n.sp(n.sp() * -1);
 254         n.st(0);
 255         n.next();
 256         WVPASS(g.drivable(n)); // pass backward corner case
 257
 258         n = BicycleCar(g);
 259         n.rotate(g.ccr(), M_PI/2);
 260         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
 261         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 262         tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
 263         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 264         WVPASS(g.drivable(n)); // pass right corner case
 265
 266         n = BicycleCar(g);
 267         n.rotate(g.ccl(), -M_PI/2);
 268         WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
 269         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 270         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
 271         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 272         WVPASS(g.drivable(n)); // pass left corner case
 273
 274         for (double a = 0; a < M_PI / 2; a += 0.01) {
 275                 n = BicycleCar(g);
 276                 n.rotate(g.ccr(), a);
 277                 WVPASS(g.drivable(n)); // pass drivable border
 278         }
 279         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 280                 // - 0.1 -- compensate for Euclid. dist. check
 281                 n = BicycleCar(g);
 282                 n.x(n.x() - 0.1*cos(n.h()));
 283                 n.y(n.y() - 0.1*sin(n.h()));
 284                 n.rotate(n.ccr(), a);
 285                 WVPASS(g.drivable(n)); // pass near drivable border
 286         }
 287         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 288                 // = 0.1 -- compensate for near goal
 289                 n = BicycleCar(g);
 290                 n.x(n.x() + 0.1*cos(n.h()));
 291                 n.y(n.y() + 0.1*sin(n.h()));
 292                 n.rotate(n.ccr(), a);
 293                 WVPASS(!g.drivable(n)); // fail near drivable border
 294         }
 295         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 296                 n = BicycleCar(g);
 297                 n.rotate(g.ccl(), a);
 298                 WVPASS(g.drivable(n)); // pass drivable border
 299         }
 300         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 301                 // + 0.1 -- compensate for Euclid. dist. check
 302                 n = BicycleCar(g);
 303                 n.x(n.x() - 0.1*cos(n.h()));
 304                 n.y(n.y() - 0.1*sin(n.h()));
 305                 n.rotate(n.ccl(), a);
 306                 WVPASS(g.drivable(n)); // pass near drivable border
 307         }
 308         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 309                 // = -0.1 -- compensate for near goal
 310                 n = BicycleCar(g);
 311                 n.x(n.x() + 0.1*cos(n.h()));
 312                 n.y(n.y() + 0.1*sin(n.h()));
 313                 n.rotate(n.ccl(), a);
 314                 WVPASS(!g.drivable(n)); // fail near drivable border
 315         }
 316 }