//! Nature of Code, Example 5.8: Path Following. Space toggles the //! debugging lines; a click makes a new path. const std = @import("std"); const sk = @import("sketch"); const vec = sk.math; pub const name = "path_following"; pub const width = 640; pub const height = 360; /// How far a vehicle may wander from the path before it steers back. const radius = 20; const Path = [4]vec.Vec; fn newPath(rng: std.Random) Path { return .{ .{ -20, height / 2 }, .{ vec.uniform(rng, 0, width / 2), vec.uniform(rng, 0, height) }, .{ vec.uniform(rng, width / 2, width), vec.uniform(rng, 0, height) }, .{ width + 20, height / 2 }, }; } /// Where a vehicle is headed and what it steers toward: `follow` in the /// book, which also draws it. Here `update` steers by it and `draw` shows /// it, both computing it from State. const Aim = struct { future: vec.Vec, normal: vec.Vec, target: vec.Vec, distance: f32 }; const Vehicle = struct { position: vec.Vec, velocity: vec.Vec = .{ 2, 0 }, maxspeed: f32, maxforce: f32, const r = 4; /// Reynolds' path following: predict 50 px ahead, find the nearest /// normal point on the path, aim 10 px further along it. fn aim(v: Vehicle, path: Path) Aim { const future = v.position + vec.withLength(v.velocity, 50); var best: Aim = .{ .future = future, .normal = future, .target = future, .distance = std.math.inf(f32) }; for (path[0 .. path.len - 1], path[1..]) |a, b| { const ab = vec.normalize(b - a); var normal = a + vec.scale(ab, vec.dot(future - a, ab)); // The book's shortcut: the path runs left to right. if (normal[0] < a[0] or normal[0] > b[0]) normal = b; const d = vec.distance(future, normal); if (d < best.distance) best = .{ .future = future, .normal = normal, .target = normal + vec.scale(ab, 10), .distance = d }; } return best; } fn step(v: *Vehicle, path: Path) void { const a = v.aim(path); var force: vec.Vec = .{ 0, 0 }; if (a.distance > radius) { const desired = a.target - v.position; if (vec.length(desired) > 0) force = vec.clampLength(vec.withLength(desired, v.maxspeed) - v.velocity, v.maxforce); } v.velocity = vec.clampLength(v.velocity + force, v.maxspeed); v.position += v.velocity; // Past the end, back to the start. if (v.position[0] > path[3][0] + r) v.position = .{ path[0][0] - r, path[0][1] + (v.position[1] - path[3][1]) }; } }; pub const State = struct { rng: std.Random.Xoshiro256 = .init(8), debug: bool = true, path: Path = undefined, cars: [2]Vehicle = .{ .{ .position = .{ 0, height / 2 }, .maxspeed = 2, .maxforce = 0.04 }, .{ .position = .{ 0, height / 2 }, .maxspeed = 3, .maxforce = 0.1 }, }, clock: sk.Clock = .{}, }; pub fn init(s: *State) void { s.path = newPath(s.rng.random()); } pub fn update(s: *State, in: *const sk.Input) void { if (in.pressed(sk.Key.space)) s.debug = !s.debug; if (in.mousePressed(.left) != null) s.path = newPath(s.rng.random()); for (0..s.clock.steps(in)) |_| for (&s.cars) |*car| car.step(s.path); } /// The path, as wide as the radius on either side: translucent black over /// white makes Processing's gray 200, and where the round joins meet the /// segments nothing is drawn twice. const road: sk.draw.Pen = .{ .stroke = sk.Color.black.alpha(55), .width = radius * 2, .join = .round }; const centre: sk.draw.Pen = .{ .stroke = .black }; const body: sk.draw.Pen = .{ .fill = .gray(127), .stroke = .black, .width = 2 }; const marker: sk.draw.Pen = .{ .fill = .gray(127), .stroke = .black }; const triangle = [_]vec.Vec{ .{ Vehicle.r * 2, 0 }, .{ -Vehicle.r * 2, -Vehicle.r }, .{ -Vehicle.r * 2, Vehicle.r } }; pub fn draw(s: *const State, f: *sk.Frame) void { f.clear(sk.Color.white); sk.draw.polyline(f, &s.path, road); sk.draw.polyline(f, &s.path, centre); if (s.debug) for (s.cars) |car| { const a = car.aim(s.path); sk.draw.line(f, car.position, a.future, centre); sk.draw.circle(f, a.future, 2, marker); sk.draw.circle(f, a.normal, 2, marker); sk.draw.line(f, a.future, a.normal, centre); // Red while steering back toward the path. sk.draw.circle(f, a.target, 4, .{ .fill = if (a.distance > radius) .hex(0xff0000) else .gray(127) }); }; for (s.cars) |car| sk.draw.polygon(f, &triangle, body.transformed(.place(car.position, vec.angle(car.velocity)))); }