1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
|
//! 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))));
}
|