]> rtime.felk.cvut.cz Git - hubacji1/bcar.git/blobdiff - ut/bcar.t.cc
Add edist to rr, lf method
[hubacji1/bcar.git] / ut / bcar.t.cc
index bdd43549af6e8524aa0f7c41e030fc71017e2ac2..6849630d7b381f22ab9444e2e83f873bf4252d05 100644 (file)
@@ -1,3 +1,9 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 #include <cmath>
 #include "wvtest.h"
 
@@ -27,23 +33,25 @@ WVTEST_MAIN("bcar basic geometry")
        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
@@ -54,7 +62,7 @@ WVTEST_MAIN("bcar basic geometry")
        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);
@@ -68,6 +76,12 @@ WVTEST_MAIN("bcar basic geometry")
        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")
@@ -83,8 +97,8 @@ 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));
@@ -178,6 +192,8 @@ WVTEST_MAIN("drivable")
        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);
@@ -186,7 +202,7 @@ WVTEST_MAIN("drivable")
        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);