+
+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
+ }
+}