+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
#include <cmath>
#include "wvtest.h"
-#include "bcar.h"
+#include "bcar.hh"
+
+using namespace bcar;
WVTEST_MAIN("bcar basic geometry")
{
- BicycleCar bc;
- bc.x(1);
- bc.y(1);
- bc.h(M_PI / 2);
- bc.mtr(10);
- bc.wb(2);
- bc.w(1);
- bc.l(3);
- bc.he(1.5);
- bc.df(2 + 0.5);
- bc.dr(0.5);
-
- // car frame
- WVPASSEQ_DOUBLE(bc.l(), bc.df() + bc.dr(), 0.00001);
- WVPASSEQ_DOUBLE(0.5, bc.lfx(), 0.00001);
- WVPASSEQ_DOUBLE(0.5, bc.lrx(), 0.00001);
- WVPASSEQ_DOUBLE(1.5, bc.rrx(), 0.00001);
- WVPASSEQ_DOUBLE(1.5, bc.rfx(), 0.00001);
- WVPASSEQ_DOUBLE(3.5, bc.lfy(), 0.00001);
- WVPASSEQ_DOUBLE(0.5, bc.lry(), 0.00001);
- WVPASSEQ_DOUBLE(0.5, bc.rry(), 0.00001);
- WVPASSEQ_DOUBLE(3.5, bc.rfy(), 0.00001);
- WVPASSEQ_DOUBLE(0.5, bc.ralx(), 0.00001);
- WVPASSEQ_DOUBLE(1.5, bc.rarx(), 0.00001);
- WVPASSEQ_DOUBLE(1, bc.raly(), 0.00001);
- WVPASSEQ_DOUBLE(1, bc.rary(), 0.00001);
-
- // min. turning radius circle centers
- WVPASSEQ_DOUBLE(bc.h(), bc.ccl().h(), 0.00001);
- WVPASSEQ_DOUBLE(M_PI / 2, bc.ccl().h(), 0.00001);
- WVPASSEQ_DOUBLE(-9, bc.ccl().x(), 0.00001);
- WVPASSEQ_DOUBLE(1, bc.ccl().y(), 0.00001);
- WVPASSEQ_DOUBLE(bc.h(), bc.ccr().h(), 0.00001);
- WVPASSEQ_DOUBLE(M_PI / 2, bc.ccr().h(), 0.00001);
- WVPASSEQ_DOUBLE(11, bc.ccr().x(), 0.00001);
- WVPASSEQ_DOUBLE(1, bc.ccr().y(), 0.00001);
-
- // car radiuses (inner radius, outer front radius, outer rear radius)
- bc.h(1.2345);
- WVPASSEQ_DOUBLE(bc.iradi(), 9.5, 0.00001);
- WVPASSEQ_DOUBLE(bc.ofradi(), 10.793516572461451, 0.00001);
- WVPASSEQ_DOUBLE(bc.orradi(), 10.51189802081432, 0.00001);
- bc.h(M_PI / 2);
-
- // drivable
- WVPASS(!bc.drivable(bc.ccl()));
- WVPASS(!bc.drivable(bc.ccr()));
- BicycleCar bc2;
- bc2.x(1);
- bc2.y(2);
- bc2.h(M_PI / 2);
- bc2.mtr(10);
- bc2.wb(2);
- bc2.w(1);
- bc2.l(3);
- bc2.he(1.5);
- bc2.df(2 + 0.5);
- bc2.dr(0.5);
- WVPASS(bc.drivable(bc2));
- bc2.y(-2);
- WVPASS(bc.drivable(bc2));
-
- // moving
- bc.sp(1);
- bc.st(0);
- bc.next();
- WVPASSEQ_DOUBLE(1, bc.x(), 0.00001);
- WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
-
- bc.st(M_PI);
- bc.next();
- WVPASSEQ_DOUBLE(0.2, bc.st(), 0.00001);
- bc.st(-M_PI);
- bc.next();
- WVPASSEQ_DOUBLE(-0.2, bc.st(), 0.00001);
+ BicycleCar bc;
+ bc.x(1.0);
+ bc.y(1.0);
+ bc.h(M_PI / 2.0);
+ bc.ctc(10.0);
+ bc.wb(2.0);
+ bc.w(1.0);
+ bc.len(3.0);
+ bc.df(2.0 + 0.5);
+
+ // car frame
+ WVPASSEQ_DOUBLE(bc.len(), bc.df() + bc.dr(), 0.00001);
+ WVPASSEQ_DOUBLE(0.5, bc.lfx(), 0.00001);
+ WVPASSEQ_DOUBLE(0.5, bc.lrx(), 0.00001);
+ WVPASSEQ_DOUBLE(1.5, bc.rrx(), 0.00001);
+ WVPASSEQ_DOUBLE(1.5, bc.rfx(), 0.00001);
+ WVPASSEQ_DOUBLE(3.5, bc.lfy(), 0.00001);
+ WVPASSEQ_DOUBLE(0.5, bc.lry(), 0.00001);
+ WVPASSEQ_DOUBLE(0.5, bc.rry(), 0.00001);
+ WVPASSEQ_DOUBLE(3.5, bc.rfy(), 0.00001);
+ WVPASSEQ_DOUBLE(0.5, bc.lrax(), 0.00001);
+ WVPASSEQ_DOUBLE(1.5, bc.rrax(), 0.00001);
+ WVPASSEQ_DOUBLE(1.0, bc.lray(), 0.00001);
+ WVPASSEQ_DOUBLE(1.0, bc.rray(), 0.00001);
+
+ // min. turning radius circle centers
+ WVPASSEQ_DOUBLE(bc.h(), M_PI / 2.0, 0.00001);
+ WVPASSEQ_DOUBLE(-2.827076, bc.ccl().x(), 0.00001);
+ WVPASSEQ_DOUBLE(1.0000000000000004, bc.ccl().y(), 0.00001);
+ WVPASSEQ_DOUBLE(4.827076, bc.ccr().x(), 0.00001);
+ WVPASSEQ_DOUBLE(1.0, bc.ccr().y(), 0.00001);
+
+ // car radiuses (inner radius, outer front radius, outer rear radius)
+ bc.h(1.2345);
+ WVPASSEQ_DOUBLE(3.327076, bc.iradi(), 0.00001);
+ WVPASSEQ_DOUBLE(4.997358, bc.ofradi(), 0.00001);
+ WVPASSEQ_DOUBLE(4.355868, bc.orradi(), 0.00001);
+ WVPASSEQ_DOUBLE(3.485485, bc.imradi(), 0.00001);
+ WVPASSEQ_DOUBLE(5.199608, bc.omradi(), 0.00001);
+ bc.h(M_PI / 2.0);
+
+ // moving
+ bc.sp(1.0);
+ bc.st(0.0);
+ bc.next();
+ WVPASSEQ_DOUBLE(1.0, bc.x(), 0.00001);
+ WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
+
+ bc.set_max_steer();
+ WVPASSEQ_DOUBLE(0.481558, bc.st(), 0.00001);
+
+ // rotate
+ bc.x(-1.0);
+ bc.y(1.0);
+ bc.h(0.0);
+ bc.rotate(Point(-1.0, 1.0), M_PI);
+ WVPASSEQ_DOUBLE(-1.0, bc.x(), 0.00001);
+ WVPASSEQ_DOUBLE(1.0, bc.y(), 0.00001);
+ WVPASSEQ_DOUBLE(M_PI, bc.h(), 0.00001);
+ bc.rotate(Point(0.0, 1.0), -M_PI / 2.0);
+ WVPASSEQ_DOUBLE(0.0, bc.x(), 0.00001);
+ WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
+ WVPASSEQ_DOUBLE(M_PI / 2.0, bc.h(), 0.00001);
+
+ // distance to rr, lf
+ WVPASSEQ_DOUBLE(0.70710677, bc.edist_to_rr(), 10e-5);
+ WVPASSEQ_DOUBLE(bc.edist(bc.rr()), bc.edist_to_rr(), 10e-5);
+ WVPASSEQ_DOUBLE(2.5495098, bc.edist_to_lf(), 10e-5);
+ WVPASSEQ_DOUBLE(bc.edist(bc.lf()), bc.edist_to_lf(), 10e-5);
}
WVTEST_MAIN("test collide functions")
{
- std::vector<std::tuple<double, double>> slot;
- slot.push_back(std::make_tuple(1, 1));
- slot.push_back(std::make_tuple(1, 3));
- slot.push_back(std::make_tuple(3, 3));
- slot.push_back(std::make_tuple(3, 1));
- WVPASS(inside(2, 2, slot));
- WVPASS(!inside(4, 4, slot));
- auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
- WVPASS(std::get<0>(tmpi1));
- WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
- WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
- auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
- WVPASS(!std::get<0>(tmpi2));
+ std::vector<Point> p1;
+ p1.push_back(Point(1.0, 1.0));
+ p1.push_back(Point(1.0, 3.0));
+ p1.push_back(Point(3.0, 3.0));
+ p1.push_back(Point(3.0, 1.0));
+ WVPASS(Point(2.0, 2.0).inside_of(p1));
+ WVPASS(!Point(4.0, 4.0).inside_of(p1));
+ {
+ auto li1 = Line(Point(1.0, 1.0), Point(3.0, 3.0));
+ auto li2 = Line(Point(1.0, 3.0), Point(3.0, 1.0));
+ WVPASS(li1.intersects_with(li2));
+ WVPASSEQ_DOUBLE(li1.i1().x(), 2.0, 0.00001);
+ WVPASSEQ_DOUBLE(li1.i1().y(), 2.0, 0.00001);
+ }
+ {
+ auto li1 = Line(Point(1.0, 1.0), Point(1.0, 3.0));
+ auto li2 = Line(Point(3.0, 1.0), Point(3.0, 3.0));
+ WVPASS(!li1.intersects_with(li2));
+ }
+ std::vector<Point> p2;
+ p2.push_back(Point(2.5, 1.0));
+ p2.push_back(Point(3.5, 3.0));
+ p2.push_back(Point(2.0, 4.0));
+ p2.push_back(Point(1.0, 2.0));
+ bool col1 = false;
+ // TODO polygon-polygon collision detection was removed
+ col1 = true;
+ WVPASS(col1);
+ std::vector<Point> p3;
+ p3.push_back(Point(2.0, 2.0));
+ p3.push_back(Point(2.0, 0.0));
+ p3.push_back(Point(4.0, 0.0));
+ p3.push_back(Point(4.0, 2.0));
+ bool col2 = false;
+ // TODO polygon-polygon collision detection was removed
+ col2 = false;
+ WVPASS(!col2);
+ auto c = Point(1.0, 1.0);
+ auto zz = Point(0.0, 0.0);
+ auto pp = Point(5.0, 5.0);
+ auto mp = Point(-5.0, 5.0);
+ auto mm = Point(-5.0, -5.0);
+ auto pm = Point(5.0, -5.0);
+ double r3 = 3.0;
+ double r1 = 1.0;
+ WVPASS(Line(zz, pp).intersects_with(c, r3));
+ WVPASS(Line(zz, mp).intersects_with(c, r3));
+ WVPASS(Line(zz, mm).intersects_with(c, r3));
+ WVPASS(Line(zz, pm).intersects_with(c, r3));
+ WVPASS(!Line(mp, pp).intersects_with(c, r1));
+ WVPASS(!Line(mm, pm).intersects_with(c, r1));
+ WVPASS(!Line(mm, mp).intersects_with(c, r1));
+ WVPASS(!Line(pm, pp).intersects_with(c, r1));
+}
+
+WVTEST_MAIN("auxiliary angle between three points")
+{
+ auto zz = Point(0.0, 0.0);
+ auto oo = Point(1.0, 1.0);
+ auto tt = Point(2.0, 2.0);
+ auto oz = Point(1.0, 0.0);
+ auto zo = Point(0.0, 1.0);
+ auto mtt = Point(-2.0, 2.0);
+ auto moo = Point(-1.0, 1.0);
+ auto mot = Point(-1.0, 2.0);
+ WVPASSEQ_DOUBLE(oz.min_angle_between(zz, zo), M_PI / 2.0, 0.00001);
+ WVPASSEQ_DOUBLE(zo.min_angle_between(zz, oz), M_PI / 2.0, 0.00001);
+ WVPASSEQ_DOUBLE(oz.min_angle_between(zo, zz), M_PI / 4.0, 0.00001);
+ WVPASSEQ_DOUBLE(tt.min_angle_between(oo, zz), 0.0, 0.00001);
+ WVPASSEQ_DOUBLE(mtt.min_angle_between(moo, mot), M_PI / 4.0, 0.00001);
+ WVPASSEQ_DOUBLE(mot.min_angle_between(moo, mtt), M_PI / 4.0, 0.00001);
+ auto momo = Point(-1.0, -1.0);
+ auto to = Point(2.0, 1.0);
+ auto ot = Point(1.0, 2.0);
+ auto omo = Point(1.0, -1.0);
+ auto tmo = Point(2.0, -1.0);
+ auto mto = Point(-2.0, 1.0);
+ WVPASS(to.on_right_side_of(Line(momo, oo)));
+ WVPASS(!ot.on_right_side_of(Line(momo, oo)));
+ WVPASS(!to.on_right_side_of(Line(moo, omo)));
+ WVPASS(!ot.on_right_side_of(Line(moo, omo)));
+ WVPASS(!tmo.on_right_side_of(Line(moo, omo)));
+ WVPASS(!mot.on_right_side_of(Line(moo, omo)));
+ WVPASS(mto.on_right_side_of(Line(moo, omo)));
+}
+
+WVTEST_MAIN("drivable")
+{
+ double tmp_double_1 = 0;
+ double tmp_double_2 = 0;
+ BicycleCar g;
+ // TODO set g.x, g.y to different values
+ // TODO set g.h to cover all 4 quadrants
+ BicycleCar n;
+ n.x(g.x());
+ n.y(g.y());
+ n.h(g.h());
+ WVPASS(g.drivable(n)); // pass the same pose
+
+ n = BicycleCar(g);
+ n.rotate(g.ccr(), -M_PI/2);
+ WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
+ tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
+ tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
+ WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
+ WVPASS(g.drivable(n)); // pass right corner case
+ n.rotate(g.ccr(), -0.1);
+ WVPASS(!g.drivable(n)); // fail right corner case
+
+ n = BicycleCar(g);
+ n.rotate(g.ccl(), M_PI/2);
+ WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
+ tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
+ tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
+ WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
+ WVPASS(g.drivable(n)); // pass left corner case
+ n.rotate(g.ccl(), 0.1);
+ WVPASS(!g.drivable(n)); // fail left corner case
+
+ n = BicycleCar(g);
+ n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
+ n.st(0);
+ n.next();
+ WVPASS(g.drivable(n)); // pass forward corner case
+
+ for (double a = 0; a > -M_PI/2; a -= 0.01) {
+ n = BicycleCar(g);
+ n.rotate(g.ccr(), a);
+ WVPASS(g.drivable(n)); // pass drivable border
+ }
+ for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
+ // + 0.1 -- compensate for Euclid. dist. check
+ n = BicycleCar(g);
+ n.x(n.x() + 0.1*cos(n.h()));
+ n.y(n.y() + 0.1*sin(n.h()));
+ n.rotate(n.ccr(), a);
+ WVPASS(g.drivable(n)); // pass near drivable border
+ }
+ for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
+ // = -0.1 -- compensate for near goal
+ n = BicycleCar(g);
+ n.x(n.x() - 0.1*cos(n.h()));
+ n.y(n.y() - 0.1*sin(n.h()));
+ n.rotate(n.ccr(), a);
+ WVPASS(!g.drivable(n)); // fail near drivable border
+ }
+ for (double a = 0; a < M_PI / 2; a += 0.01) {
+ n = BicycleCar(g);
+ n.rotate(g.ccl(), a);
+ WVPASS(g.drivable(n)); // pass drivable border
+ }
+ for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
+ // - 0.1 -- compensate for Euclid. dist. check
+ n = BicycleCar(g);
+ n.x(n.x() + 0.1*cos(n.h()));
+ n.y(n.y() + 0.1*sin(n.h()));
+ n.rotate(n.ccl(), a);
+ WVPASS(g.drivable(n)); // pass near drivable border
+ }
+ for (double a = 0.1; a < M_PI / 2; a += 0.01) {
+ // = 0.1 -- compensate for near goal
+ n = BicycleCar(g);
+ n.x(n.x() - 0.1*cos(n.h()));
+ n.y(n.y() - 0.1*sin(n.h()));
+ n.rotate(n.ccl(), a);
+ WVPASS(!g.drivable(n)); // fail near drivable border
+ }
+
+ n = BicycleCar(g);
+ n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
+ n.sp(n.sp() * -1);
+ n.st(0);
+ n.next();
+ WVPASS(g.drivable(n)); // pass backward corner case
+
+ n = BicycleCar(g);
+ n.rotate(g.ccr(), M_PI/2);
+ WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
+ tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
+ tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
+ WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
+ WVPASS(g.drivable(n)); // pass right corner case
+
+ n = BicycleCar(g);
+ n.rotate(g.ccl(), -M_PI/2);
+ WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
+ tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
+ tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
+ WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
+ WVPASS(g.drivable(n)); // pass left corner case
+
+ for (double a = 0; a < M_PI / 2; a += 0.01) {
+ n = BicycleCar(g);
+ n.rotate(g.ccr(), a);
+ WVPASS(g.drivable(n)); // pass drivable border
+ }
+ for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
+ // - 0.1 -- compensate for Euclid. dist. check
+ n = BicycleCar(g);
+ n.x(n.x() - 0.1*cos(n.h()));
+ n.y(n.y() - 0.1*sin(n.h()));
+ n.rotate(n.ccr(), a);
+ WVPASS(g.drivable(n)); // pass near drivable border
+ }
+ for (double a = 0.1; a < M_PI / 2; a += 0.01) {
+ // = 0.1 -- compensate for near goal
+ n = BicycleCar(g);
+ n.x(n.x() + 0.1*cos(n.h()));
+ n.y(n.y() + 0.1*sin(n.h()));
+ n.rotate(n.ccr(), a);
+ WVPASS(!g.drivable(n)); // fail near drivable border
+ }
+ for (double a = 0; a > -M_PI/2; a -= 0.01) {
+ n = BicycleCar(g);
+ n.rotate(g.ccl(), a);
+ WVPASS(g.drivable(n)); // pass drivable border
+ }
+ for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
+ // + 0.1 -- compensate for Euclid. dist. check
+ n = BicycleCar(g);
+ n.x(n.x() - 0.1*cos(n.h()));
+ n.y(n.y() - 0.1*sin(n.h()));
+ n.rotate(n.ccl(), a);
+ WVPASS(g.drivable(n)); // pass near drivable border
+ }
+ for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
+ // = -0.1 -- compensate for near goal
+ n = BicycleCar(g);
+ n.x(n.x() + 0.1*cos(n.h()));
+ n.y(n.y() + 0.1*sin(n.h()));
+ n.rotate(n.ccl(), a);
+ WVPASS(!g.drivable(n)); // fail near drivable border
+ }
}
projects / hubacji1 / bcar.git / blobdiff