lib/geometry/src/closest.zig
daab053ee43316e1809a84551d573ddd1e5bf3d2
1 //! Closest points between shapes, after Ericson, Real-Time Collision
2 //! Detection, chapter 5. Each query returns points on both shapes, so a caller
3 //! reads the separation as their difference.
4 const std = @import("std");
5 const linear = @import("linear");
6 const primitive = @import("primitive.zig");
7 const intersect = @import("intersect.zig");
8 const tolerance = @import("tolerance.zig");
9
10 const assert = std.debug.assert;
11 const Aabb = primitive.Aabb;
12 const Obb = primitive.Obb;
13 const Plane = primitive.Plane;
14 const Ray = primitive.Ray;
15 const Segment = primitive.Segment;
16 const Triangle = primitive.Triangle;
17 const Vec3 = linear.Vec3;
18
19 /// A point on each of two shapes, with each point's parameter on its segment
20 /// where the shape is one.
21 pub const Pair = struct {
22 first: Vec3,
23 second: Vec3,
24 first_t: f32 = 0,
25 second_t: f32 = 0,
26
27 pub fn distanceSq(pair: Pair) f32 {
28 return pair.first.distanceSq(pair.second);
29 }
30 };
31
32 fn clamp01(value: f32) f32 {
33 return @max(0, @min(1, value));
34 }
35
36 /// The parameter of the point on `segment` nearest `p`.
37 pub 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 }
43
44 pub fn pointSegment(p: Vec3, segment: Segment) Vec3 {
45 return segment.at(segmentParameter(p, segment));
46 }
47
48 pub fn pointAabb(p: Vec3, box: Aabb) Vec3 {
49 assert(!box.isEmpty());
50 return p.max(box.min).min(box.max);
51 }
52
53 pub 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 }
57
58 pub fn pointPlane(p: Vec3, plane: Plane) Vec3 {
59 return p.sub(plane.normal.scale(plane.signedDistance(p)));
60 }
61
62 /// The point of `triangle` nearest `p`, by Voronoi region. A degenerate
63 /// triangle answers with the nearest point of its three edges.
64 pub 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;
77
78 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;
82
83 const vc = d1 * d4 - d3 * d2;
84 if (vc <= 0 and d1 >= 0 and d3 <= 0) return a.add(ab.scale(d1 / (d1 - d3)));
85
86 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;
90
91 const vb = d5 * d2 - d1 * d6;
92 if (vb <= 0 and d2 >= 0 and d6 <= 0) return a.add(ac.scale(d2 / (d2 - d6)));
93
94 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 }
98
99 const inverse = 1 / (va + vb + vc);
100 return a.add(ab.scale(vb * inverse)).add(ac.scale(vc * inverse));
101 }
102
103 fn 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 }
114
115 /// The nearest points of two segments. A segment shorter than the length
116 /// floor acts as its first endpoint.
117 pub 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 }
148
149 /// The nearest points of a segment and a triangle, with the segment's point
150 /// first. A segment that crosses the triangle answers with the crossing.
151 pub 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 }
172
173 const testing = std.testing;
174 const unit_triangle = Triangle{ .a = .{}, .b = Vec3.init(1, 0, 0), .c = Vec3.init(0, 1, 0) };
175
176 test "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 }
184
185 test "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 }
194
195 test "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 }
199
200 test "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 }
213
214 test "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());
221
222 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);
226
227 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());
229
230 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 }
233
234 test "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);
239
240 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);
244
245 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 }