+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
#include <cmath>
#include "wvtest.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);
+ 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.l(), bc.df() + bc.dr(), 0.00001);
+ 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(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);
+ 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(), 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);
+ 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(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);
+ 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);
- bc.st(0);
+ bc.sp(1.0);
+ bc.st(0.0);
bc.next();
- WVPASSEQ_DOUBLE(1, bc.x(), 0.00001);
- WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
+ WVPASSEQ_DOUBLE(1.0, bc.x(), 0.00001);
+ WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
- bc.set_max_steer();//bc.st(M_PI);
- bc.next();
- WVPASSEQ_DOUBLE(0.2, bc.st(), 0.01);
- bc.st(bc.st() * -1);
- bc.next();
- WVPASSEQ_DOUBLE(-0.2, bc.st(), 0.01);
+ bc.set_max_steer();
+ WVPASSEQ_DOUBLE(0.481558, bc.st(), 0.00001);
// rotate
- bc.x(-1);
- bc.y(1);
- bc.h(0);
- bc.rotate(-1, 1, M_PI);
- WVPASSEQ_DOUBLE(-1, bc.x(), 0.00001);
- WVPASSEQ_DOUBLE(1, bc.y(), 0.00001);
+ 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(0, 1, -M_PI / 2);
- WVPASSEQ_DOUBLE(0, bc.x(), 0.00001);
- WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
- WVPASSEQ_DOUBLE(M_PI / 2, 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>> p1;
- p1.push_back(std::make_tuple(1, 1));
- p1.push_back(std::make_tuple(1, 3));
- p1.push_back(std::make_tuple(3, 3));
- p1.push_back(std::make_tuple(3, 1));
- WVPASS(inside(2, 2, p1));
- WVPASS(!inside(4, 4, p1));
- 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<std::tuple<double, double>> p2;
- p2.push_back(std::make_tuple(2.5, 1));
- p2.push_back(std::make_tuple(3.5, 3));
- p2.push_back(std::make_tuple(2, 4));
- p2.push_back(std::make_tuple(1, 2));
- auto col1 = collide(p1, p2);
- WVPASS(std::get<0>(col1));
- WVPASSEQ(std::get<1>(col1), 0); // first segment (indexing from 0)
- WVPASSEQ(std::get<2>(col1), 2); // the last segment
- std::vector<std::tuple<double, double>> p3;
- p3.push_back(std::make_tuple(2, 2));
- p3.push_back(std::make_tuple(2, 0));
- p3.push_back(std::make_tuple(4, 0));
- p3.push_back(std::make_tuple(4, 2));
- WVPASS(!std::get<0>(collide(p1, p3)));
- auto tmpi3 = intersect(1, 1, 3, 0, 0, 5, 5);
- WVPASS(std::get<0>(tmpi3));
- auto tmpi4 = intersect(1, 1, 3, 0, 0, -5, 5);
- WVPASS(std::get<0>(tmpi4));
- auto tmpi5 = intersect(1, 1, 3, 0, 0, -5, -5);
- WVPASS(std::get<0>(tmpi5));
- auto tmpi6 = intersect(1, 1, 3, 0, 0, 5, -5);
- WVPASS(std::get<0>(tmpi6));
- auto tmpi7 = intersect(1, 1, 1, -5, 5, 5, 5);
- WVPASS(!std::get<0>(tmpi7));
- auto tmpi8 = intersect(1, 1, 1, -5, -5, 5, -5);
- WVPASS(!std::get<0>(tmpi8));
- auto tmpi9 = intersect(1, 1, 1, -5, -5, -5, 5);
- WVPASS(!std::get<0>(tmpi9));
- auto tmpi10 = intersect(1, 1, 1, 5, -5, 5, 5);
- WVPASS(!std::get<0>(tmpi10));
+ 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")
{
- double a;
- a = angle_between_three_points(1, 0, 0, 0, 0, 1);
- WVPASSEQ_DOUBLE(a, M_PI/2, 0.00001);
- a = angle_between_three_points(0, 1, 0, 0, 1, 0);
- WVPASSEQ_DOUBLE(a, M_PI/2, 0.00001);
- a = angle_between_three_points(2, 2, 1, 1, 0, 0);
- WVPASSEQ_DOUBLE(a, 0, 0.00001);
- a = angle_between_three_points(-2, 2, -1, 1, -1, 2);
- WVPASSEQ_DOUBLE(a, M_PI/4, 0.00001);
- a = angle_between_three_points(-1, 2, -1, 1, -2, 2);
- WVPASSEQ_DOUBLE(a, M_PI/4, 0.00001);
-
- bool r;
- r = right_side_of_line(-1, -1, 1, 1, 2, 1);
- WVPASS(r);
- r = right_side_of_line(-1, -1, 1, 1, 1, 2);
- WVPASS(!r);
- r = right_side_of_line(-1, 1, 1, -1, 2, 1);
- WVPASS(!r);
- r = right_side_of_line(-1, 1, 1, -1, 1, 2);
- WVPASS(!r);
- r = right_side_of_line(-1, 1, 1, -1, 2, -1);
- WVPASS(!r);
- r = right_side_of_line(-1, 1, 1, -1, -1, 2);
- WVPASS(!r);
- r = right_side_of_line(-1, 1, 1, -1, -2, 1);
- WVPASS(r);
+ 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")
WVPASS(g.drivable(n)); // pass the same pose
n = BicycleCar(g);
- n.rotate(g.ccr().x(), g.ccr().y(), -M_PI/2);
+ 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().x(), g.ccl().y(), M_PI/2);
+ 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().x(), g.ccl().y(), 0.01);
+ n.rotate(g.ccl(), 0.1);
WVPASS(!g.drivable(n)); // fail left corner case
n = BicycleCar(g);
for (double a = 0; a > -M_PI/2; a -= 0.01) {
n = BicycleCar(g);
- n.rotate(g.ccr().x(), g.ccr().y(), a);
+ 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) {
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().x(), n.ccr().y(), a);
+ 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) {
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().x(), n.ccr().y(), a);
+ 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().x(), g.ccl().y(), a);
+ 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) {
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().x(), n.ccl().y(), a);
+ 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) {
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().x(), n.ccl().y(), a);
+ n.rotate(n.ccl(), a);
WVPASS(!g.drivable(n)); // fail near drivable border
}
WVPASS(g.drivable(n)); // pass backward corner case
n = BicycleCar(g);
- n.rotate(g.ccr().x(), g.ccr().y(), M_PI/2);
+ 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));
WVPASS(g.drivable(n)); // pass right corner case
n = BicycleCar(g);
- n.rotate(g.ccl().x(), g.ccl().y(), -M_PI/2);
+ 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));
for (double a = 0; a < M_PI / 2; a += 0.01) {
n = BicycleCar(g);
- n.rotate(g.ccr().x(), g.ccr().y(), a);
+ 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) {
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().x(), n.ccr().y(), a);
+ 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) {
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().x(), n.ccr().y(), a);
+ 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().x(), g.ccl().y(), a);
+ 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) {
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().x(), n.ccl().y(), a);
+ 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) {
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().x(), n.ccl().y(), a);
+ n.rotate(n.ccl(), a);
WVPASS(!g.drivable(n)); // fail near drivable border
}
}
projects / hubacji1 / bcar.git / blobdiff