Skip to content

Commit

Permalink
Make compatible with Unitful
Browse files Browse the repository at this point in the history
  • Loading branch information
@arlk
arlk committed Jul 4, 2019
1 parent 0f57bbe commit d1c62e9
Show file tree
Hide file tree
Showing 3 changed files with 33 additions and 27 deletions.
2 changes: 1 addition & 1 deletion Project.toml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
name = "ConvexBodyProximityQueries"
uuid = "e9983d58-8b29-5530-8046-db49618142f9"
authors = ["Arun Lakshmanan <lakshma2@illinois.edu>"]
version = "0.1.3"
version = "0.1.4"

[deps]
LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
Expand Down
23 changes: 12 additions & 11 deletions src/proximity.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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

Expand All @@ -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
Expand All @@ -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
35 changes: 20 additions & 15 deletions src/simplex.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand Down

0 comments on commit d1c62e9

Please sign in to comment.