Update drivable ut

diff --git a/ut/bcar.t.cc b/ut/bcar.t.cc
index a0a14ddb79eb7487bf8638e699de458041cf85ac..f06bd673d6a9d69de7dcce4f9c08f259d02c146b 100644 (file)
--- a/ut/bcar.t.cc
+++ b/ut/bcar.t.cc
@@ -49,24 +49,6 @@ WVTEST_MAIN("bcar basic geometry")
         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);
@@ -126,3 +108,150 @@ WVTEST_MAIN("test collide functions")
         p3.push_back(std::make_tuple(4, 2));
         WVPASS(!std::get<0>(collide(p1, p3)));
 }
+
+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().x(), g.ccr().y(), -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().x(), g.ccl().y(), 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);
+        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().x(), g.ccr().y(), 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().x(), n.ccr().y(), 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().x(), n.ccr().y(), 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);
+                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().x(), n.ccl().y(), 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().x(), n.ccl().y(), 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().x(), g.ccr().y(), 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().x(), g.ccl().y(), -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().x(), g.ccr().y(), 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().x(), n.ccr().y(), 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().x(), n.ccr().y(), 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);
+                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().x(), n.ccl().y(), 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().x(), n.ccl().y(), a);
+                WVPASS(!g.drivable(n)); // fail near drivable border
+        }
+}