diff --git a/src/GeometryBasics.jl b/src/GeometryBasics.jl index fb60f3a8..3244543e 100644 --- a/src/GeometryBasics.jl +++ b/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_() diff --git a/src/distances.jl b/src/distances.jl new file mode 100644 index 00000000..b8018c28 --- /dev/null +++ b/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)) diff --git a/test/runtests.jl b/test/runtests.jl index fb576457..74f448e1 100644 --- a/test/runtests.jl +++ b/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