Added functions to get distances to primatives and meshes

src/GeometryBasics.jl

@@ -23,6 +23,7 @@ include("meshes.jl")
 include("triangulation.jl")
 include("lines.jl")
 include("boundingboxes.jl")
+include("distances.jl")
 
 include("deprecated.jl")
 
@@ -70,6 +71,8 @@ export max_dist_dim, max_euclidean, max_euclideansq, min_dist_dim, min_euclidean
 export min_euclideansq, minmax_dist_dim, minmax_euclidean, minmax_euclideansq
 export self_intersections, split_intersections
 
+export signed_distance, absolute_distance
+
 if Base.VERSION >= v"1.4.2"
     include("precompile.jl")
     _precompile_()

src/distances.jl

@@ -0,0 +1,137 @@
+#=
+Functions to compute distances from points to primatives
+=#
+
+using LinearAlgebra: normalize,norm,⋅
+"""
+    closest_point_to_tri(p, a, b, c)
+
+This method from Ericson, "Real-time collision detection"
+2005, doesn't require the normal or cross products.
+"""
+@fastmath function closest_point_to_tri(p,a,b,c)
+    # is point `a` closest?
+    ab,ac,ap = b-a,c-a,p-a
+    d1,d2 = ab ⋅ ap, ac ⋅ ap
+    d1 <= 0 && d2 <= 0 && return a
+
+    # is point `b` closest?
+    bp = p-b
+    d3,d4 = ab ⋅ bp, ac ⋅ bp
+    d3 >= 0 && d4 <= d3 && return b
+
+    # is point `c` closest?
+    cp = p-c
+    d5,d6 = ab ⋅ cp, ac ⋅ cp
+    d6 >= 0 && d5 <= d6 && return c
+
+    # is segment `ab` closest?
+    vc = d1*d4 - d3*d2
+    if (vc <= 0 && d1 >= 0 && d3 <= 0)
+        d1==d3 && @show d1,d3
+        return a + ab * d1 * inv(d1 - d3)
+    end
+
+    # is segment `ac` closest?
+    vb = d5*d2 - d1*d6
+    if (vb <= 0 && d2 >= 0 && d6 <= 0)
+        return a + ac * d2 * inv(d2 - d6)
+    end
+
+    # is segment `bc` closest?
+    va = d3*d6 - d5*d4
+    if (va <= 0 && d4 >= d3 && d5 >= d6)
+        return b + (c - b) * (d4 - d3) * inv(d4 - d3 + d5 - d6)
+    end
+
+    # closest is interior to `abc`
+    denom = inv(va + vb + vc)
+    v,w = vb * denom, vc * denom
+    return a + ab * v + ac * w
+end
+"""
+    closest(p,tri::Triangle)
+
+Determine the closest point on triangle `tri` to point `p`.
+"""
+closest(p,tri::Triangle) = closest_point_to_tri(p,tri.points.data...)
+
+"""
+    absolute_distance(p,tri::Triangle)
+
+Determine the absolute distance from point `p` to the closest point on triangle `tri`.
+"""
+absolute_distance(p,tri::Triangle) = norm(p-closest(p,tri))
+
+"""
+    signed_distance(p,tri::Triangle)
+
+Determine the signed distance from point `p` to the plane defined by triangle `tri`.
+Note that the sign depends on triangle point ordering and `d=0` only implies `p`
+lies on the plane, not that it is within the triangle.
+"""
+signed_distance(p,tri::Triangle) = signed_distance_to_tri(p,tri.points.data...)
+signed_distance_to_tri(p,a,b,c) = normalize(orthogonal_vector(a,b,c))⋅(p-a)
+
+nonzero(mesh::AbstractMesh) = (i for i in mesh if sum(abs2,orthogonal_vector(i.points.data...))>0)
+"""
+    absolute_distance(p, mesh::AbstractMesh)
+
+Return the minimum absolute distance from point `p` to any Triangle in `mesh`.
+"""
+absolute_distance(p,mesh::AbstractMesh) = minimum(t->absolute_distance(p,t), nonzero(mesh))
+
+"""
+    signed_distance(p, mesh::AbstractMesh)
+
+Return the signed distance to the geometry defined by the union of planes
+passing through each `mesh` face.
+"""
+signed_distance(p,mesh::AbstractMesh) = maximum(t->signed_distance(p,t), nonzero(mesh))
+
+"""
+    signed_distance(p,s::HyperSphere)
+
+Return the signed distance from `p` to `s`.
+"""
+signed_distance(p,s::HyperSphere) = norm(p-origin(s))-radius(s)
+
+distance_to_corner(q) = norm(max.(q, 0.))+min(maximum(q),0.)
+"""
+    signed_distance(p,r::Rect)
+
+Return the signed distance from `p` to `r`.
+"""
+function signed_distance(p,r::Rect)
+    # Get vector relative to Rect corner
+    q = abs.(p-origin(r).-0.5*width(r)).-0.5*width(r)
+    return distance_to_corner(q)
+end
+
+"""
+    signed_distance(p,c::Cylinder)
+
+Return the signed distance from `p` to `c`.
+"""
+function signed_distance(p,c::Cylinder)
+    e = (extremity(c)-origin(c))/2    # center to edge (defines length direction)
+    r = p-(extremity(c)+origin(c))/2  # center to point
+    e₁ = norm(e)                      # cylinder half-length
+    r₁ = abs(e ⋅ r) / e₁              # projected length to point
+    r₂ = √(r ⋅ r - r₁^2)              # height to point
+    return distance_to_corner(Point(r₁-e₁,r₂-radius(c)))
+end
+
+"""
+    signed_distance(p,prim)
+
+Fallback signed distance function.
+"""
+signed_distance(p,prim::GeometryPrimitive) = signed_distance(p,GeometryBasics.mesh(prim))
+
+"""
+    absolute_distance(p,prim) = |signed_distance(p,prim)|
+
+Fallback absolute distance function.
+"""
+absolute_distance(p,prim) = abs(signed_distance(p,prim))

