| |
| #include "drive.hpp" |
| #include <cmath> |
| #include <cstdio> |
| using namespace vkgs; |
| int main(int, char** argv) { |
| ObstacleGrid og; og.load(argv[1]); Drivable road; |
| int fails = 0; |
| |
| Vehicle car; car.x = 0; car.y = 0.5f; car.yaw = 0; |
| ContactInfo cur = vehicle_contact(car, og, road, false); |
| std::printf("start contact %d\n", cur.total()); |
| |
| if (cur.total() <= 0) { std::puts("FAIL start pose does not overlap the pillar"); ++fails; } |
| VehicleControl go; go.throttle = 1; |
| float maxx = 0; int blocked = 0; |
| for (int i = 0; i < 2000; ++i) { |
| Vehicle prev = car; car.step(go, 0.01f); |
| ContactInfo hit = vehicle_contact(car, og, road, false); |
| if (hit.total() > cur.total()) { car = prev; car.v = 0; ++blocked; } else cur = hit; |
| maxx = std::max(maxx, car.x + car.geom.front); |
| } |
| std::printf("front reached x=%.2f (wall at 10.0), blocked steps %d\n", maxx, blocked); |
| |
| if (maxx > 10.25f || maxx < 9.8f) { std::puts("FAIL wall"); ++fails; } |
| |
| Vehicle r; VehicleControl back; back.brake = 1; |
| for (int i = 0; i < 500; ++i) r.step(back, 0.01f); |
| std::printf("reverse v=%.2f x=%.2f\n", r.v, r.x); |
| if (r.v > -2.9f || r.x > -5.0f) { std::puts("FAIL reverse"); ++fails; } |
| |
| VehicleControl fwd; fwd.throttle = 1; int k = 0; |
| while (r.v < 0 && k < 300) { r.step(fwd, 0.01f); ++k; } |
| std::printf("stopped after %.2f s, v=%.2f\n", k * 0.01f, r.v); |
| if (k > 100) { std::puts("FAIL brake-from-reverse"); ++fails; } |
| |
| VehicleGeom g; std::printf("zoe geom rear %.2f front %.2f center %.2f\n", g.rear, g.front, g.center()); |
| |
| if (std::fabs(g.wheelbase - 2.588f) > 1e-4f || std::fabs(g.rear + 0.65f) > 1e-4f || |
| std::fabs(g.front - 3.43f) > 1e-4f || std::fabs(g.half_width - 0.87f) > 1e-4f || |
| std::fabs(g.center() - 0.5f * (g.front + g.rear)) > 1e-5f) { |
| std::puts("FAIL zoe geom"); |
| ++fails; |
| } |
| |
| auto drive_into = [&](const std::vector<Footprint>& others) { |
| Vehicle c; ObstacleGrid none; Drivable nr; |
| ContactInfo cc = vehicle_contact(c, none, nr, false, 0.15f, 0.1f, &others); |
| float mx = 0; |
| for (int i = 0; i < 1500; ++i) { |
| Vehicle prev = c; c.step(go, 0.01f); |
| ContactInfo h = vehicle_contact(c, none, nr, false, 0.15f, 0.1f, &others); |
| if (h.total() > cc.total()) { c = prev; c.v = 0; if (h.kind != Contact::Vehicle) return -1.0f; } else cc = h; |
| mx = std::max(mx, c.x + c.geom.front); |
| } |
| return mx; |
| }; |
| float f1 = drive_into({Footprint{12, 0, 0, 2.0f, 1.0f}}); |
| float f2 = drive_into({Footprint{12, 0, 1.5707963f, 2.0f, 1.0f}}); |
| float f3 = drive_into({Footprint{12, 3.0f, 0, 2.0f, 1.0f}}); |
| std::printf("vehicle ahead stop %.2f (10), sideways %.2f (11), next lane %.2f (free)\n", f1, f2, f3); |
| if (f1 > 10.25f || f1 < 9.8f) { std::puts("FAIL vehicle ahead"); ++fails; } |
| if (f2 > 11.25f || f2 < 10.8f) { std::puts("FAIL vehicle sideways"); ++fails; } |
| if (f3 < 30.0f) { std::puts("FAIL next lane blocked"); ++fails; } |
| |
| { |
| Vehicle c; c.x = 3.0f; c.y = 4.0f; c.yaw = 0.3f; |
| ContactInfo cc = vehicle_contact(c, og, road, false); |
| auto step_blocked = [&](const VehicleControl& ctl) { |
| Vehicle prev = c; c.step(ctl, 0.01f); |
| ContactInfo h = vehicle_contact(c, og, road, false); |
| float dir = c.v > 0 ? 1.0f : c.v < 0 ? -1.0f : 0.0f; |
| if (h.depth(2) > cc.depth(2) && !escaping(c, dir, cc.depth(2), og, road, false, 0.15f, 0.1f, 2)) { |
| c = prev; c.v = 0; return true; |
| } |
| cc = h; return false; |
| }; |
| int blocked = 0; |
| for (int i = 0; i < 600; ++i) blocked += step_blocked(go); |
| float x_hit = c.x; |
| VehicleControl back; back.brake = 1; |
| for (int i = 0; i < 300; ++i) step_blocked(back); |
| std::printf("angled wall: stopped x=%.2f (blocked %d, contact %d), after 3 s reverse x=%.2f\n", x_hit, blocked, cc.total(), c.x); |
| if (blocked == 0 || x_hit + c.geom.front * std::cos(0.3f) > 10.4f) { std::puts("FAIL angled wall not blocking"); ++fails; } |
| if (c.x > x_hit - 1.0f) { std::puts("FAIL stuck after angled hit (reverse)"); ++fails; } |
| } |
| |
| for (int gz : {2, 0}) { |
| Vehicle c; c.x = -16.0f; c.y = 4.28f; c.yaw = 0; |
| ContactInfo cc = vehicle_contact(c, og, road, false); |
| int blocked = 0; |
| for (int i = 0; i < 400; ++i) { |
| Vehicle prev = c; c.step(go, 0.01f); |
| ContactInfo h = vehicle_contact(c, og, road, false); |
| float dir = c.v > 0 ? 1.0f : -1.0f; |
| if (h.depth(gz) > cc.depth(gz) && !escaping(c, dir, cc.depth(gz), og, road, false, 0.15f, 0.1f, gz)) { |
| c = prev; c.v = 0; ++blocked; |
| } else cc = h; |
| } |
| std::printf("graze %d: lone cell beside the car, x=%.2f blocked %d\n", gz, c.x, blocked); |
| if (gz == 2 && (blocked || c.x < -5.0f)) { std::puts("FAIL grazing a lone cell blocks the car"); ++fails; } |
| if (gz == 0 && !blocked) { std::puts("FAIL test setup: the lone cell does not touch the car"); ++fails; } |
| } |
| return fails; |
| } |
|
|
