+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
#include <cmath>
#include "wvtest.h"
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.0, bc.raly(), 0.00001);
- WVPASSEQ_DOUBLE(1.0, 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(), M_PI / 2.0, 0.00001);
- WVPASSEQ_DOUBLE(-3.08257569495584, bc.ccl().x(), 0.00001);
+ WVPASSEQ_DOUBLE(-2.827076, bc.ccl().x(), 0.00001);
WVPASSEQ_DOUBLE(1.0000000000000004, bc.ccl().y(), 0.00001);
- WVPASSEQ_DOUBLE(5.08257569495584, bc.ccr().x(), 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(), 3.58257569495584, 0.00001);
- WVPASSEQ_DOUBLE(bc.ofradi(), 5.220153254455275, 0.00001);
- WVPASSEQ_DOUBLE(bc.orradi(), 4.6097722286464435, 0.00001);
+ 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
WVPASSEQ_DOUBLE(2.0, bc.y(), 0.00001);
bc.set_max_steer();
- WVPASSEQ_DOUBLE(0.45552437743483687, bc.st(), 0.00001);
+ WVPASSEQ_DOUBLE(0.481558, bc.st(), 0.00001);
// rotate
bc.x(-1.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")
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.in1().x(), 2.0, 0.00001);
- WVPASSEQ_DOUBLE(li1.in1().y(), 2.0, 0.00001);
+ 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));
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);
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.01);
+ n.rotate(g.ccl(), 0.1);
WVPASS(!g.drivable(n)); // fail left corner case
n = BicycleCar(g);