test/runtests.jl

@@ -707,6 +707,47 @@ end
     )
 end
 
+@testset "Distance functions" begin
+
+    # any non-co-linear a,b,c should work
+    a,b,c = Point3f(1,0,0),Point3f(0,1,0),Point3f(0,0,1)
+    n = GeometryBasics.orthogonal_vector(a,b,c)
+    tri = Triangle(a,b,c)
+    for p ∈ (a,b,c,(a+b)/2,(a+c)/2,(c+b)/2,(a+b+c)/3)
+        s = p-(a+b+c)/3 # tangent vector from center to p
+        q = p-n+s
+        @test GeometryBasics.closest(q,tri) ≈ p
+        @test absolute_distance(q,tri) ≈ norm(n+s) # ignores sign
+        @test signed_distance(q,tri) ≈ -norm(n) # ignores s offset
+    end
+
+    # HyperRectangle test
+    r = Rect(Vec3(1.),Vec3(2.))
+    p = Point3f(0)
+    @test signed_distance(p,r) ≈ √3
+    m = GeometryBasics.mesh(r) # aligns perfectly with r
+    @test absolute_distance(p,m) ≈ √3
+    @test signed_distance(p,m) ≈ 1  # ∞-norm
+
+    # HyperSphere test
+    s = Sphere(Point3f(1),2)
+    @test signed_distance(p,s) ≈ √3-2
+    @test absolute_distance(p,s) ≈ 2-√3
+    m = GeometryBasics.mesh(s) # only approximately aligns with s
+    @test isapprox(signed_distance(p,m),√3-2,rtol=0.05)
+    @test isapprox(absolute_distance(p,m),2-√3,rtol=0.05)
+
+    # Cylinder test
+    s = Cylinder(Point3(0.),Point3(1.),2.)
+    p = Point(0.,2.,-2.)
+    @test signed_distance(p,s) ≈ √8-2
+
+    # Pyramid test
+    s = Pyramid(Point3(0.),1.,1.)
+    p = Point3f(0,0,-1)
+    @test signed_distance(p,s) ≈ 1
+end
+
 @testset "Tests from GeometryTypes" begin
     include("geometrytypes.jl")
 end