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.ralx(), 0.00001);
  37         WVPASSEQ_DOUBLE(1.5, bc.rarx(), 0.00001);
  38         WVPASSEQ_DOUBLE(1.0, bc.raly(), 0.00001);
  39         WVPASSEQ_DOUBLE(1.0, bc.rary(), 0.00001);
  40
  41         // min. turning radius circle centers
  42         WVPASSEQ_DOUBLE(bc.h(), M_PI / 2.0, 0.00001);
  43         WVPASSEQ_DOUBLE(-3.08257569495584, bc.ccl().x(), 0.00001);
  44         WVPASSEQ_DOUBLE(1.0000000000000004, bc.ccl().y(), 0.00001);
  45         WVPASSEQ_DOUBLE(5.08257569495584, 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(bc.iradi(), 3.58257569495584, 0.00001);
  51         WVPASSEQ_DOUBLE(bc.ofradi(), 5.220153254455275, 0.00001);
  52         WVPASSEQ_DOUBLE(bc.orradi(), 4.6097722286464435, 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.45552437743483687, 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
 188         n = BicycleCar(g);
 189         n.rotate(g.ccl(), 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 left corner case
 195         n.rotate(g.ccl(), 0.01);
 196         WVPASS(!g.drivable(n)); // fail left corner case
 197
 198         n = BicycleCar(g);
 199         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 200         n.st(0);
 201         n.next();
 202         WVPASS(g.drivable(n)); // pass forward corner case
 203
 204         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 205                 n = BicycleCar(g);
 206                 n.rotate(g.ccr(), a);
 207                 WVPASS(g.drivable(n)); // pass drivable border
 208         }
 209         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 210                 // + 0.1 -- compensate for Euclid. dist. check
 211                 n = BicycleCar(g);
 212                 n.x(n.x() + 0.1*cos(n.h()));
 213                 n.y(n.y() + 0.1*sin(n.h()));
 214                 n.rotate(n.ccr(), a);
 215                 WVPASS(g.drivable(n)); // pass near drivable border
 216         }
 217         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 218                 // = -0.1 -- compensate for near goal
 219                 n = BicycleCar(g);
 220                 n.x(n.x() - 0.1*cos(n.h()));
 221                 n.y(n.y() - 0.1*sin(n.h()));
 222                 n.rotate(n.ccr(), a);
 223                 WVPASS(!g.drivable(n)); // fail near drivable border
 224         }
 225         for (double a = 0; a < M_PI / 2; a += 0.01) {
 226                 n = BicycleCar(g);
 227                 n.rotate(g.ccl(), a);
 228                 WVPASS(g.drivable(n)); // pass drivable border
 229         }
 230         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 231                 // - 0.1 -- compensate for Euclid. dist. check
 232                 n = BicycleCar(g);
 233                 n.x(n.x() + 0.1*cos(n.h()));
 234                 n.y(n.y() + 0.1*sin(n.h()));
 235                 n.rotate(n.ccl(), a);
 236                 WVPASS(g.drivable(n)); // pass near drivable border
 237         }
 238         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 239                 // = 0.1 -- compensate for near goal
 240                 n = BicycleCar(g);
 241                 n.x(n.x() - 0.1*cos(n.h()));
 242                 n.y(n.y() - 0.1*sin(n.h()));
 243                 n.rotate(n.ccl(), a);
 244                 WVPASS(!g.drivable(n)); // fail near drivable border
 245         }
 246
 247         n = BicycleCar(g);
 248         n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
 249         n.sp(n.sp() * -1);
 250         n.st(0);
 251         n.next();
 252         WVPASS(g.drivable(n)); // pass backward corner case
 253
 254         n = BicycleCar(g);
 255         n.rotate(g.ccr(), M_PI/2);
 256         WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
 257         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 258         tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
 259         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 260         WVPASS(g.drivable(n)); // pass right corner case
 261
 262         n = BicycleCar(g);
 263         n.rotate(g.ccl(), -M_PI/2);
 264         WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
 265         tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
 266         tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
 267         WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
 268         WVPASS(g.drivable(n)); // pass left corner case
 269
 270         for (double a = 0; a < M_PI / 2; a += 0.01) {
 271                 n = BicycleCar(g);
 272                 n.rotate(g.ccr(), a);
 273                 WVPASS(g.drivable(n)); // pass drivable border
 274         }
 275         for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
 276                 // - 0.1 -- compensate for Euclid. dist. check
 277                 n = BicycleCar(g);
 278                 n.x(n.x() - 0.1*cos(n.h()));
 279                 n.y(n.y() - 0.1*sin(n.h()));
 280                 n.rotate(n.ccr(), a);
 281                 WVPASS(g.drivable(n)); // pass near drivable border
 282         }
 283         for (double a = 0.1; a < M_PI / 2; a += 0.01) {
 284                 // = 0.1 -- compensate for near goal
 285                 n = BicycleCar(g);
 286                 n.x(n.x() + 0.1*cos(n.h()));
 287                 n.y(n.y() + 0.1*sin(n.h()));
 288                 n.rotate(n.ccr(), a);
 289                 WVPASS(!g.drivable(n)); // fail near drivable border
 290         }
 291         for (double a = 0; a > -M_PI/2; a -= 0.01) {
 292                 n = BicycleCar(g);
 293                 n.rotate(g.ccl(), a);
 294                 WVPASS(g.drivable(n)); // pass drivable border
 295         }
 296         for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
 297                 // + 0.1 -- compensate for Euclid. dist. check
 298                 n = BicycleCar(g);
 299                 n.x(n.x() - 0.1*cos(n.h()));
 300                 n.y(n.y() - 0.1*sin(n.h()));
 301                 n.rotate(n.ccl(), a);
 302                 WVPASS(g.drivable(n)); // pass near drivable border
 303         }
 304         for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
 305                 // = -0.1 -- compensate for near goal
 306                 n = BicycleCar(g);
 307                 n.x(n.x() + 0.1*cos(n.h()));
 308                 n.y(n.y() + 0.1*sin(n.h()));
 309                 n.rotate(n.ccl(), a);
 310                 WVPASS(!g.drivable(n)); // fail near drivable border
 311         }
 312 }