summaryrefslogtreecommitdiff
path: root/examples/noc/src/path_following.zig
blob: b1715dcf8ec72f06cf58cbb693f93e4d85cbf46b (plain) (blame)
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))));
}