From d1c62e9920f8838839cbdded04a0881961a3d9fb Mon Sep 17 00:00:00 2001 From: Arun Lakshmanan Date: Thu, 4 Jul 2019 01:39:00 -0500 Subject: [PATCH] Make compatible with Unitful --- Project.toml | 2 +- src/proximity.jl | 23 ++++++++++++----------- src/simplex.jl | 35 ++++++++++++++++++++--------------- 3 files changed, 33 insertions(+), 27 deletions(-) diff --git a/Project.toml b/Project.toml index 3dfd088..9d0f343 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ConvexBodyProximityQueries" uuid = "e9983d58-8b29-5530-8046-db49618142f9" authors = ["Arun Lakshmanan "] -version = "0.1.3" +version = "0.1.4" [deps] LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" diff --git a/src/proximity.jl b/src/proximity.jl index 455113e..1aac4d1 100644 --- a/src/proximity.jl +++ b/src/proximity.jl @@ -18,7 +18,7 @@ julia> closest_points(p, q, dir) ([1.0, 1.0], [2.0, 1.0]) ``` """ -function closest_points(p::Any, q::Any, init_dir::SVector; max_iter=100, atol=0.0) +function closest_points(p::Any, q::Any, init_dir::SVector{D, T}; max_iter=100, atol::T=sqrt(eps(T))*oneunit(T)) where {D, T} collision, dir, psimplex, qsimplex, sz = gjk(p, q, init_dir, max_iter, atol, minimum_distance_cond) if !collision @@ -44,7 +44,7 @@ julia> minimum_distance(p, q, dir) 1.0 ``` """ -function minimum_distance(p::Any, q::Any, init_dir::SVector; max_iter=100, atol=0.0) +function minimum_distance(p::Any, q::Any, init_dir::SVector{D, T}; max_iter=100, atol::T=sqrt(eps(T))*oneunit(T)) where {D, T} collision, dir, psimplex, qsimplex, sz = gjk(p, q, init_dir, max_iter, atol, minimum_distance_cond) return collision ? 0.0 : norm(dir) end @@ -66,7 +66,7 @@ julia> tolerance_verification(p, q, dir, 0.25) true ``` """ -function tolerance_verification(p::Any, q::Any, init_dir::SVector, τ::Real; max_iter=100, atol=0.0) +function tolerance_verification(p::Any, q::Any, init_dir::SVector{D, T}, τ::T; max_iter=100, atol::T=sqrt(eps(T))*oneunit(T)) where {D, T} collision, = gjk(p, q, init_dir, max_iter, atol, tolerance_verification_cond, τ) return !collision end @@ -88,13 +88,13 @@ julia> collision_detection(p, q, dir) false ``` """ -function collision_detection(p::Any, q::Any, init_dir::SVector; max_iter=100) - collision, = gjk(p, q, init_dir, max_iter, Inf, collision_detection_cond) +function collision_detection(p::Any, q::Any, init_dir::SVector{D, T}; max_iter=100) where {D, T} + collision, = gjk(p, q, init_dir, max_iter, Inf*oneunit(T), collision_detection_cond) return collision end # See Julia issue #28720 for why we have `where {F<:Function}` signature -function gjk(p::Any, q::Any, init_dir::SVector{N, T}, max_iter::Int, atol::Real, query::F, params...) where {F<:Function, N, T} +function gjk(p::Any, q::Any, init_dir::SVector{N, T}, max_iter::Int, atol::T, query::F, params...) where {F<:Function, N, T} sz = 1 collision = false distance_condition = false @@ -103,8 +103,9 @@ function gjk(p::Any, q::Any, init_dir::SVector{N, T}, max_iter::Int, atol::Real, psimplex = insertcolumn(ps) qsimplex = insertcolumn(qs) dir = qs - ps + ntol2 = eps(T)*oneunit(T)^2 # numerical tolerance - if sum(abs2, dir) ≤ 2*eps(Float64) + if sum(abs2, dir) ≤ ntol2 return true, dir, psimplex, qsimplex, sz end @@ -120,7 +121,7 @@ function gjk(p::Any, q::Any, init_dir::SVector{N, T}, max_iter::Int, atol::Real, qs = support(q, -dir) s = ps - qs collision = query(dir, collision, params...) - distance_condition = s⋅(-dir) ≥ eps(T) && abs(sum(abs2, dir) - s⋅(-dir)) ≤ atol^2 + distance_condition = (s⋅(-dir)) ≥ ntol2 && (sum(abs2, dir) - s⋅(-dir)) ≤ atol^2 max_iter -= 1 if max_iter ≤ 0 @@ -139,14 +140,14 @@ function collision_detection_cond(dir::SVector, collision::Bool) collision end -function tolerance_verification_cond(dir::SVector, collision::Bool, tv::Real) - tv^2 > sum(abs2, dir) ? true : collision +function tolerance_verification_cond(dir::SVector, collision::Bool, tv) + tv > norm(dir) ? true : collision end @generated function check_degeneracy(simplex::SMatrix{N, M, T}, s::SVector{N, T}, sz::Vararg{Int, 1}) where {N, M, T} exprs = :(false) for i = 1:M - exprs = :($i > sz[1] ? $exprs : ($exprs || sum(abs2, simplex[:, $i] - s) ≤ 2*eps($T))) + exprs = :($i > sz[1] ? $exprs : ($exprs || norm(simplex[:, $i] - s) ≤ eps($T)*oneunit($T))) end return exprs end diff --git a/src/simplex.jl b/src/simplex.jl index e3c3605..30a64e6 100644 --- a/src/simplex.jl +++ b/src/simplex.jl @@ -88,15 +88,17 @@ function findline(psimplex::SMatrix{N}, qsimplex::SMatrix{N}) where {N} simplex = psimplex - qsimplex AB = simplex[:, 1] - simplex[:, 2] AO = -simplex[:, 2] - if AB ⋅ AO > 0 + T = eltype(psimplex) + ntol = eps(T)*oneunit(T) + if AB ⋅ AO > zero(T)^2 dir = AO - proj(AB, AO) - collision = sum(abs2, dir) ≤ 2*eps(Float64) + collision = norm(dir) ≤ ntol return psimplex, qsimplex, dir, collision, 2 else dir = AO psimplex = pickcolumns(psimplex, 2) qsimplex = pickcolumns(qsimplex, 2) - collision = sum(abs2, dir) ≤ 2*eps(Float64) + collision = norm(dir) ≤ ntol return psimplex, qsimplex, dir, collision, 1 end end @@ -107,26 +109,28 @@ function findtriangle(psimplex::SMatrix{N}, qsimplex::SMatrix{N}) where {N} AC = simplex[:, 1] - simplex[:, 3] BC = simplex[:, 1] - simplex[:, 2] AO = -simplex[:, 3] - if (AC ⋅ AB * BC - AC ⋅ BC * AB) ⋅ AO > 0 - if AC ⋅ AO > 0 + T = eltype(psimplex) + ntol = eps(T)*oneunit(T) + if (AC ⋅ AB * BC - AC ⋅ BC * AB) ⋅ AO > zero(T)^4 + if AC ⋅ AO > zero(T)^2 psimplex = pickcolumns(psimplex, 1, 3) qsimplex = pickcolumns(qsimplex, 1, 3) dir = AO - proj(AC, AO) - collision = sum(abs2, dir) ≤ 2*eps(Float64) + collision = norm(dir) ≤ ntol return psimplex, qsimplex, dir, collision, 2 else psimplex = pickcolumns(psimplex, 2, 3) qsimplex = pickcolumns(qsimplex, 2, 3) return findline(psimplex, qsimplex) end - elseif (AB ⋅ BC * AB - AB ⋅ AB * BC) ⋅ AO > 0 + elseif (AB ⋅ BC * AB - AB ⋅ AB * BC) ⋅ AO > zero(T)^3 psimplex = pickcolumns(psimplex, 2, 3) qsimplex = pickcolumns(qsimplex, 2, 3) return findline(psimplex, qsimplex) else - if sum(abs2, AO) ≤ 2*eps(Float64) + if norm(AO) ≤ ntol return psimplex, qsimplex, AO, true, 3 - elseif sum(abs2, AC - proj(AB, AC)) ≤ 2*eps(Float64) + elseif norm(AC - proj(AB, AC)) ≤ ntol psimplex = pickcolumns(psimplex, 2, 3) qsimplex = pickcolumns(qsimplex, 2, 3) return findline(psimplex, qsimplex) @@ -135,13 +139,13 @@ function findtriangle(psimplex::SMatrix{N}, qsimplex::SMatrix{N}) where {N} else ABC = AB × BC dir = proj(ABC, AO) - if ABC ⋅ AO > 0 + if ABC ⋅ AO > zero(T)^3 psimplex = pickcolumns(psimplex, 2, 1, 3) qsimplex = pickcolumns(qsimplex, 2, 1, 3) - collision = sum(abs2, dir) ≤ 2*eps(Float64) + collision = norm(dir) ≤ ntol return psimplex, qsimplex, dir, collision, 3 else - collision = sum(abs2, dir) ≤ 2*eps(Float64) + collision = norm(dir) ≤ ntol return psimplex, qsimplex, dir, collision, 3 end end @@ -213,9 +217,10 @@ function trianglecombination(simplex::SMatrix{N}, vec::SVector{N}) where {N} AB = simplex[:, 2] - simplex[:, 3] AC = simplex[:, 1] - simplex[:, 3] BC = simplex[:, 1] - simplex[:, 2] + T = eltype(simplex) - if (AC ⋅ AB * BC - AC ⋅ BC * AB) ⋅ AV > 0 - if AC ⋅ AV > 0 + if (AC ⋅ AB * BC - AC ⋅ BC * AB) ⋅ AV > zero(T)^3 + if AC ⋅ AV > zero(T)^2 dir = AO - proj(AC, AV) idx = SMatrix{3, 2}(1, 0, 0, 0, 0, 1) simplex = pickcolumns(simplex, 1, 3) @@ -227,7 +232,7 @@ function trianglecombination(simplex::SMatrix{N}, vec::SVector{N}) where {N} λAB1, λAB2 = linecombination(simplex, -dir) return 0.0, λAB1, λAB2 end - elseif (AB ⋅ BC * AB - AB ⋅ AB * BC) ⋅ AV > 0 + elseif (AB ⋅ BC * AB - AB ⋅ AB * BC) ⋅ AV > zero(T)^3 dir = AO - proj(AB, AV) simplex = pickcolumns(simplex, 2, 3) λAB1, λAB2 = linecombination(simplex, -dir)