diff --git a/src/distances.jl b/src/distances.jl index 8313598..834c0c0 100644 --- a/src/distances.jl +++ b/src/distances.jl @@ -3,445 +3,374 @@ Return a tuple `(d², r, s, t)` of: 1. `d²`: the minimum squared distance between the two simplexes. -2. `r`: the minimum separation vector, pointing from the closest point on `x` to the closest point on `y`. -3. `s`: the barycentric coordinates of the closest point on `x`. -4. `t`: the barycentric coordinates of the closest point on `y`. +2. `r`: the minimum separation vector, pointing from a closest point on `x` to a closest point on `y`. +3. `s`: the barycentric coordinates of a closest point on `x`. +4. `t`: the barycentric coordinates of a closest point on `y`. `s` holds the barycentric weights of vertices `x[2:N]`; the weight of `x[1]` -is implied as `1 - sum(s)`, so the closest point on `x` is: +is implied as `1 - sum(s)`, so a closest point on `x` is: (1 - sum(s))*x[1] + sum(s[i]*x[i+1] for i in 1:N-1) Each weight is in `[0, 1]` with `sum(s) ≤ 1`. For a `Point`, `s` is empty; -for a `LineSeg`, the closest point is `(1 - s[1])*x[1] + s[1]*x[2]`. +for a `LineSeg`, a closest point is `(1 - s[1])*x[1] + s[1]*x[2]`. `t` works the same way for `y`. """ function dist_sqr_r_s_t end +#= +What a `dist_sqr_r_s_t` method must do to be correct, for an element type `T`. Here `d` is the exact distance between the +simplexes as given, and `ℓ` is the largest absolute coordinate. + +For finite coordinates below `(floatmax(T)^(1/4))/4` (1e9 in `Float32`, +3e76 in `Float64`): + +1. The result is finite: never `NaN` or `Inf`, even for degenerate simplexes + (repeated vertices, zero length edges, collinear triangles). +2. Each weight in `s` and `t` is in `[0, 1]`, and the computed `sum(s)` and + `sum(t)` are at most one. +3. `d²` is computed from `r` alone, the same way in every method: + `fma(r[3], r[3], fma(r[2], r[2], r[1]*r[1]))`. +4. `|√d² - d| ≤ 8*eps(T)*ℓ + floatmin(T)^(1/4)`. +5. With `X(s)` and `Y(t)` the points the weights give, computed exactly, + `‖r - (Y(t) - X(s))‖` is within the same bound. + +The `floatmin(T)^(1/4)` term (3e-10 in `Float32`, 1e-77 in `Float64`) is only +for when something underflows. Then accuracy may be lost, but only by that +much. + +Above `(floatmax(T)^(1/4))/4` anything goes, except that for any +coordinates, if one is `NaN`, `d²` is `NaN`. + +The methods must also accept `SIMD.Vec` elements and be bit identical to the scalar method. + +`tiny_normal_cases` and `hard_points` in test/test-mindistance.jl partially check +this at the edges of the range and at hard points. +=# + """ dist_sqr(x::Simplex{N,T}, y::Simplex{M,T})::T Return the minimum squared distance between two simplexes. + +This is the `d²` of [`dist_sqr_r_s_t`](@ref), bit for bit. """ -function dist_sqr(x::Simplex{N, T}, y::Simplex{M, T}) where {T, N, M} - d2, r, s, t = @inline dist_sqr_r_s_t(y, x) +@inline function dist_sqr(x::Simplex{N, T}, y::Simplex{M, T}) where {T, N, M} + d2, r, s, t = dist_sqr_r_s_t(x, y) d2 end # Dispatch N > M to N ≤ M -function dist_sqr_r_s_t(x::Simplex{3, T}, y::Simplex{2, T}) where T - d2, r, s, t = @inline dist_sqr_r_s_t(y, x) +@inline function dist_sqr_r_s_t(x::Simplex{3, T}, y::Simplex{2, T}) where T + d2, r, s, t = dist_sqr_r_s_t(y, x) d2, -r, t, s end -function dist_sqr_r_s_t(x::Simplex{3, T}, y::Simplex{1, T}) where T - d2, r, s, t = @inline dist_sqr_r_s_t(y, x) +@inline function dist_sqr_r_s_t(x::Simplex{3, T}, y::Simplex{1, T}) where T + d2, r, s, t = dist_sqr_r_s_t(y, x) d2, -r, t, s end -function dist_sqr_r_s_t(x::Simplex{2, T}, y::Simplex{1, T}) where T - d2, r, s, t = @inline dist_sqr_r_s_t(y, x) +@inline function dist_sqr_r_s_t(x::Simplex{2, T}, y::Simplex{1, T}) where T + d2, r, s, t = dist_sqr_r_s_t(y, x) d2, -r, t, s end -function dist_sqr(x::Simplex{3, T}, y::Simplex{2, T}) where T - @inline dist_sqr(y, x) -end -function dist_sqr(x::Simplex{3, T}, y::Simplex{1, T}) where T - @inline dist_sqr(y, x) -end -function dist_sqr(x::Simplex{2, T}, y::Simplex{1, T}) where T - @inline dist_sqr(y, x) -end -# Branch free update of the running minimum `r` and its `s` and `t`. -# Works on `SIMD.Vec` lanes. Ties keep the earlier minimum. -@inline function _min_r_s_t(minr, mins, mint, r, s, t) - better = r < minr - SIMD.vifelse(better, r, minr), SIMD.vifelse(better, s, mins), SIMD.vifelse(better, t, mint) +# Branch free select, elementwise for an `SVector`. Works on `SIMD.Vec` lanes. +@inline _vifelse(c, a, b) = SIMD.vifelse(c, a, b) +@inline _vifelse(c, a::SVector{N}, b::SVector{N}) where N = SVector(ntuple(i -> SIMD.vifelse(c, a[i], b[i]), Val(N))) + +# Branch free selection of `(d2, r, s, t)` tuples, with `r` a `Vec3` and `s` +# and `t` scalars or `SVector`s. Works on `SIMD.Vec` lanes. +@inline function _select_d2_r_s_t(c, (d2a, ra, sa, ta), (d2b, rb, sb, tb)) + _vifelse(c, d2a, d2b), _vifelse(c, ra, rb), _vifelse(c, sa, sb), _vifelse(c, ta, tb) end +# Branch free update of the running minimum `best` with `cand`. +# Ties keep the earlier minimum. A NaN `d2` is kept, so a NaN coordinate gives a NaN `d2`. +@inline _min_d2_r_s_t(best, cand) = _select_d2_r_s_t((cand[1] < best[1]) | isnan(cand[1]), cand, best) + # The methods -function dist_sqr_r_s_t(x::Point{T}, y::Point{T}) where T +@inline function dist_sqr_r_s_t(x::Point{T}, y::Point{T}) where T d = y[1] - x[1] dot3(d, d), d, SVector{0,T}(), SVector{0,T}() end -function dist_sqr_r_s_t(x::Point{T}, y::LineSeg{T}) where T +@inline function dist_sqr_r_s_t(x::Point{T}, y::LineSeg{T}) where T r = y[2] - y[1] - p = x[1] - y[1] - t = clamp01nan(dot3(p, r)/dot3(r, r)) - d = scale3(t, r) - p + # Written as `y - x[1]` so it folds to `y[1]` when `x[1]` is positive zero. + p = y[1] - x[1] + t = clamp01nan(-dot3(p, r)/dot3(r, r)) + d = fma3(t, r, p) dot3(d, d), d, SVector{0,T}(), SVector{1,T}(t) end #= -Based on https://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf -But not fully optimized. -// David Eberly, Geometric Tools, Redmond WA 98052 -// Copyright (c) 1998-2022 -// Distributed under the Boost Software License, Version 1.0. -// https://www.boost.org/LICENSE_1_0.txt -// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt -// Version: 6.0.2022.01.06 -=# -function dist_sqr_r_s_t(x::Point{T}, y::Triangle{T}) where T - P = x[1] - B = y[1] - E0 = y[2] - B - E1 = y[3] - B - BP = B - P - a = dot3(E0, E0) - b = dot3(E0, E1) - c = dot3(E1, E1) - d = dot3(E0, BP) - e = dot3(E1, BP) - f = dot3(BP, BP) - Δ = a*c - b*b - invΔ = inv(Δ) - sbar = clamp01nan((b*e - c*d)*invΔ) - tbar = clamp01nan((-a*e + b*d)*invΔ) - - # this ensures sbar and tbar are in the domain - # if they are out of the domain, the min distance is on a boundary - outside = sbar+tbar > one(sbar) - sbar = SIMD.vifelse(outside,zero(sbar),sbar) - tbar = SIMD.vifelse(outside,zero(tbar),tbar) - s0 = clamp01nan(-d/a) - t0 = clamp01nan(-e/c) - sd = clamp01nan(-(b-c+d-e)/(a-2b+c)) - td = one(sd) - sd - @inline r(s,t) = a*(s*s) + 2b*s*t + c*(t*t) + 2d*s + 2e*t - - mins = sbar - mint = tbar - minr = r(sbar,tbar) - minr, mins, mint = _min_r_s_t(minr, mins, mint, r(s0,zero(a)), s0, zero(a)) - minr, mins, mint = _min_r_s_t(minr, mins, mint, r(zero(a),t0), zero(a), t0) - minr, mins, mint = _min_r_s_t(minr, mins, mint, r(sd,td), sd, td) - d2cl = Base.FastMath.max_fast(zero(a),f+minr) - rcl = scale3(mins, E0) + scale3(mint, E1) + BP - d2cl, rcl, SVector{0,T}(), SVector{2,T}(mins, mint) -end +For the triangle `A`, `B`, `C`, the closest point is the projection `X'` of +the point onto the plane if that is inside the triangle, and otherwise on an +edge. Rather than test which, this takes the closest of three points of the +triangle: the closest point on edges `AC` and `BC`, and `X'` with its weights +clamped into the triangle, which also covers edge `AB`. Each is a point of +the triangle, so none is closer than the true distance, and `d2` is +`dot(r, r)` of its own separation `r`, which does not cancel. -#= -Based on https://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf -But not fully optimized. -// David Eberly, Geometric Tools, Redmond WA 98052 -// Copyright (c) 1998-2022 -// Distributed under the Boost Software License, Version 1.0. -// https://www.boost.org/LICENSE_1_0.txt -// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt -// Version: 6.0.2022.01.06 +The weights `(u, v)` of `X'` are a least squares solve. With +`E1 = c1*E0 + e` and `e ⟂ E0`, `X - A = (u + v*c1)*E0 + v*e` plus a normal +part, so `v = e⋅(X - A)/(e⋅e)` and `u = E0⋅(X - A)/(E0⋅E0) - v*c1`. Then +`u*E0 + v*E1 = (u + v*c1)*E0 + v*e`, so the errors of `u` and `v` move the +point by about a rounding of `‖X - A‖`, not over the sine of an angle. + +`e` itself is never formed. `E1 - c1*E0` cancels for a sliver, but with +`N = E0×E1`, `N×E0 = (E0⋅E0)*e`, so `v = N⋅(E0×(X - A))/(N⋅N)`. The rounding +of `E0×(X - A)` moves `X'` by about a rounding of `‖X - A‖`. The rounding of +`N` is about a rounding of `‖E0‖*‖E1‖`. Along `N` it only scales `v`, which +moves `X'` by at most about a rounding of `‖E1‖`. In the plane it tilts the +normal, which moves `X'` in the plane by about a rounding of the height over +the sine of the sliver's angle. That adds to the distance in second order, +and when it is large the sliver is thin and an edge is close: the smaller of +the two is at most about a rounding of `‖E1‖`. The two cross products don't +wait on each other or on a division, which keeps the chain of dependent +operations short. + +Edge `AB` is `v = 0`, and `u` is solved after `v` is clamped. So when `X'` +is past `AB`, `v` clamps to zero and `X'` becomes exactly the closest point +on `AB`, with an exact zero weight. `v = min(v, 1 - u)` then keeps `X'` in +the triangle. Past `AC` or `BC` it is just some point of the triangle, and +the edge candidates give the closest point. + +`N⋅N` and `N⋅(E0×(X - A))` are at most fourth powers of the coordinates, so +they are in range. When `N⋅N` underflows, every point of the triangle is +within about `floatmin^(1/4)` of edges `AC` and `BC`: within `‖E0‖/2` and +within the height over `AB`, whose product is `‖N‖/2`. A degenerate triangle +or edge gives NaN or infinite weights, which `clamp01nan` makes a point of +the triangle, and then the edges are exact or within the length of the edge. =# -@inline function dist_sqr(x::Point{T}, y::Triangle{T}) where T - P = x[1] - B = y[1] - E0 = y[3] - B - E1 = y[2] - B - BP = B - P - a = dot3(E0, E0) - b = dot3(E0, E1) - c = dot3(E1, E1) - d = dot3(E0, BP) - e = dot3(E1, BP) - f = dot3(BP, BP) - Δ = a*c - b*b - invΔ = inv(Δ) - sbar = clamp01nan((b*e - c*d)*invΔ) - tbar = clamp01nan((-a*e + b*d)*invΔ) - - # this ensures sbar and tbar are in the domain - # if they are out of the domain, the min distance is on a boundary - outside = sbar+tbar > one(sbar) - sbar = SIMD.vifelse(outside,zero(sbar),sbar) - tbar = SIMD.vifelse(outside,zero(tbar),tbar) - s0 = clamp01nan(-d/a) - t0 = clamp01nan(-e/c) - sd = clamp01nan(-(b-c+d-e)/(a-2b+c)) - td = one(sd) - sd - @inline q(s,t) = a*(s*s) + 2b*s*t + c*(t*t) + 2d*s + 2e*t - - Base.FastMath.max_fast(zero(sd),f + Base.FastMath.min_fast( - q(s0,zero(a)), - q(zero(a),t0), - q(sd,td), - q(sbar,tbar), - )) +@inline function dist_sqr_r_s_t(x::Point{T}, y::Triangle{T}) where T + A = y[1] + E0 = y[2] - A + E1 = y[3] - A + # Written as `y - x[1]` so it folds to `A` when `x[1]` is positive zero. + AX = A - x[1] + d2in, rin, u, v = _tri_face_candidate(AX, E0, E1, cross3(E0, E1)) + # The closest points on the edges `AC` and `BC`. + u1 = clamp01nan(-dot3(E1, AX)/dot3(E1, E1)) + r1 = fma3(u1, E1, AX) + best = (dot3(r1, r1), r1, zero(T), u1) + E2 = E1 - E0 + BX = AX + E0 + u2 = clamp01nan(-dot3(BX, E2)/dot3(E2, E2)) + r2 = fma3(u2, E2, BX) + best = _min_d2_r_s_t(best, (dot3(r2, r2), r2, one(T) - u2, u2)) + # Last, so a tie with an edge keeps the edge's exact zero weight. + d2, r, s, t = _min_d2_r_s_t(best, (d2in, rin, u, v)) + d2, r, SVector{0,T}(), SVector{2,T}(s, t) end -#= -Using the simple algorithm and some comments from -https://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf -// David Eberly, Geometric Tools, Redmond WA 98052 -// Copyright (c) 1998-2022 -// Distributed under the Boost Software License, Version 1.0. -// https://www.boost.org/LICENSE_1_0.txt -// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt -// Version: 6.0.2022.01.06 -Ignores the case of degenerate line segments. -=# -function dist_sqr_r_s_t(x::LineSeg{T}, y::LineSeg{T}) where T - P0 = x[1] - P1 = x[2] - Q0 = y[1] - Q1 = y[2] - P = P1-P0 - Q = Q1-Q0 - P0mQ0 = P0 - Q0 - a = dot3(P, P) - b = dot3(P, Q) - c = dot3(Q, Q) - d = dot3(P, P0mQ0) - e = dot3(Q, P0mQ0) - f = dot3(P0mQ0, P0mQ0) - Δ = a*c - b*b - #assuming both segments are not zero length - #critical points - s0 = clamp01nan(-d/a) - s1 = clamp01nan((b-d)/a) - t0 = clamp01nan(e/c) - t1 = clamp01nan((b+e)/c) - sbar = clamp01nan((b*e - c*d)/Δ) - tbar = clamp01nan((a*e - b*d)/Δ) - @inline r(s,t) = a*(s*s) - 2b*s*t + c*(t*t) + 2d*s - 2e*t - mins = sbar - mint = tbar - minr = r(sbar,tbar) - minr, mins, mint = _min_r_s_t(minr, mins, mint, r(s0,zero(a)), s0, zero(a)) - minr, mins, mint = _min_r_s_t(minr, mins, mint, r(s1,one(a)), s1, one(a)) - minr, mins, mint = _min_r_s_t(minr, mins, mint, r(zero(a),t0), zero(a), t0) - minr, mins, mint = _min_r_s_t(minr, mins, mint, r(one(a),t1), one(a), t1) - d2cl = Base.FastMath.max_fast(zero(a),f+minr) - rcl = (scale3(mint, Q) - scale3(mins, P)) - P0mQ0 - d2cl, rcl, SVector{1,T}(mins), SVector{1,T}(mint) +# The point `X'` of the triangle `A`, `B`, `C` for the point `X`, as above, +# with `AX = A - X`, `E0 = B - A`, `E1 = C - A`, and `N` about `E0×E1`. +# This is the point and triangle kernel without its edges, for the line +# segment and triangle kernels, which have closer candidates on the edges +# already and share `N` between calls. Return `(d2, r, u, v)`, with `r` from `X` to `X'`, `d2 = dot(r, r)`, and `u` +# and `v` the weights of `B` and `C`. +@inline function _tri_face_candidate(AX, E0, E1, N) + ia = inv(dot3(E0, E0)) + c1 = dot3(E0, E1)*ia + # The weights of `B` and `C` at `X'`, clamped into the triangle, `v` first. + v = clamp01nan(-dot3(N, cross3(E0, AX))/dot3(N, N)) + u = clamp01nan(fma(-v, c1, -dot3(E0, AX)*ia)) + # A select, as `Base.min` also checks for NaN, which can't happen here. + w = one(u) - u + v = SIMD.vifelse(v < w, v, w) + r = fma3(v, E1, fma3(u, E0, AX)) + dot3(r, r), r, u, v end #= -Using the simple algorithm and some comments from -https://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf -// David Eberly, Geometric Tools, Redmond WA 98052 -// Copyright (c) 1998-2022 -// Distributed under the Boost Software License, Version 1.0. -// https://www.boost.org/LICENSE_1_0.txt -// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt -// Version: 6.0.2022.01.06 -Ignores the case of degenerate line segments. +The separation between the points at `s` on `x` and `t` on `y` is +`Q0mP0 + t*Q - s*P`. Rather than test where the closest points are, this +takes the closest of three pairs of points: the closest points on `y` of the +end points of `x`, and the closest point on `x` of the point at `t` on `y`, +with `t` from the closest points of the lines through the line segments, +clamped. Each is a pair of points of the line segments, so none is closer +than the true distance, `d2` is `dot(r, r)` of its own separation `r`, which +does not cancel, and `r` is the separation of the points the weights give. + +With `t` clamped to zero or one, `s` is the closest point on `x` of an end +point of `y`, so those pairs need no candidates of their own. So if the +closest points have `s` at an end of `x`, an end point candidate has them, +and otherwise the pair from `t` does. + +`t = -n⋅(P×Q0mP0)/(n⋅n)` with `n = P×Q` is a triple product over `‖n‖²` +(Lagrange's identity), as in Kahan, "Computing Cross-Products and Rotations +in 2- and 3-Dimensional Euclidean Spaces" (2016), §9. Unlike the Gram +determinant `a*c - b*b` of Lumelsky (1985) and Ericson, "Real-Time Collision +Detection" (2005), §5.1.9, it doesn't cancel for nearly parallel line +segments. The rounding of `P×Q0mP0` moves `t` by about a rounding of +`‖Q0mP0‖` over `‖Q‖*sinθ`, which moves the point on `y` by about a rounding +of `‖Q0mP0‖` across the line through `x`. If that moves its closest point on +`x` off an end, the end point there is within as much of `y`. The rounding +of `n` is about a rounding of `‖P‖*‖Q‖`. Along `n` it only scales `t`, which +moves the point on `y` across the line through `x` by at most about a +rounding of `‖Q‖`. In the plane it moves that point by about a rounding of +the height over `sinθ`, which adds to the distance in second order. That is +at most `‖Q‖*sinθ` too, as the point stays on `y`, and the smaller of the +two adds at most about a rounding of `‖Q‖`. + +`n⋅n` and `n⋅(P×Q0mP0)` are at most fourth powers of the coordinates, so +they are in range. When `n⋅n` underflows, `‖P‖*‖Q‖*sinθ` is below about +`floatmin^(1/2)`, so `y` is shorter than about `floatmin^(1/4)`, or `x` is +within that of a line segment parallel to `y`. For parallel line segments +any `t` will do: if an end point of `x` is closest, an end point candidate +has it, and otherwise all of `y` is beside `x`. Zero length or parallel line +segments give NaN or infinite parameters, which `clamp01nan` makes points of +the line segments. + +For a different robust method see Eberly, "Robust Computation of Distance +Between Line Segments" (Geometric Tools, 2018, revised 2023). =# -@inline function dist_sqr(x::LineSeg{T}, y::LineSeg{T}) where T +@inline function dist_sqr_r_s_t(x::LineSeg{T}, y::LineSeg{T}) where T P0 = x[1] - P1 = x[2] Q0 = y[1] - Q1 = y[2] - P = P1-P0 - Q = Q1-Q0 - P0mQ0 = P0 - Q0 - a = dot3(P, P) + P = x[2] - P0 + Q = y[2] - Q0 + # Written as `y - x[1]` so it folds to `Q0` when `x[1]` is positive zero. + Q0mP0 = Q0 - P0 b = dot3(P, Q) c = dot3(Q, Q) - d = dot3(P, P0mQ0) - e = dot3(Q, P0mQ0) - f = dot3(P0mQ0, P0mQ0) - Δ = a*c - b*b - invΔ = inv(Δ) - #assuming both segments are not zero length - #critical points - s0 = clamp01nan(-d/a) - s1 = clamp01nan((b-d)/a) - t0 = clamp01nan(e/c) - t1 = clamp01nan((b+e)/c) - sbar = clamp01nan((b*e - c*d)*invΔ) - tbar = clamp01nan((a*e - b*d)*invΔ) - @inline r(s,t) = a*(s*s) - 2b*s*t + c*(t*t) + 2d*s - 2e*t - Base.FastMath.max_fast(zero(s0),f+Base.FastMath.min_fast( - r(s0,zero(a)), - r(s1,one(a)), - r(zero(a),t0), - r(one(a),t1), - r(sbar,tbar), - )) -end - -#= -Given point O, ray vector D, and a triangle with points (A,B,C), test whether the ray intersects the triangle. -If there is an intersection P. Then P can be represented as - - P = A + u(B-A) + v(C-A) = O + tD - -If there is not an intersection, `t`, `u` and `v` are undefined. -=# -struct SegTriangleIntersectResult{Float} - intersect::Bool - t::Float - u::Float - v::Float + e = dot3(Q, Q0mP0) + n = cross3(P, Q) + # `t` of the closest points of the lines through the line segments, + # clamped, and the closest point on `x` of the point at `t` on `y`. + t = clamp01nan(-dot3(n, cross3(P, Q0mP0))/dot3(n, n)) + s = clamp01nan(fma(t, b, dot3(P, Q0mP0))/dot3(P, P)) + rin = fma3(-s, P, fma3(t, Q, Q0mP0)) + # The closest points on `y` of the end points of `x`. + t0 = clamp01nan(-e/c) + r0 = fma3(t0, Q, Q0mP0) + best = (dot3(r0, r0), r0, zero(T), t0) + t1 = clamp01nan((b - e)/c) + r1 = fma3(t1, Q, Q0mP0 - P) + best = _min_d2_r_s_t(best, (dot3(r1, r1), r1, one(T), t1)) + # Last, so a tie with an end point keeps its exact weight. + d2, r, s, t = _min_d2_r_s_t(best, (dot3(rin, rin), rin, s, t)) + d2, r, SVector{1,T}(s), SVector{1,T}(t) end #= -A fast ray-triangle intersection algorithm by Moller and Trumbore. -Ref: Tomas Möller and Ben Trumbore, "Fast, minimum storage ray-triangle intersection" (1997) Journal of Graphics Tools. -=# -function moller_trumbore_intersect(o, d, a, b, c) - fzero = zero(eltype(o)) - seg_triangle_no_intersect = SegTriangleIntersectResult(false, fzero, fzero, fzero) +The closest points of a line segment `x` and a triangle are on an edge of +the triangle, or are an end point of `x` over the face, or are where `x` +crosses the face, at distance zero. If `x` is parallel to the face, sliding +along it keeps the distance until an end point or an edge. So this takes the +closest of six pairs of points: the closest points of `x` and each edge, the +end points of `x` with their `X'` as for a point and a triangle, and the +point of `x` where its line meets the plane of the triangle, with `s` +clamped, with its `X'`. Each is a pair of points of the simplexes, so none is +closer than the true distance, and `d2` is `dot(r, r)` of its own separation +`r`, which does not cancel. An `X'` past an edge is some point of the +triangle, and the edge pair is at least as close. - rab = b - a - rac = c - a - cross_d_rac = cross3(d, rac) - det = dot3(cross_d_rac, rab) +Testing whether `x` crosses the triangle instead, as Möller and Trumbore, +"Fast, minimum storage ray-triangle intersection" (1997), decides the +distance is zero from the signs of triple products, which are rounding +errors when `x` is in the plane of the triangle. Then a line segment beside +a triangle can get a distance of zero. - if det == zero(det) - return seg_triangle_no_intersect - end +`s = N⋅(A - P0)/(N⋅P)`. The rounding of the dot products moves the point +along `x` by about a rounding of `‖A - P0‖` or `s*‖P‖` over the sine of the +angle between `x` and the plane, which moves it off the plane by just a +rounding of those. A tilt of `N` by an angle `τ` moves the point off the +plane by about `τ*‖X - A‖`, and so the distance at first order, unlike for a +point. `cross3` tilts `N` by a rounding over the sine of the triangle's +smallest angle, which for a line segment through a sliver gave errors near +`√eps*ℓ`, so `N` is `cross3_accurate`, which tilts it by about a rounding. - invdet = inv(det) - rao = o - a - u = dot3(cross_d_rac, rao) * invdet - if u < 0 || u > 1 - return seg_triangle_no_intersect - end +When `N⋅N` underflows the edges are within about `floatmin^(1/4)` of every +point of the triangle, as for a point and a triangle. A line segment +parallel to the plane gives NaN or infinite `s`, which `clamp01nan` makes an +end point. - cross_rao_rab = cross3(rao, rab) - v = dot3(cross_rao_rab, d) * invdet - if v < 0 || u + v > 1 - return seg_triangle_no_intersect - end - - t = dot3(cross_rao_rab, rac) * invdet - if t < 0 || t > 1 - return seg_triangle_no_intersect +For two triangles, the closest points are on an edge of each, or are a +vertex of one over the face of the other, or are where an edge of one +crosses the other. So that takes the closest of the pairs of edges, and of +the vertices and the edges of each with the face of the other, in the same +way. +=# +@inline function dist_sqr_r_s_t(x::LineSeg{T}, y::Triangle{T}) where T + A = y[1] + E0 = y[2] - A + E1 = y[3] - A + N = cross3_accurate(E0, E1) + z = zero(T) + # The closest points of `x` and the edges. + d2, r, s, t = dist_sqr_r_s_t(x, SA[y[1], y[2]]) + best = (d2, r, s, SVector(t[1], z)) + d2, r, s, t = dist_sqr_r_s_t(x, SA[y[1], y[3]]) + best = _min_d2_r_s_t(best, (d2, r, s, SVector(z, t[1]))) + d2, r, s, t = dist_sqr_r_s_t(x, SA[y[2], y[3]]) + best = _min_d2_r_s_t(best, (d2, r, s, SVector(one(T) - t[1], t[1]))) + # The end points of `x` with the face. + for i in 1:2 + d2, r, u, v = _tri_face_candidate(A - x[i], E0, E1, N) + best = _min_d2_r_s_t(best, (d2, r, SVector(ifelse(i == 1, z, one(T))), SVector(u, v))) end - - SegTriangleIntersectResult(true, t, u, v) + # Last, so a tie keeps an exact weight. + d2, r, s, u, v = _seg_tri_crossing_candidate(x[1], x[2], A, E0, E1, N) + _min_d2_r_s_t(best, (d2, r, SVector(s), SVector(u, v))) end -function dist_sqr_r_s_t(x::LineSeg{T}, y::Triangle{T}) where T - fzero = zero(T) - o = x[1] - d = x[2] - x[1] - a = y[1] - b = y[2] - c = y[3] - result = moller_trumbore_intersect(o, d, a, b, c) - if result.intersect == true - fzero, zero(Vec3{T}), SA[result.t,], SA[result.u, result.v] - else - # no intersection or seg and triangle are parallel. - # min distance is on an edge of the triangle. - d2min, minr, mins, _tmin = dist_sqr_r_s_t(x,SA[a,b]) - mint = SA[_tmin[1], fzero] - d2, _rmin, _smin, _tmin = dist_sqr_r_s_t(x,SA[b,c]) - if d2 < d2min - d2min = d2 - minr = _rmin - mins = _smin - mint = SA[one(T) - _tmin[1], _tmin[1]] - end - d2, _rmin, _smin, _tmin = dist_sqr_r_s_t(x,SA[c,a]) - if d2 < d2min - d2min = d2 - minr = _rmin - mins = _smin - mint = SA[fzero, one(T) - _tmin[1]] - end - d2, _rmin, _, _tri_tmin = dist_sqr_r_s_t(SA[x[1],],y) - if d2 < d2min - d2min = d2 - minr = _rmin - mins = SA[fzero] - mint = _tri_tmin - end - d2, _rmin, _, _tri_tmin = dist_sqr_r_s_t(SA[x[2],],y) - if d2 < d2min - d2min = d2 - minr = _rmin - mins = SA[one(T)] - mint = _tri_tmin - end - d2min, minr, mins, mint - end +# The point at `s` of the line segment from `P0` to `P1` where its line meets +# the plane of the triangle with `y[1] = A`, `E0`, `E1`, and `N` about +# `E0×E1`, with `s` clamped, and its `X'`. Return `(d2, r, s, u, v)`. +@inline function _seg_tri_crossing_candidate(P0, P1, A, E0, E1, N) + P = P1 - P0 + AP0 = A - P0 + s = clamp01nan(dot3(N, AP0)/dot3(N, P)) + d2, r, u, v = _tri_face_candidate(fma3(-s, P, AP0), E0, E1, N) + d2, r, s, u, v end -function dist_sqr(x::LineSeg{T}, y::Triangle{T})::T where T - fzero = zero(T) - o = x[1] - d = x[2] - x[1] - a = y[1] - b = y[2] - c = y[3] - result = moller_trumbore_intersect(o, d, a, b, c) - if result.intersect == true - fzero - else - # no intersection or seg and triangle are parallel. - # min distance is on an edge of the triangle. - dab = dist_sqr(x,SA[a,b]) - dbc = dist_sqr(x,SA[b,c]) - dca = dist_sqr(x,SA[c,a]) - dx1y = dist_sqr(SA[x[1],],y) - dx2y = dist_sqr(SA[x[2],],y) - Base.FastMath.min_fast( - dab, - dbc, - dca, - dx1y, - dx2y, - ) - end +# The weights of vertices 2 and 3 of a triangle at the point +# `(1 - w)*V[i] + w*V[j]` on an edge, with `i < j`, or at `V[i]` with +# `i == j` and `w = 0`. +@inline function _tri_weights(i, j, w) + z = zero(w) + wi = one(w) - w + SVector(ifelse(i == 2, wi, ifelse(j == 2, w, z)), ifelse(i == 3, wi, ifelse(j == 3, w, z))) end -function dist_sqr_r_s_t(x::Triangle{T}, y::Triangle{T}) where T - fzero = zero(T) - fone = one(T) - d2, minr, _s, mint = dist_sqr_r_s_t(SA[x[1],x[2]], y) - _s_all = SA[fzero, _s[1], fone - _s[1]] - mins = SA[_s_all[2], _s_all[1]] - iszero(d2) && return d2, minr, mins, mint - ai = 2 - bi = 3 - ci = 1 - for i in 2:3 - this_d2, _r, _s, _t = dist_sqr_r_s_t(SA[x[ai],x[bi]], y) - if this_d2 < d2 - d2 = this_d2 - minr = _r - _s_all = SA[fzero, _s[1], fone - _s[1]] - mins = SA[_s_all[bi], _s_all[ai]] - mint = _t - end - iszero(d2) && return d2, minr, mins, mint - ai, bi, ci = bi, ci, ai - end - for i in 1:3 - this_d2, _r, _s, _t = dist_sqr_r_s_t(SA[y[ai],y[bi]], x) - if this_d2 < d2 - d2 = this_d2 - minr = -_r - _s_all = SA[fzero, _s[1], fone - _s[1]] - mint = SA[_s_all[bi], _s_all[ai]] - mins = _t - end - iszero(d2) && return d2, minr, mins, mint - ai, bi, ci = bi, ci, ai +@inline function dist_sqr_r_s_t(x::Triangle{T}, y::Triangle{T}) where T + Ax = x[1] + E0x = x[2] - Ax + E1x = x[3] - Ax + Nx = cross3_accurate(E0x, E1x) + Ay = y[1] + E0y = y[2] - Ay + E1y = y[3] - Ay + Ny = cross3_accurate(E0y, E1y) + z = zero(T) + edges = ((1, 2), (1, 3), (2, 3)) + # A pair of vertices, to start. + r = Ay - Ax + best = (dot3(r, r), r, SVector(z, z), SVector(z, z)) + # The closest points of the pairs of edges. + for (i, j) in edges, (k, l) in edges + d2, r, s, t = dist_sqr_r_s_t(SA[x[i], x[j]], SA[y[k], y[l]]) + best = _min_d2_r_s_t(best, (d2, r, _tri_weights(i, j, s[1]), _tri_weights(k, l, t[1]))) end - return d2, minr, mins, mint -end - -function dist_sqr(x::Triangle{T}, y::Triangle{T}) where T - d2 = dist_sqr(SA[x[1],x[2]], y) - iszero(d2) && return d2 - ai = 2 - bi = 3 - ci = 1 - for i in 2:3 - this_d2 = dist_sqr(SA[x[ai],x[bi]], y) - d2 = Base.FastMath.min_fast(d2, this_d2) - iszero(d2) && return d2 - ai, bi, ci = bi, ci, ai + # The vertices of each with the face of the other. + for k in 1:3 + d2, r, u, v = _tri_face_candidate(Ay - x[k], E0y, E1y, Ny) + best = _min_d2_r_s_t(best, (d2, r, _tri_weights(k, k, z), SVector(u, v))) + d2, r, u, v = _tri_face_candidate(Ax - y[k], E0x, E1x, Nx) + best = _min_d2_r_s_t(best, (d2, -r, SVector(u, v), _tri_weights(k, k, z))) end - for i in 1:3 - this_d2 = dist_sqr(SA[y[ai],y[bi]], x) - d2 = Base.FastMath.min_fast(d2, this_d2) - iszero(d2) && return d2 - ai, bi, ci = bi, ci, ai + # Last, so a tie keeps an exact weight. The edges of each with the face + # of the other. + for (i, j) in edges + d2, r, s, u, v = _seg_tri_crossing_candidate(x[i], x[j], Ay, E0y, E1y, Ny) + best = _min_d2_r_s_t(best, (d2, r, _tri_weights(i, j, s), SVector(u, v))) + d2, r, s, u, v = _seg_tri_crossing_candidate(y[i], y[j], Ax, E0x, E1x, Nx) + best = _min_d2_r_s_t(best, (d2, -r, SVector(u, v), _tri_weights(i, j, s))) end - return d2 + best end + diff --git a/src/util.jl b/src/util.jl index 7c0ec1a..3cf6aaf 100644 --- a/src/util.jl +++ b/src/util.jl @@ -1,5 +1,8 @@ +# Clamp `x` to `[0, 1]`, with NaN to zero. Written as selects that LLVM makes +# one max and one min, as `x > 0` is false for NaN. @inline function clamp01nan(x) - SIMD.vifelse(isnan(x), zero(x), Base.FastMath.min_fast(Base.FastMath.max_fast(x, zero(x)), one(x))) + y = SIMD.vifelse(x > zero(x), x, zero(x)) + SIMD.vifelse(y < one(x), y, one(x)) end """ @@ -26,6 +29,31 @@ Also works on `SVector{3, SIMD.Vec{N,T}}`, bit identical to the scalar version i ) end +""" + cross3_accurate(a::Vec3, b::Vec3) + +Return the cross product of two 3D vectors, each component accurate to about +one rounding, using Kahan's algorithm for `p*q - u*v` with fused multiply adds. + +[`cross3`](@ref) is cheaper, but for nearly parallel `a` and `b` its +components cancel, with an error of about a rounding of `norm(a)*norm(b)`. + +Also works on `SVector{3, SIMD.Vec{N,T}}`, bit identical to the scalar version in each lane. +""" +@inline function cross3_accurate(a::Vec3, b::Vec3) + SVector( + _det2_accurate(a[2], b[3], a[3], b[2]), + _det2_accurate(a[3], b[1], a[1], b[3]), + _det2_accurate(a[1], b[2], a[2], b[1]), + ) +end + +# `p*q - u*v`, with the rounding of `u*v` recovered by a fused multiply add. +@inline function _det2_accurate(p, q, u, v) + uv = u*v + fma(p, q, -uv) + fma(-u, v, uv) +end + """ scale3(t, v::Vec3) scale3(v::Vec3, t) @@ -37,6 +65,15 @@ Unlike `t*v`, this also works when `t` is a `SIMD.Vec{N,T}`. @inline scale3(t::Union{Number, SIMD.Vec}, v::Vec3) = SVector(t*v[1], t*v[2], t*v[3]) @inline scale3(v::Vec3, t::Union{Number, SIMD.Vec}) = SVector(v[1]*t, v[2]*t, v[3]*t) +""" + fma3(t, a::Vec3, b::Vec3) + +Return `t*a + b` elementwise, computed with fused multiply adds. + +Also works when `t` is a `SIMD.Vec{N,T}`, bit identical to the scalar version in each lane. +""" +@inline fma3(t::Union{Number, SIMD.Vec}, a::Vec3, b::Vec3) = SVector(fma(t, a[1], b[1]), fma(t, a[2], b[2]), fma(t, a[3], b[3])) + function relu(x::T)::T where T SIMD.vifelse(x > zero(x), x, zero(x)) end @@ -51,7 +88,7 @@ Return the Euclidean norm of a 3D vector, using [`dot3`](@ref) and an unchecked Also works on `SVector{3, SIMD.Vec{N,T}}`. """ -norm_fast(v::Vec3) = _sqrt_fast(dot3(v, v)) +@inline norm_fast(v::Vec3) = _sqrt_fast(dot3(v, v)) # Return the zero based XYZ grid index. # `pos_norm` has an origin at the center of the grid diff --git a/test/test-mindistance.jl b/test/test-mindistance.jl index c2743f7..d47b585 100644 --- a/test/test-mindistance.jl +++ b/test/test-mindistance.jl @@ -39,7 +39,7 @@ function distance_tests(as, bs) local b = bs[i] local d2, r, s, t = dist_sqr_r_s_t(a, b) @test refDist2(a, b) ≈ d2 atol = 1E-10 rtol = 1E-11 - @test d2 ≈ dist_sqr(a, b) atol = 1E-10 rtol = 1E-11 + @test d2 === dist_sqr(a, b) @test r ⋅ r ≈ d2 atol = 1E-10 rtol = 1E-11 local s_all = [one(eltype(s)) - sum(s); s;] local t_all = [one(eltype(t)) - sum(t); t;] @@ -58,6 +58,27 @@ end distance_tests(as, bs) end +# Vertices drawn from a small pool, so simplexes share vertices and are often +# degenerate: zero length line segments, triangles with repeated vertices. +@testset "distance with shared vertices $N to $M" for N in 1:3, M in 1:3 + trials = 300 + pools = [rand(SVector{3,Float64}, 4) for _ in 1:trials] + as = [SVector{N}(ntuple(_ -> rand(pool), N)) for pool in pools] + bs = [SVector{M}(ntuple(_ -> rand(pool), M)) for pool in pools] + distance_tests(as, bs) +end + +@testset "Float32 distance with shared vertices $N to $M" for N in 1:3, M in 1:3 + for trial in 1:3000 + box = 50 + 100rand() + pool = [SVector{3, Float32}(rand(3) .* box) for _ in 1:6] + a = SVector{N}(ntuple(_ -> rand(pool), N)) + b = SVector{M}(ntuple(_ -> rand(pool), M)) + ref = sqrt(dist_sqr(map(p -> Float64.(p), a), map(p -> Float64.(p), b))) + @test abs(sqrt(dist_sqr(a, b)) - ref) ≤ 4*eps(Float32)*box + end +end + @testset "line to triangle edge case" begin a = SA[ SA[0.7962602087717146, 0.7678176789137129, 0.33365738129157096], @@ -76,6 +97,299 @@ end @test ref_d2min ≈ d2min atol = 1E-11 rtol = 1E-11 end +# Simplexes about 100 across, 0.01 apart. Expanding the squared distance, as +# Float64 does, gives a Float32 `d2` that is far off or zero here, and for +# nearly parallel line segments `a*c - b*b` cancels and moves the closest point. +@testset "Float32 large simplexes close together $name" for (name, a, b) in ( + ("point-triangle", + SA[SA[3f0, 2f0, 0.01f0]], + SA[SA[-23f0, -7f0, 0f0], SA[31f0, -9f0, 0f0], SA[2f0, 27f0, 0f0]]), + ("line-line crossing", + SA[SA[-37f0, 0f0, 0f0], SA[63f0, 0f0, 0f0]], + SA[SA[-20.5f0, -35.507042f0, 0.01f0], SA[29.5f0, 51.0955f0, 0.01f0]]), + ("line-line nearly parallel", + SA[SA[-50f0, 0f0, 0f0], SA[50f0, 0f0, 0f0]], + SA[SA[-30f0, -0.1f0, 0.01f0], SA[70f0, 0.1f0, 0.01f0]]), + ("line-line nearly parallel closest point", + SA[SA[-50f0, 0f0, 0f0], SA[50f0, 0f0, 0f0]], + SA[SA[-32.51f0, -0.17f0, 0.01f0], SA[67.49f0, 0.17f0, 0.01f0]]), + ("line-triangle", + SA[SA[-40f0, 3f0, 0.01f0], SA[60f0, -2f0, 0.02f0]], + SA[SA[-23f0, -7f0, 0f0], SA[31f0, -9f0, 0f0], SA[2f0, 27f0, 0f0]]), + ("triangle-triangle", + SA[SA[-20f0, -5f0, 0.01f0], SA[25f0, -6f0, 0.02f0], SA[0f0, 30f0, 0.015f0]], + SA[SA[-23f0, -7f0, 0f0], SA[31f0, -9f0, 0f0], SA[2f0, 27f0, 0f0]]), + ) + for (x, y) in ((a, b), (b, a)) + # Float64 of the same coordinates, which is accurate here. + ref = sqrt(dist_sqr(map(p -> Float64.(p), x), map(p -> Float64.(p), y))) + @test 0.0099 < ref < 0.013 + d2, r, s, t = dist_sqr_r_s_t(x, y) + @test sqrt(d2) ≈ ref rtol = 1E-5 + @test norm(r) ≈ ref rtol = 1E-5 + local s_all = [one(eltype(s)) - sum(s); s;] + local t_all = [one(eltype(t)) - sum(t); t;] + @test norm(r - (sum(y .* t_all) - sum(x .* s_all))) ≤ 1E-4 + end +end + +# A needle triangle 100 long with a short edge 0.001 long, and a point 0.01 +# above its plane that projects 0.2 past the short edge. Testing +# `sbar + tbar ≤ 1` judged it inside in `Float32`, giving a distance of 0.01. +@testset "Float32 point past the short edge of a needle" begin + Random.seed!(5) + for trial in 1:3000 + n = normalize(randn(3)) + e1 = normalize(cross(n, randn(3))) + e2 = cross(n, e1) + a = 100randn(3) + b = a .+ 100e1 + c = b .+ 0.001e2 + p = b .+ 0.001rand()*e2 .+ 0.2rand()*e1 .+ 0.01n + x = SA[SVector{3, Float32}(p)] + y = SA[SVector{3, Float32}(a), SVector{3, Float32}(b), SVector{3, Float32}(c)] + ref = sqrt(dist_sqr(map(v -> Float64.(v), x), map(v -> Float64.(v), y))) + @test abs(sqrt(dist_sqr(x, y)) - ref) ≤ 16*eps(Float32)*200 + @test abs(sqrt(dist_sqr(y, x)) - ref) ≤ 16*eps(Float32)*200 + end +end + +# Line segments on nearly the same line, or nearly parallel. For collinear +# segments to within rounding, `n = P×Q` and `Q0mP0×Q` were both rounding +# errors, so `sbar` and `tbar` were arbitrary, the inside test could pass, +# and the distance between disjoint segments 0.56 apart came out as 3e-8. +# Nearly parallel overlapping segments got the distance right, but with +# weights giving points far apart. Integer coordinates miss this, as an +# exactly collinear `n` is zero. +@testset "$T nearly collinear and nearly parallel line segments" for T in (Float32, Float64) + rng = Random.Xoshiro(29) + # The case 0.56 apart, kept since the random ones below fail it only + # about once in 700 for the old method. + cases = Any[( + SA[SA[-1.1168967f0, -0.4735017f0, -0.30628234f0], SA[-0.43016562f0, 0.06529092f0, -0.054257788f0]], + SA[SA[0.96934664f0, 1.1633145f0, 0.45935148f0], SA[-0.007197912f0, 0.39714116f0, 0.10096781f0]], + )] + for trial in 1:10000 + q = normalize(randn(rng, 3)) + e1 = normalize(cross(q, randn(rng, 3))) + e2 = cross(q, e1) + o = randn(rng, 3) + if isodd(trial) + # Four points on a line, with offsets below a rounding or a few. + ts = randn(rng, 4) + pts = [o .+ t .* q .+ rand(rng, (0, 0.1, 4)) * eps(T) .* (randn(rng)*e1 .+ randn(rng)*e2) for t in ts] + xt, yt = (pts[1], pts[2]), (pts[3], pts[4]) + else + # Nearly parallel, offset along the line and across it. + θ = 10.0^(-rand(rng)*12) + dq = cos(θ)*q .+ sin(θ)*e1 + c = o .+ 2randn(rng)*q .+ 10.0^(-rand(rng)*6)*randn(rng)*e2 + xt = (o .- q, o .+ q) + yt = (c .- exp(randn(rng)) .* dq, c .+ exp(randn(rng)) .* dq) + end + push!(cases, (SVector{2}(map(v -> SVector{3, T}(v), xt)), SVector{2}(map(v -> SVector{3, T}(v), yt)))) + end + for (x, y) in cases + x = map(v -> SVector{3, T}(v), x) + y = map(v -> SVector{3, T}(v), y) + tol = 8*eps(T)*maximum(v -> maximum(abs, v), (x..., y...)) + for (a, b) in ((x, y), (y, x)) + ref = sqrt(dist_sqr(map(v -> big.(v), a), map(v -> big.(v), b))) + d2, r, s, t = dist_sqr_r_s_t(a, b) + @test abs(sqrt(d2) - ref) ≤ tol + # `r` is the separation of the points the weights give. + pa = (1 - big(s[1])) .* a[1] .+ big(s[1]) .* a[2] + pb = (1 - big(t[1])) .* b[1] .+ big(t[1]) .* b[2] + @test norm(r .- (pb .- pa)) ≤ tol + @test abs(sqrt(d2) - norm(r)) ≤ tol + end + end +end + +# Line segments in, or nearly in, the plane of a triangle beside it, and line +# segments through the face of a sliver, and triangles with those as an edge. +# Testing whether the line segment crosses the triangle, from the signs of +# triple products that are rounding errors in the plane, gave a distance of +# zero for line segments 1.7 from the triangle, about one time in 300. Through +# a sliver, `N = E0×E1` from `cross3` tilted the plane enough for errors near +# `√eps*ℓ`. +@testset "$T line segments and triangles near the plane of a triangle" for T in (Float32, Float64) + rng = Random.Xoshiro(31) + # The case 1.7 apart. + cases = Any[( + SA[SA[0.27440169104786705, -1.8352000938383282, -1.1414620437379672], SA[-0.4237474034660068, -1.74076257090927, -0.005187092841748675]], + SA[SA[-0.051006641386534685, 0.8264827967799175, 0.247674370931405], SA[0.3840271906908225, 0.44295798307158735, -0.5669754202169636], SA[-0.3934509020879434, -0.08075321479510644, 0.491917584990893]], + )] + for trial in 1:3000 + n = normalize(randn(rng, 3)) + e1 = normalize(cross(n, randn(rng, 3))) + e2 = cross(n, e1) + o = randn(rng, 3) + P(a, b, c = 0) = o .+ a .* e1 .+ b .* e2 .+ c .* n + θ = rand(rng, (0.0, 10.0^(-rand(rng)*12))) + if isodd(trial) + # A unit triangle, and a line segment beside it, tilted out of the + # plane by about θ. + yt = (P(0, 0), P(1, 0), P(0, 1)) + xt = (P(2 + rand(rng), -1 + 3rand(rng), -θ), P(-1 + 3rand(rng), 2 + rand(rng), θ)) + else + # A sliver, and a line segment through its face. + yt = (P(0, 0), P(1, 0), P(0.5 + rand(rng), 10.0^(-rand(rng)*12))) + w = rand(rng, 3) + X = sum(w ./ sum(w) .* collect(yt)) + d = normalize(randn(rng, 3)) + a = rand(rng) + xt = (X .- a .* d, X .+ (1 - a) .* d) + end + x = SVector{2}(map(v -> SVector{3, T}(v), xt)) + y = SVector{3}(map(v -> SVector{3, T}(v), yt)) + # A triangle with `x` as an edge, its third vertex 1 or θ away. + c = SVector{3, T}(xt[1] .+ ifelse(rand(rng, Bool), 1, θ) .* normalize(randn(rng, 3))) + push!(cases, (x, y), (SA[x[1], x[2], c], y)) + end + for (x, y) in cases + x = map(v -> SVector{3, T}(v), x) + y = map(v -> SVector{3, T}(v), y) + tol = 8*eps(T)*maximum(v -> maximum(abs, v), (x..., y...)) + for (a, b) in ((x, y), (y, x)) + ref = sqrt(dist_sqr(map(v -> big.(v), a), map(v -> big.(v), b))) + d2, r, s, t = dist_sqr_r_s_t(a, b) + @test abs(sqrt(d2) - ref) ≤ tol + @test all(≥(0), s) && all(≥(0), t) && sum(s) ≤ 1 && sum(t) ≤ 1 + # `r` is the separation of the points the weights give. + pa = (1 - sum(big.(s))) .* a[1] .+ sum(big.(s) .* a[2:end]) + pb = (1 - sum(big.(t))) .* b[1] .+ sum(big.(t) .* b[2:end]) + @test norm(r .- (pb .- pa)) ≤ tol + end + end +end + +# Hard points: one coordinate carrying the separation, separations that +# cancel, nearly parallel line segments, and points on the border between +# two closest features, where rounding picks either. Each is tested as given, +# then with Gaussian noise of a few ulps on the coordinates, which scrambles +# the roundings and moves the border points to either side. +const hard_points = [ + ("point-point, one coordinate separation", + ((-1.0, 0.03, -0.02),), ((1.0, 0.06, 0.03),)), + ("point-point, balanced coordinates", + ((-0.95, -1.0, 0.9),), ((0.9, 0.85, -1.0),)), + ("point-line, closest at the first end point", + ((1.0, 0.12, -0.03),), ((-1.0, 0.0, 0.0), (-1.0, -1.0, 0.0))), + ("point-line, closest inside", + ((1.0, 0.0, 0.05),), ((-1.0, 1.0, 0.0), (-1.0, -1.0, 0.0))), + ("point-line, closest inside a short line segment", + ((1.0, 0.0, 0.0),), ((-1.0, 0.1, 0.02), (-1.0, -0.1, 0.02))), + # The second end point is near the point, so `y[2] - x` cancels. + ("point-line, closest at the second end point", + ((1.0, 1.0, 0.04),), ((-1.0, -1.0, 0.0), (0.98, 0.98, 0.0))), + ("point-line, border at the first end point", + ((1.0, 1.0, 0.0),), ((-1.0, 0.0, 0.0), (-0.5, -1.0, 0.0))), + ("point-line, border at the second end point", + ((0.5, 0.5, 0.0),), ((-1.0, 1.0, 0.0), (0.0, 0.0, 0.0))), + # A tilted triangle, so no product in `N` or `N⋅(X - A)` is exact. + ("point-triangle, over the face of a tilted triangle", + ((-0.01, 0.01, 0.29),), ((-0.9, -0.6, 0.2), (0.8, -0.2, -0.3), (-0.2, 0.9, 0.1))), + ("point-triangle, near the face of a tilted triangle", + ((-0.09, 0.03, 0.02),), ((-0.9, -0.6, 0.2), (0.8, -0.2, -0.3), (-0.2, 0.9, 0.1))), + ("point-triangle, closest inside edge AB", + ((0.2, -0.9, 0.4),), ((-0.9, -0.6, 0.0), (0.8, -0.2, 0.0), (-0.2, 0.9, 0.0))), + ("point-triangle, closest inside edge BC", + ((0.56, 0.59, 0.3),), ((-0.9, -0.6, 0.0), (0.8, -0.2, 0.0), (-0.2, 0.9, 0.0))), + ("point-triangle, closest at vertex A", + ((-1.0, -0.95, 0.35),), ((-0.9, -0.6, 0.0), (0.8, -0.2, 0.0), (-0.2, 0.9, 0.0))), + ("point-triangle, closest at vertex B", + ((1.0, -0.45, 0.3),), ((-0.9, -0.6, 0.0), (0.8, -0.2, 0.0), (-0.2, 0.9, 0.0))), + ("point-triangle, border of the face and edge AB, over its midpoint", + ((-0.05, -0.4, 0.6),), ((-0.9, -0.6, 0.0), (0.8, -0.2, 0.0), (-0.2, 0.9, 0.0))), + ("point-triangle, border of edge AB and vertex B", + ((0.88, -0.54, 0.3),), ((-0.9, -0.6, 0.0), (0.8, -0.2, 0.0), (-0.2, 0.9, 0.0))), + ("line-line, crossing", + ((-1.0, -0.2, 0.0), (1.0, 0.2, 0.0)), ((-0.3, -1.0, 0.25), (0.3, 1.0, 0.25))), + ("line-line, nearly parallel crossing", + ((-1.0, -0.004, 0.0), (1.0, 0.004, 0.0)), ((-1.0, 0.004, 0.15), (1.0, -0.004, 0.15))), + ("line-line, inside x and at the first end point of y", + ((-1.0, -0.2, 0.0), (1.0, 0.2, 0.0)), ((0.1, 0.3, 0.3), (0.9, 0.9, 0.5))), + ("line-line, at the first end point of x and inside y", + ((-0.4, -0.3, 0.0), (0.56, 0.42, 0.0)), ((-0.31, -0.67, 0.25), (-0.73, -0.11, 0.25))), + ("line-line, at the first end points", + ((-0.4, -0.3, 0.0), (0.56, 0.42, 0.0)), ((-0.7, -0.8, 0.3), (-1.0, -0.9, 0.5))), + ("line-line, border with y crossing over the first end point of x", + ((-0.8, -0.6, 0.0), (0.8, 0.6, 0.0)), ((-0.68, -0.76, 0.25), (-0.92, -0.44, 0.25))), + ("line-line, border of the first end points and inside y", + ((-0.4, -0.3, 0.0), (0.56, 0.42, 0.0)), ((-0.52, -0.39, 0.25), (-0.82, 0.01, 0.25))), +] + +@testset "hard point: $name" for (name, xt, yt) in hard_points + rng = Random.Xoshiro(2026) + for T in (Float32, Float64) + for σ in (0, 8, 64, 1024) .* eps(T), _ in 1:(iszero(σ) ? 1 : 100) + x = SVector(map(v -> SVector{3, T}(v .+ σ .* randn(rng, 3)), xt)) + y = SVector(map(v -> SVector{3, T}(v .+ σ .* randn(rng, 3)), yt)) + tol = 8*eps(T)*maximum(v -> maximum(abs, v), (x..., y...)) + ref = sqrt(dist_sqr(map(v -> big.(v), x), map(v -> big.(v), y))) + @test abs(sqrt(dist_sqr(x, y)) - ref) ≤ tol + @test abs(sqrt(dist_sqr(y, x)) - ref) ≤ tol + end + end +end + +# Small or thin simplexes, where a squared length, or a squared area like +# `Δ = |n|²`, is out of range. When something underflows the distance may +# lose accuracy, but it must stay finite and off by at most about +# `floatmin^(1/4)`, which bounds the inradius of a triangle with `Δ` below +# `floatmin`. The point-triangle method used to form `inv(Δ)`, and returned +# `Inf` for a point over the face when it overflowed, or `NaN` in the plane, +# even for a unit sliver. When `Δ` underflows the line-line `t` is arbitrary, +# and when `‖r‖²` is subnormal the point-line quotient for `t` keeps only a +# few bits, but both are within the floor. Nearly parallel unit line +# segments do not reach this: their end points are within about ℓ·sinθ of +# the crossing. +const tiny_normal_cases = [ + ("point-triangle, tiny, inv(Δ) overflows", Float32, + ((0.25e-10, 0.25e-10, 1e-10),), ((0, 0, 0), (1e-10, 0, 0), (0, 1e-10, 0))), + ("point-triangle, tiny, Δ underflows", Float32, + ((0.25e-12, 0.25e-12, 1e-12),), ((0, 0, 0), (1e-12, 0, 0), (0, 1e-12, 0))), + ("point-triangle, unit sliver, inv(Δ) overflows", Float32, + ((1.5, 0.6e-20, 0.1),), ((0, 0, 0), (1, 0, 0), (2, 1e-20, 0))), + ("point-triangle, unit sliver, in plane", Float32, + ((1.5, 0.6e-20, 0),), ((0, 0, 0), (1, 0, 0), (2, 1e-20, 0))), + ("line-line, tiny crossing, inv(Δ) overflows", Float32, + ((-0.5e-10, 0, 0), (0.5e-10, 0, 0)), ((0, -0.5e-10, 1e-10), (0, 0.5e-10, 1e-10))), + ("line-line, tiny crossing, Δ underflows", Float32, + ((-0.5e-12, 0, 0), (0.5e-12, 0, 0)), ((0, -0.5e-12, 1e-12), (0, 0.5e-12, 1e-12))), + ("point-triangle, tiny, inv(Δ) overflows", Float64, + ((0.25e-80, 0.25e-80, 1e-80),), ((0, 0, 0), (1e-80, 0, 0), (0, 1e-80, 0))), + ("point-triangle, tiny, Δ underflows", Float64, + ((0.25e-90, 0.25e-90, 1e-90),), ((0, 0, 0), (1e-90, 0, 0), (0, 1e-90, 0))), + ("point-triangle, unit sliver, inv(Δ) overflows", Float64, + ((1.5, 0.6e-160, 0.1),), ((0, 0, 0), (1, 0, 0), (2, 1e-160, 0))), + ("point-triangle, unit sliver, in plane", Float64, + ((1.5, 0.6e-160, 0),), ((0, 0, 0), (1, 0, 0), (2, 1e-160, 0))), + ("line-line, tiny crossing, inv(Δ) overflows", Float64, + ((-0.5e-80, 0, 0), (0.5e-80, 0, 0)), ((0, -0.5e-80, 1e-80), (0, 0.5e-80, 1e-80))), + ("line-line, tiny crossing, Δ underflows", Float64, + ((-0.5e-90, 0, 0), (0.5e-90, 0, 0)), ((0, -0.5e-90, 1e-90), (0, 0.5e-90, 1e-90))), + ("point-line, tiny, subnormal norm", Float32, + ((0.5e-22, 1e-22, 0),), ((0, 0, 0), (1e-22, 0, 0))), + ("point-line, tiny, subnormal norm", Float64, + ((0.5e-160, 1e-160, 0),), ((0, 0, 0), (1e-160, 0, 0))), + ("line-line, tiny, inv of a norm overflows", Float32, + ((0, 0, 0), (1e-21, 0, 0)), ((0.5e-21, 1e-21, 0), (0.5e-21, 2e-21, 0))), + ("line-line, tiny, inv of a norm overflows", Float64, + ((0, 0, 0), (1e-155, 0, 0)), ((0.5e-155, 1e-155, 0), (0.5e-155, 2e-155, 0))), +] + +@testset "tiny normal: $name $T" for (name, T, xt, yt) in tiny_normal_cases + x = SVector(map(v -> SVector{3, T}(v), xt)) + y = SVector(map(v -> SVector{3, T}(v), yt)) + ref = sqrt(dist_sqr(map(v -> big.(v), x), map(v -> big.(v), y))) + # A few roundings of the largest coordinate, plus the underflow floor. + tol = 8*eps(T)*maximum(v -> maximum(abs, v), (x..., y...)) + floatmin(T)^(1/4) + @test abs(sqrt(dist_sqr(x, y)) - ref) ≤ tol + @test abs(sqrt(dist_sqr(y, x)) - ref) ≤ tol +end + # Return `W` simplices as one simplex with a lane per simplex. function simplex_lanes(xs::NTuple{W, SVector{N, SVector{3, T}}}) where {W, N, T} SVector{N}(ntuple(Val(N)) do v @@ -98,7 +412,7 @@ end # The grid neighbor list checks distances several pairs at a time, which # must give bit identical results to the scalar version. @testset "dist_sqr on SIMD lanes $T $N to $M" for T in (Float32, Float64), - (N, M) in ((1, 1), (1, 2), (2, 1), (1, 3), (3, 1), (2, 2)) + (N, M) in ((1, 1), (1, 2), (2, 1), (1, 3), (3, 1), (2, 2), (2, 3), (3, 2), (3, 3)) W = 8 for trial in 1:500 as = ntuple(_ -> rand_simplex(T, N), Val(W)) @@ -107,4 +421,55 @@ end @test all(l -> d2[l] === dist_sqr(as[l], bs[l]), 1:W) end end + +# Coordinates just below `floatmax(T)^(1/4)/4`, the top of the range, where +# `N⋅N` and the triple products are near `floatmax`: the hard points scaled +# up, and `rand_simplex` scaled up, half of them with coordinates in +# `(-L, 0, L)`, where the edges are longest, and repeated vertices, parallel +# edges, and coplanar triangles are common. +@testset "$T near the top of the range" for T in (Float32, Float64) + L = prevfloat(floatmax(T)^T(1/4)/4) + cases = Any[] + for (name, xt, yt) in hard_points + push!(cases, (SVector(map(v -> SVector{3, T}(L .* v), xt)), SVector(map(v -> SVector{3, T}(L .* v), yt)))) + end + for N in 1:3, M in 1:3, _ in 1:300 + push!(cases, (rand_simplex(T, N) .* L, rand_simplex(T, M) .* L)) + end + for (x, y) in cases, (a, b) in ((x, y), (y, x)) + d2, r, s, t = dist_sqr_r_s_t(a, b) + @test isfinite(d2) && all(isfinite, r) + @test all(≥(0), s) && all(≥(0), t) && sum(s) ≤ 1 && sum(t) ≤ 1 + tol = 8*eps(T)*maximum(v -> maximum(abs, v), (a..., b...)) + ref = sqrt(dist_sqr(map(v -> big.(v), a), map(v -> big.(v), b))) + @test abs(sqrt(d2) - ref) ≤ tol + pa = (1 - sum(big.(s))) .* a[1] .+ sum(big.(s) .* a[2:end]; init = zero(SVector{3, BigFloat})) + pb = (1 - sum(big.(t))) .* b[1] .+ sum(big.(t) .* b[2:end]; init = zero(SVector{3, BigFloat})) + @test norm(r .- (pb .- pa)) ≤ tol + end +end + +# A NaN coordinate anywhere gives a NaN `d²`, whatever the candidate it would +# lose to, also in SIMD lanes next to lanes without NaN. The weights stay in +# range. +@testset "$T NaN coordinate $N to $M" for T in (Float32, Float64), N in 1:3, M in 1:3 + for _ in 1:20, i in 1:(N + M), k in 1:3 + x = rand_simplex(T, N) + y = rand_simplex(T, M) + if i ≤ N + x = setindex(x, setindex(x[i], T(NaN), k), i) + else + y = setindex(y, setindex(y[i - N], T(NaN), k), i - N) + end + for (a, b) in ((x, y), (y, x)) + d2, r, s, t = dist_sqr_r_s_t(a, b) + @test isnan(d2) + @test all(w -> 0 ≤ w ≤ 1, s) && all(w -> 0 ≤ w ≤ 1, t) + as = (a, ntuple(_ -> rand_simplex(T, length(a)), Val(7))...) + bs = (b, ntuple(_ -> rand_simplex(T, length(b)), Val(7))...) + d2s = dist_sqr(simplex_lanes(as), simplex_lanes(bs)) + @test isnan(d2s[1]) && all(l -> d2s[l] === dist_sqr(as[l], bs[l]), 2:8) + end + end +end nothing