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);
+ 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);
- // car frame
- WVPASSEQ_DOUBLE(bc.l(), 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(1.5, bc.rfx(), 0.00001);
- WVPASSEQ_DOUBLE(3.5, bc.lfy(), 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);
+ // car frame
+ WVPASSEQ_DOUBLE(bc.l(), 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(1.5, bc.rfx(), 0.00001);
+ WVPASSEQ_DOUBLE(3.5, bc.lfy(), 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);
- // 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);
+ // 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);
- // 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);
+ // 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);
- // moving
- bc.sp(1);
- bc.st(0);
- bc.next();
- WVPASSEQ_DOUBLE(1, bc.x(), 0.00001);
- WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
+ // moving
+ bc.sp(1);
+ bc.st(0);
+ bc.next();
+ WVPASSEQ_DOUBLE(1, bc.x(), 0.00001);
+ WVPASSEQ_DOUBLE(2, 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();//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);
- // 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);
- 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);
+ // 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);
+ 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);
}
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<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));
}
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);
+ 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);
+ 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);
}
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
+ 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.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.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
+ 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
- }
+ 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.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.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 = 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
- }
+ 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
+ }
}
projects / hubacji1 / bcar.git / blobdiff