tiny.geometry.sweep

1pub const sweep = @import("sweep.zig");

1//! Swept shapes against triangles.2//!3//! A sweep translates a shape by `displacement` and reports the first contact4//! with a two-sided triangle. First contact is the earliest of three feature5//! families: the face, an edge and a vertex. For a capsule, the face family6//! reduces to its endpoint spheres, and the edge and vertex families gain the7//! capsule axis, whose line-to-line distance to an edge changes linearly under8//! translation. Taken together the families are exact.9const std = @import("std");10const linear = @import("linear");11const closest = @import("closest.zig");12const intersect = @import("intersect.zig");13const primitive = @import("primitive.zig");14const tolerance = @import("tolerance.zig");1516const assert = std.debug.assert;17const Capsule = primitive.Capsule;18const Segment = primitive.Segment;19const Sphere = primitive.Sphere;20const Triangle = primitive.Triangle;21const Vec3 = linear.Vec3;2223/// First contact of a sweep.24pub const Hit = struct {25    /// The fraction of the displacement travelled at contact, in [0, 1]. Zero26    /// means the shapes already touch or overlap at the start.27    t: f32,28    /// The contact point on the triangle.29    point: Vec3,30    /// The unit normal at contact, from the triangle toward the moving shape.31    normal: Vec3,32};3334/// The direction to push a shape out of a triangle when their closest points35/// coincide: the face normal turned against the motion, else against the36/// motion itself, else up.37fn fallbackNormal(triangle: Triangle, displacement: Vec3) Vec3 {38    if (triangle.normal()) |n| {39        if (n.dot(displacement) > 0) return n.negate();40        return n;41    }42    if (displacement.normalized(0)) |direction| return direction.negate();43    return Vec3.init(0, 0, 1);44}4546fn separationNormal(separation: Vec3, triangle: Triangle, displacement: Vec3) Vec3 {47    if (separation.lengthSq() <= tolerance.length_sq_floor) return fallbackNormal(triangle, displacement);48    return separation.normalized(0) orelse fallbackNormal(triangle, displacement);49}5051const Best = struct {52    hit: ?Hit = null,5354    fn consider(best: *Best, candidate: Hit) void {55        assert(candidate.t >= 0);56        assert(candidate.t <= 1);57        if (best.hit) |held| {58            if (candidate.t >= held.t) return;59        }60        best.hit = candidate;61    }62};6364/// The first time in [0, 1] at which the moving point `origin + motion t`65/// comes within `radius` of the line through `anchor` along `axis`, with the66/// point starting farther than `radius` from the line.67fn cylinderEntry(origin: Vec3, motion: Vec3, anchor: Vec3, axis: Vec3, radius: f32) ?f32 {68    const axis_sq = axis.lengthSq();69    assert(axis_sq > tolerance.length_sq_floor);70    const m = origin.sub(anchor);71    const m_perp = m.sub(axis.scale(m.dot(axis) / axis_sq));72    const d_perp = motion.sub(axis.scale(motion.dot(axis) / axis_sq));73    const a = d_perp.lengthSq();74    if (a <= tolerance.closing_sq_floor) return null;75    const c = m_perp.lengthSq() - radius * radius;76    if (c <= 0) return null;77    const b = m_perp.dot(d_perp);78    const discriminant = b * b - a * c;79    if (discriminant < 0) return null;80    const t = (-b - @sqrt(discriminant)) / a;81    if (t < 0) return null;82    if (t > 1) return null;83    return t;84}8586fn edges(triangle: Triangle) [3]Segment {87    return .{88        .{ .a = triangle.a, .b = triangle.b },89        .{ .a = triangle.b, .b = triangle.c },90        .{ .a = triangle.c, .b = triangle.a },91    };92}9394fn insideFace(p: Vec3, triangle: Triangle, n: Vec3) bool {95    for (edges(triangle)) |edge| {96        if (edge.b.sub(edge.a).cross(p.sub(edge.a)).dot(n) < 0) return false;97    }98    return true;99}100101/// Face, edge and vertex contacts of a sphere that starts clear of the102/// triangle.103fn sphereFeatures(best: *Best, center: Vec3, radius: f32, displacement: Vec3, triangle: Triangle) void {104    if (triangle.normal()) |n| {105        const distance = n.dot(center.sub(triangle.a));106        const closing = n.dot(displacement);107        const side: f32 = if (distance >= 0) 1 else -1;108        if (side * closing < 0 and closing * closing > tolerance.closing_sq_floor) {109            const t = (side * radius - distance) / closing;110            if (t >= 0 and t <= 1) {111                const facing = n.scale(side);112                const point = center.add(displacement.scale(t)).sub(facing.scale(radius));113                if (insideFace(point, triangle, n)) best.consider(.{ .t = t, .point = point, .normal = facing });114            }115        }116    }117    for (edges(triangle)) |edge| {118        const axis = edge.b.sub(edge.a);119        if (axis.lengthSq() <= tolerance.length_sq_floor) continue;120        const t = cylinderEntry(center, displacement, edge.a, axis, radius) orelse continue;121        const moved = center.add(displacement.scale(t));122        const along = moved.sub(edge.a).dot(axis) / axis.lengthSq();123        if (along < 0 or along > 1) continue;124        const point = edge.a.add(axis.scale(along));125        best.consider(.{ .t = t, .point = point, .normal = separationNormal(moved.sub(point), triangle, displacement) });126    }127    for (0..3) |index| {128        const corner = triangle.vertex(index);129        const roots = intersect.lineSphereRoots(center, displacement, corner, radius) orelse continue;130        const t = roots[0];131        if (t < 0 or t > 1) continue;132        const moved = center.add(displacement.scale(t));133        best.consider(.{ .t = t, .point = corner, .normal = separationNormal(moved.sub(corner), triangle, displacement) });134    }135}136137/// First contact of `sphere` moving by `displacement` with `triangle`, or null138/// when it passes clear.139pub fn sphereTriangle(sphere: Sphere, displacement: Vec3, triangle: Triangle) ?Hit {140    assert(sphere.radius >= 0);141    const nearest = closest.pointTriangle(sphere.center, triangle);142    const separation = sphere.center.sub(nearest);143    if (separation.lengthSq() <= sphere.radius * sphere.radius) {144        return .{ .t = 0, .point = nearest, .normal = separationNormal(separation, triangle, displacement) };145    }146    var best = Best{};147    sphereFeatures(&best, sphere.center, sphere.radius, displacement, triangle);148    return best.hit;149}150151/// First contact of `capsule` moving by `displacement` with `triangle`, or152/// null when it passes clear.153pub fn capsuleTriangle(capsule: Capsule, displacement: Vec3, triangle: Triangle) ?Hit {154    assert(capsule.radius >= 0);155    const radius = capsule.radius;156    const start = closest.segmentTriangle(capsule.axis(), triangle);157    const separation = start.first.sub(start.second);158    if (separation.lengthSq() <= radius * radius) {159        return .{ .t = 0, .point = start.second, .normal = separationNormal(separation, triangle, displacement) };160    }161    var best = Best{};162    sphereFeatures(&best, capsule.a, radius, displacement, triangle);163    sphereFeatures(&best, capsule.b, radius, displacement, triangle);164165    const axis = capsule.b.sub(capsule.a);166    const axis_sq = axis.lengthSq();167    if (axis_sq <= tolerance.length_sq_floor) return best.hit;168    for (edges(triangle)) |edge| axisEdge(&best, capsule, displacement, edge);169    for (0..3) |index| {170        const corner = triangle.vertex(index);171        const t = cylinderEntry(corner, displacement.negate(), capsule.a, axis, radius) orelse continue;172        const moved_a = capsule.a.add(displacement.scale(t));173        const along = corner.sub(moved_a).dot(axis) / axis_sq;174        if (along < 0 or along > 1) continue;175        const axis_point = moved_a.add(axis.scale(along));176        best.consider(.{ .t = t, .point = corner, .normal = separationNormal(axis_point.sub(corner), triangle, displacement) });177    }178    return best.hit;179}180181/// The contact between the capsule axis and an edge interior. The distance182/// between the two lines is `f0 + f1 t`, so contact is the root of183/// `|f0 + f1 t| = radius` where the lines' closest points lie inside both184/// segments.185fn axisEdge(best: *Best, capsule: Capsule, displacement: Vec3, edge: Segment) void {186    const u = capsule.b.sub(capsule.a);187    const v = edge.b.sub(edge.a);188    const n = u.cross(v);189    if (tolerance.isParallel(n.lengthSq(), u.lengthSq() * v.lengthSq())) return;190    const unit = n.normalized(0) orelse return;191    const f0 = capsule.a.sub(edge.a).dot(unit);192    const f1 = displacement.dot(unit);193    if (@abs(f0) <= capsule.radius) return;194    const side: f32 = if (f0 > 0) 1 else -1;195    if (side * f1 >= 0) return;196    if (f1 * f1 <= tolerance.closing_sq_floor) return;197    const t = (side * capsule.radius - f0) / f1;198    if (t < 0 or t > 1) return;199200    const w = capsule.a.add(displacement.scale(t)).sub(edge.a);201    const aa = u.lengthSq();202    const bb = u.dot(v);203    const cc = v.lengthSq();204    const du = u.dot(w);205    const ev = v.dot(w);206    const denominator = aa * cc - bb * bb;207    if (denominator <= 0) return;208    const along_axis = (bb * ev - cc * du) / denominator;209    const along_edge = (aa * ev - bb * du) / denominator;210    if (along_axis < 0 or along_axis > 1) return;211    if (along_edge < 0 or along_edge > 1) return;212    best.consider(.{ .t = t, .point = edge.at(along_edge), .normal = unit.scale(side) });213}214215const testing = std.testing;216const unit_triangle = Triangle{ .a = .{}, .b = Vec3.init(1, 0, 0), .c = Vec3.init(0, 1, 0) };217218fn expectHit(expected: Hit, actual: ?Hit) !void {219    const hit = actual orelse return error.TestExpectedHit;220    try testing.expectApproxEqAbs(expected.t, hit.t, 1e-6);221    try testing.expectApproxEqAbs(expected.point.x, hit.point.x, 1e-6);222    try testing.expectApproxEqAbs(expected.point.y, hit.point.y, 1e-6);223    try testing.expectApproxEqAbs(expected.point.z, hit.point.z, 1e-6);224    try testing.expectApproxEqAbs(expected.normal.x, hit.normal.x, 1e-6);225    try testing.expectApproxEqAbs(expected.normal.y, hit.normal.y, 1e-6);226    try testing.expectApproxEqAbs(expected.normal.z, hit.normal.z, 1e-6);227}228229test "a falling sphere lands on the face" {230    const ball = Sphere{ .center = Vec3.init(0.25, 0.25, 2), .radius = 0.5 };231    try expectHit(232        .{ .t = 0.375, .point = Vec3.init(0.25, 0.25, 0), .normal = Vec3.init(0, 0, 1) },233        sphereTriangle(ball, Vec3.init(0, 0, -4), unit_triangle),234    );235    const below = Sphere{ .center = Vec3.init(0.25, 0.25, -2), .radius = 0.5 };236    try expectHit(237        .{ .t = 0.375, .point = Vec3.init(0.25, 0.25, 0), .normal = Vec3.init(0, 0, -1) },238        sphereTriangle(below, Vec3.init(0, 0, 4), unit_triangle),239    );240}241242test "a sliding sphere strikes an edge and a vertex" {243    const edge_ball = Sphere{ .center = Vec3.init(0.5, -2, 0), .radius = 0.5 };244    try expectHit(245        .{ .t = 0.375, .point = Vec3.init(0.5, 0, 0), .normal = Vec3.init(0, -1, 0) },246        sphereTriangle(edge_ball, Vec3.init(0, 4, 0), unit_triangle),247    );248    const vertex_ball = Sphere{ .center = Vec3.init(3, 0, 0), .radius = 0.5 };249    try expectHit(250        .{ .t = 0.375, .point = Vec3.init(1, 0, 0), .normal = Vec3.init(1, 0, 0) },251        sphereTriangle(vertex_ball, Vec3.init(-4, 0, 0), unit_triangle),252    );253}254255test "a sphere that starts touching reports time zero and one that passes misses" {256    const touching = Sphere{ .center = Vec3.init(0.25, 0.25, 0.5), .radius = 0.5 };257    try expectHit(258        .{ .t = 0, .point = Vec3.init(0.25, 0.25, 0), .normal = Vec3.init(0, 0, 1) },259        sphereTriangle(touching, Vec3.init(1, 0, 0), unit_triangle),260    );261    const clear = Sphere{ .center = Vec3.init(3, 3, 1), .radius = 0.5 };262    try testing.expectEqual(@as(?Hit, null), sphereTriangle(clear, Vec3.init(0, 0, -4), unit_triangle));263    try testing.expectEqual(@as(?Hit, null), sphereTriangle(.{ .center = Vec3.init(0.25, 0.25, 2), .radius = 0.5 }, Vec3.init(0, 0, -1), unit_triangle));264}265266test "a capsule lands on an endpoint, an edge crossing and a vertex" {267    const upright = Capsule{ .a = Vec3.init(0.25, 0.25, 1), .b = Vec3.init(0.25, 0.25, 3), .radius = 0.5 };268    try expectHit(269        .{ .t = 0.25, .point = Vec3.init(0.25, 0.25, 0), .normal = Vec3.init(0, 0, 1) },270        capsuleTriangle(upright, Vec3.init(0, 0, -2), unit_triangle),271    );272    const lying = Capsule{ .a = Vec3.init(0.5, -1, 2), .b = Vec3.init(0.5, 1, 2), .radius = 0.5 };273    try expectHit(274        .{ .t = 0.375, .point = Vec3.init(0.5, 0, 0), .normal = Vec3.init(0, 0, 1) },275        capsuleTriangle(lying, Vec3.init(0, 0, -4), unit_triangle),276    );277    const post = Capsule{ .a = Vec3.init(-2, 0, -1), .b = Vec3.init(-2, 0, 1), .radius = 0.5 };278    try expectHit(279        .{ .t = 0.375, .point = Vec3.init(0, 0, 0), .normal = Vec3.init(-1, 0, 0) },280        capsuleTriangle(post, Vec3.init(4, 0, 0), unit_triangle),281    );282}283284test "a capsule that starts overlapping reports time zero" {285    const through = Capsule{ .a = Vec3.init(0.25, 0.25, -1), .b = Vec3.init(0.25, 0.25, 1), .radius = 0.1 };286    const hit = capsuleTriangle(through, Vec3.init(0, 0, 1), unit_triangle).?;287    try testing.expectEqual(@as(f32, 0), hit.t);288    try testing.expectEqual(Vec3.init(0, 0, -1), hit.normal);289    const clear = Capsule{ .a = Vec3.init(3, 3, 1), .b = Vec3.init(3, 3, 2), .radius = 0.5 };290    try testing.expectEqual(@as(?Hit, null), capsuleTriangle(clear, Vec3.init(0, 0, -4), unit_triangle));291}