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