tiny.geometry.closest

1//! Closest points between shapes, after Ericson, Real-Time Collision2//! Detection, chapter 5. Each query returns points on both shapes, so a caller3//! reads the separation as their difference.4const std = @import("std");5const linear = @import("linear");6const primitive = @import("primitive.zig");7const intersect = @import("intersect.zig");8const tolerance = @import("tolerance.zig");910const assert = std.debug.assert;11const Aabb = primitive.Aabb;12const Obb = primitive.Obb;13const Plane = primitive.Plane;14const Ray = primitive.Ray;15const Segment = primitive.Segment;16const Triangle = primitive.Triangle;17const Vec3 = linear.Vec3;1819/// A point on each of two shapes, with each point's parameter on its segment20/// where the shape is one.21pub const Pair = struct {22    first: Vec3,23    second: Vec3,24    first_t: f32 = 0,25    second_t: f32 = 0,2627    pub fn distanceSq(pair: Pair) f32 {28        return pair.first.distanceSq(pair.second);29    }30};3132fn clamp01(value: f32) f32 {33    return @max(0, @min(1, value));34}3536/// The parameter of the point on `segment` nearest `p`.37pub fn segmentParameter(p: Vec3, segment: Segment) f32 {38    const ab = segment.b.sub(segment.a);39    const length_sq = ab.lengthSq();40    if (length_sq <= tolerance.length_sq_floor) return 0;41    return clamp01(p.sub(segment.a).dot(ab) / length_sq);42}4344pub fn pointSegment(p: Vec3, segment: Segment) Vec3 {45    return segment.at(segmentParameter(p, segment));46}4748pub fn pointAabb(p: Vec3, box: Aabb) Vec3 {49    assert(!box.isEmpty());50    return p.max(box.min).min(box.max);51}5253pub fn pointObb(p: Vec3, box: Obb) Vec3 {54    const local = box.toLocal(p);55    return box.fromLocal(local.max(box.half_extents.negate()).min(box.half_extents));56}5758pub fn pointPlane(p: Vec3, plane: Plane) Vec3 {59    return p.sub(plane.normal.scale(plane.signedDistance(p)));60}6162/// The point of `triangle` nearest `p`, by Voronoi region. A degenerate63/// triangle answers with the nearest point of its three edges.64pub fn pointTriangle(p: Vec3, triangle: Triangle) Vec3 {65    if (triangle.isDegenerate()) {66        return pointDegenerateTriangle(p, triangle);67    }68    const a = triangle.a;69    const b = triangle.b;70    const c = triangle.c;71    const ab = b.sub(a);72    const ac = c.sub(a);73    const ap = p.sub(a);74    const d1 = ab.dot(ap);75    const d2 = ac.dot(ap);76    if (d1 <= 0 and d2 <= 0) return a;7778    const bp = p.sub(b);79    const d3 = ab.dot(bp);80    const d4 = ac.dot(bp);81    if (d3 >= 0 and d4 <= d3) return b;8283    const vc = d1 * d4 - d3 * d2;84    if (vc <= 0 and d1 >= 0 and d3 <= 0) return a.add(ab.scale(d1 / (d1 - d3)));8586    const cp = p.sub(c);87    const d5 = ab.dot(cp);88    const d6 = ac.dot(cp);89    if (d6 >= 0 and d5 <= d6) return c;9091    const vb = d5 * d2 - d1 * d6;92    if (vb <= 0 and d2 >= 0 and d6 <= 0) return a.add(ac.scale(d2 / (d2 - d6)));9394    const va = d3 * d6 - d5 * d4;95    if (va <= 0 and d4 - d3 >= 0 and d5 - d6 >= 0) {96        return b.add(c.sub(b).scale((d4 - d3) / ((d4 - d3) + (d5 - d6))));97    }9899    const inverse = 1 / (va + vb + vc);100    return a.add(ab.scale(vb * inverse)).add(ac.scale(vc * inverse));101}102103fn pointDegenerateTriangle(p: Vec3, triangle: Triangle) Vec3 {104    var best = pointSegment(p, .{ .a = triangle.a, .b = triangle.b });105    for ([_]Segment{106        .{ .a = triangle.b, .b = triangle.c },107        .{ .a = triangle.c, .b = triangle.a },108    }) |edge| {109        const candidate = pointSegment(p, edge);110        if (candidate.distanceSq(p) < best.distanceSq(p)) best = candidate;111    }112    return best;113}114115/// The nearest points of two segments. A segment shorter than the length116/// floor acts as its first endpoint.117pub fn segmentSegment(first: Segment, second: Segment) Pair {118    const d1 = first.b.sub(first.a);119    const d2 = second.b.sub(second.a);120    const r = first.a.sub(second.a);121    const a = d1.lengthSq();122    const e = d2.lengthSq();123    const f = d2.dot(r);124    var s: f32 = 0;125    var t: f32 = 0;126    if (a <= tolerance.length_sq_floor) {127        if (e > tolerance.length_sq_floor) t = clamp01(f / e);128    } else {129        const c = d1.dot(r);130        if (e <= tolerance.length_sq_floor) {131            s = clamp01(-c / a);132        } else {133            const b = d1.dot(d2);134            const denominator = a * e - b * b;135            if (denominator > 0) s = clamp01((b * f - c * e) / denominator);136            t = (b * s + f) / e;137            if (t < 0) {138                t = 0;139                s = clamp01(-c / a);140            } else if (t > 1) {141                t = 1;142                s = clamp01((b - c) / a);143            }144        }145    }146    return .{ .first = first.at(s), .second = second.at(t), .first_t = s, .second_t = t };147}148149/// The nearest points of a segment and a triangle, with the segment's point150/// first. A segment that crosses the triangle answers with the crossing.151pub fn segmentTriangle(segment: Segment, triangle: Triangle) Pair {152    const ray = Ray{ .origin = segment.a, .direction = segment.b.sub(segment.a) };153    if (intersect.rayTriangle(ray, triangle)) |hit| {154        if (hit.t >= 0 and hit.t <= 1) {155            const crossing = ray.at(hit.t);156            return .{ .first = crossing, .second = crossing, .first_t = hit.t };157        }158    }159    var best = Pair{ .first = segment.a, .second = pointTriangle(segment.a, triangle) };160    const at_b = Pair{ .first = segment.b, .second = pointTriangle(segment.b, triangle), .first_t = 1 };161    if (at_b.distanceSq() < best.distanceSq()) best = at_b;162    for ([_]Segment{163        .{ .a = triangle.a, .b = triangle.b },164        .{ .a = triangle.b, .b = triangle.c },165        .{ .a = triangle.c, .b = triangle.a },166    }) |edge| {167        const candidate = segmentSegment(segment, edge);168        if (candidate.distanceSq() < best.distanceSq()) best = candidate;169    }170    return best;171}172173const testing = std.testing;174const unit_triangle = Triangle{ .a = .{}, .b = Vec3.init(1, 0, 0), .c = Vec3.init(0, 1, 0) };175176test "point to segment clamps to the endpoints" {177    const segment = Segment{ .a = .{}, .b = Vec3.init(2, 0, 0) };178    try testing.expectEqual(Vec3.init(1, 0, 0), pointSegment(Vec3.init(1, 5, 0), segment));179    try testing.expectEqual(Vec3.init(0, 0, 0), pointSegment(Vec3.init(-3, 1, 0), segment));180    try testing.expectEqual(Vec3.init(2, 0, 0), pointSegment(Vec3.init(9, 0, 0), segment));181    const point = Segment{ .a = Vec3.init(1, 1, 1), .b = Vec3.init(1, 1, 1) };182    try testing.expectEqual(Vec3.init(1, 1, 1), pointSegment(.{}, point));183}184185test "point to triangle answers from each Voronoi region" {186    try testing.expectEqual(Vec3.init(0.25, 0.25, 0), pointTriangle(Vec3.init(0.25, 0.25, 3), unit_triangle));187    try testing.expectEqual(Vec3.init(0, 0, 0), pointTriangle(Vec3.init(-1, -1, 0), unit_triangle));188    try testing.expectEqual(Vec3.init(1, 0, 0), pointTriangle(Vec3.init(2, -1, 0), unit_triangle));189    try testing.expectEqual(Vec3.init(0, 1, 0), pointTriangle(Vec3.init(-1, 2, 0), unit_triangle));190    try testing.expectEqual(Vec3.init(0.5, 0, 0), pointTriangle(Vec3.init(0.5, -2, 1), unit_triangle));191    try testing.expectEqual(Vec3.init(0, 0.5, 0), pointTriangle(Vec3.init(-2, 0.5, 0), unit_triangle));192    try testing.expectEqual(Vec3.init(0.5, 0.5, 0), pointTriangle(Vec3.init(1, 1, 0), unit_triangle));193}194195test "a triangle with no area answers from its edges" {196    const sliver = Triangle{ .a = .{}, .b = Vec3.init(1, 0, 0), .c = Vec3.init(2, 0, 0) };197    try testing.expectEqual(Vec3.init(1.5, 0, 0), pointTriangle(Vec3.init(1.5, 1, 0), sliver));198}199200test "point to box and plane clamp and project" {201    const box = Aabb{ .min = .{}, .max = Vec3.splat(1) };202    try testing.expectEqual(Vec3.init(1, 0.5, 0), pointAabb(Vec3.init(3, 0.5, -2), box));203    const plane = Plane.fromPointNormal(Vec3.init(0, 0, 2), Vec3.init(0, 0, 1));204    try testing.expectEqual(Vec3.init(4, 5, 2), pointPlane(Vec3.init(4, 5, -7), plane));205    const turned = Obb{206        .center = .{},207        .axes = linear.Mat3.fromCols(Vec3.init(0, 1, 0), Vec3.init(-1, 0, 0), Vec3.init(0, 0, 1)),208        .half_extents = Vec3.init(2, 1, 1),209    };210    try testing.expectEqual(Vec3.init(0, 2, 0), pointObb(Vec3.init(0, 5, 0), turned));211    try testing.expectEqual(Vec3.init(-1, 0, 0), pointObb(Vec3.init(-5, 0, 0), turned));212}213214test "segment pairs meet at crossings, endpoints and parallel spans" {215    const x_axis = Segment{ .a = Vec3.init(-1, 0, 0), .b = Vec3.init(1, 0, 0) };216    const above = Segment{ .a = Vec3.init(0, -1, 2), .b = Vec3.init(0, 1, 2) };217    const crossing = segmentSegment(x_axis, above);218    try testing.expectEqual(Vec3.init(0, 0, 0), crossing.first);219    try testing.expectEqual(Vec3.init(0, 0, 2), crossing.second);220    try testing.expectEqual(@as(f32, 4), crossing.distanceSq());221222    const beyond = Segment{ .a = Vec3.init(3, 0, 0), .b = Vec3.init(5, 0, 0) };223    const gap = segmentSegment(x_axis, beyond);224    try testing.expectEqual(Vec3.init(1, 0, 0), gap.first);225    try testing.expectEqual(Vec3.init(3, 0, 0), gap.second);226227    const parallel = Segment{ .a = Vec3.init(0.5, 1, 0), .b = Vec3.init(3, 1, 0) };228    try testing.expectEqual(@as(f32, 1), segmentSegment(x_axis, parallel).distanceSq());229230    const point = Segment{ .a = Vec3.init(0, 3, 0), .b = Vec3.init(0, 3, 0) };231    try testing.expectEqual(Vec3.init(0, 0, 0), segmentSegment(point, x_axis).second);232}233234test "segment to triangle finds crossings, endpoints and edges" {235    const through = Segment{ .a = Vec3.init(0.25, 0.25, 1), .b = Vec3.init(0.25, 0.25, -1) };236    const crossing = segmentTriangle(through, unit_triangle);237    try testing.expectEqual(@as(f32, 0), crossing.distanceSq());238    try testing.expectEqual(Vec3.init(0.25, 0.25, 0), crossing.second);239240    const hovering = Segment{ .a = Vec3.init(0.2, 0.2, 1), .b = Vec3.init(0.2, 0.2, 3) };241    const endpoint = segmentTriangle(hovering, unit_triangle);242    try testing.expectEqual(Vec3.init(0.2, 0.2, 1), endpoint.first);243    try testing.expect(endpoint.second.distance(Vec3.init(0.2, 0.2, 0)) < 1e-6);244245    const beside = Segment{ .a = Vec3.init(0.5, -1, -1), .b = Vec3.init(0.5, -1, 1) };246    const edge = segmentTriangle(beside, unit_triangle);247    try testing.expectEqual(Vec3.init(0.5, 0, 0), edge.second);248    try testing.expectEqual(@as(f32, 1), edge.distanceSq());249}

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