Skip to content

Commit 3f7a7cb

Browse files
AndreaNeumayrAndreaNeumayr
committed
clean up mpr
1 parent 807b454 commit 3f7a7cb

File tree

1 file changed

+17
-28
lines changed
  • src/contactDetection/ContactDetectionMPR

1 file changed

+17
-28
lines changed

‎src/contactDetection/ContactDetectionMPR/mpr.jl‎

Lines changed: 17 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ struct SupportPoint{T}
1515
end
1616

1717

18-
function getSupportPoint(shapeA::Modia3D.Composition.Object3D, shapeB::Composition.Object3D, n::SVector{3,T}; scale::T=T(1.0)) where {T}
18+
function getSupportPoint(shapeA::Modia3D.Composition.Object3D{F}, shapeB::Composition.Object3D{F}, n::SVector{3,T}; scale::T=T(1.0)) where {T,F}
1919
a = Modia3D.supportPoint(shapeA, n)
2020
b = Modia3D.supportPoint(shapeB, -n)
2121
return SupportPoint{T}((a-b).*scale,n,a,b)
@@ -66,22 +66,21 @@ end
6666

6767
# checks if centers of shapeA and shapeB are overlapping
6868
# belongs to construction of r0
69-
function checkCentersOfShapesOverlapp(r0::SupportPoint{T}, shapeA::Composition.Object3D, shapeB::Composition.Object3D) where {T}
69+
function checkCentersOfShapesOverlapp(r0::SupportPoint{T}, shapeA::Composition.Object3D{F}, shapeB::Composition.Object3D{F}) where {T,F}
7070
if norm(r0.p) <= Modia3D.nepsType(T)
7171
error("MPR: Too large penetration (prerequisite of MPR violated). Centers are overlapping. Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
7272
end
7373
end
7474

7575

76-
function checkIfShapesArePlanar(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,n2::SVector{3,T},
77-
shapeA::Composition.Object3D,shapeB::Composition.Object3D) where {T}
76+
function checkIfShapesArePlanar(r0::SupportPoint{T},r1::SupportPoint{T},r2::SupportPoint{T},n2::SVector{3,T},
77+
shapeA::Composition.Object3D{F},shapeB::Composition.Object3D{F}) where {T,F}
7878
r3 = SupportPoint{T}
7979

8080
# r3 is in the direction of plane normal that contains triangle r0-r1-r2
8181
n3 = cross(r1.p-r0.p, r2.p-r0.p)
8282
neps = Modia3D.nepsType(T)
8383
# the triangle r0-r1-r2 has degenerated into a line segment
84-
# println("norm(n3) = ", norm(n3))
8584
if norm(n3) <= neps
8685
# change search direction for r2
8786
# because we are still interested in distances if shapes are not intersecting
@@ -103,7 +102,6 @@ function checkIfShapesArePlanar(r0::SupportPoint,r1::SupportPoint,r2::SupportPoi
103102
# check if portal triangle r1-r2-r3 has degenerated into a line segment <--> points r1,r2,r3 are on the same line
104103
r3 = getSupportPoint(shapeA, shapeB, Basics.normalizeVector(n3))
105104
n3b = cross(r2.p-r1.p, r3.p-r1.p)
106-
# println("norm(n3b) = ", norm(n3b))
107105
if norm(n3b) <= neps
108106
# change search direction for r3
109107
r3 = getSupportPoint(shapeA, shapeB, -r3.n)
@@ -123,10 +121,10 @@ end
123121
# loop around to "ensure" the tetrahedron r0,r1,r2 and r3 encloses the origin
124122
# stimmt so nicht wirklich, muss ich nochmal nachlesen!!!
125123
# Der Ursprung muss nicht enthalten sein!!!
126-
function tetrahedronEncloseOrigin(r0::SupportPoint, r1::SupportPoint,
127-
r2::SupportPoint, r3::SupportPoint,
124+
function tetrahedronEncloseOrigin(r0::SupportPoint{T}, r1::SupportPoint{T},
125+
r2::SupportPoint{T}, r3::SupportPoint{T},
128126
niter_max::Int64,
129-
shapeA::Composition.Object3D, shapeB::Composition.Object3D, scale::T) where {T}
127+
shapeA::Composition.Object3D{F}, shapeB::Composition.Object3D{F}, scale::T) where {T,F}
130128
r1org = r1
131129
r2org = r2
132130
r3org = r3
@@ -158,12 +156,11 @@ end
158156

159157
########### Phase 3, Minkowski Portal Refinement ###################
160158
# construction of r4
161-
function constructR4(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,r3::SupportPoint,
162-
shapeA::Composition.Object3D,shapeB::Composition.Object3D, scale::T) where {T}
159+
function constructR4(r0::SupportPoint{T},r1::SupportPoint{T},r2::SupportPoint{T},r3::SupportPoint{T},
160+
shapeA::Composition.Object3D{F},shapeB::Composition.Object3D{F}, scale::T) where {T,F}
163161
r4 = SupportPoint{T}
164162
n4 = cross(r2.p-r1.p, r3.p-r1.p)
165163
neps = Modia3D.nepsType(T)
166-
# println("norm(n4) = ", norm(n4))
167164
if norm(n4) <= neps
168165
r3 = getSupportPoint(shapeA, shapeB, -r3.n, scale=scale) # change search direction
169166
n4 = cross(r2.p-r1.p, r3.p-r1.p)
@@ -193,7 +190,7 @@ isNextPortal(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r4::SupportPo
193190
# r1r2r4
194191
# r2r3r4
195192
# r3r1r4
196-
function createBabyTetrahedrons(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, r4::SupportPoint)
193+
function createBabyTetrahedrons(r0::SupportPoint{T}, r1::SupportPoint{T}, r2::SupportPoint{T}, r3::SupportPoint{T}, r4::SupportPoint{T}) where {T}
197194
nextPortal = true
198195
if isNextPortal(r0,r1,r2,r4)
199196
r3 = r4
@@ -221,7 +218,7 @@ function skalarization(r0::SupportPoint{T}, r1::SupportPoint{T}, r2::SupportPoin
221218
return (r0, r1, r2, r3, scale)
222219
end
223220

224-
function finalTC2(r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, r4::SupportPoint)
221+
function finalTC2(r1::SupportPoint{T}, r2::SupportPoint{T}, r3::SupportPoint{T}, r4::SupportPoint{T}) where {T}
225222
#println("TC 2")
226223
#if !analyzeFinalPortal(r1.p, r2.p, r3.p, r4.p)
227224
# error("shapeA = ", shapeA, " shapeB = ", shapeB)
@@ -236,22 +233,18 @@ function finalTC3(r0::SupportPoint{T}, r1::SupportPoint{T}, r2::SupportPoint{T},
236233
#println("TC 3")
237234

238235
#doesRayIntersectPortal(r1.p,r2.p,r3.p, r4.p)
239-
#println("r1.p = ", r1.p , " r2.p = ", r2.p ," r3.p = ", r3.p)
240-
#println("r4.p = ", r4.p)
241-
#println(" ")
242236
#if !analyzeFinalPortal(r1.p, r2.p, r3.p, r4.p)
243237
# error("shapeA = ", shapeA, " shapeB = ", shapeB)
244238
#end
245239

246240
(r4p, distance) = signedDistanceToPortal(r0.p, r1.p, r2.p, r3.p)
247241
r4 = SupportPoint{T}(r4p, r4.n, r4.a, r4.b)
248242
r4 = barycentric(r1, r2, r3, r4)
249-
#println("distance ", distance)
250243
return distance, r1, r2, r3, r4
251244
end
252245

253246

254-
function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, niter_max::Int64, tol_rel::T, shapeA::Composition.Object3D, shapeB::Composition.Object3D, scale::T) where {T}
247+
function phase3(r0::SupportPoint{T}, r1::SupportPoint{T}, r2::SupportPoint{T}, r3::SupportPoint{T}, niter_max::Int64, tol_rel::T, shapeA::Composition.Object3D{F}, shapeB::Composition.Object3D{F}, scale::T) where {T,F}
255248
r1org = r1
256249
r2org = r2
257250
r3org = r3
@@ -355,7 +348,7 @@ end
355348
# Termination Condition 2
356349
# Termination Condition 3
357350
# Phase 3.3: construct baby tetrahedrons with r1,r2,r3,r4 and create a new portal
358-
function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T,F}, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D) where {T,F}
351+
function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T,F}, shapeA::Composition.Object3D{F}, shapeB::Modia3D.Composition.Object3D{F}) where {T,F}
359352
tol_rel = ch.tol_rel
360353
niter_max = ch.niter_max
361354
neps = Modia3D.nepsType(T)
@@ -371,12 +364,10 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T,F}, shapeA::Co
371364
# the direction of the origin ray r0 is -r0.p
372365
centroidA = getCentroid(shapeA)
373366
centroidB = getCentroid(shapeB)
374-
if isnan(centroidA[1]) || isnan(centroidA[2]) || isnan(centroidA[3]) || isnan(centroidB[1]) || isnan(centroidB[2]) || isnan(centroidB[3])
375-
println("shapeA.r_abs ", shapeA.r_abs)
376-
println("shapeA.R_abs ", shapeA.R_abs)
377-
println("shapeB.r_abs ", shapeB.r_abs)
378-
println("shapeB.R_abs ", shapeB.R_abs)
379-
error("MPR: One of the absolute position or translation is NaN.")
367+
if isnan(centroidA[1]) || isnan(centroidB[1]) ||
368+
isnan(centroidA[2]) || isnan(centroidB[2]) ||
369+
isnan(centroidA[3]) || isnan(centroidB[3])
370+
error("MPR: One of the absolute position or translation is NaN. Look at $(Modia3D.fullName(shapeA)): r_abs = $(shapeA.r_abs), R_abs = $(shapeA.R_abs) and $(Modia3D.fullName(shapeB)): r_abs = $(shapeB.r_abs), R_abs = $(shapeB.R_abs).")
380371
end
381372
r0 = SupportPoint{T}(centroidA-centroidB, -(centroidA-centroidB), SVector{3,T}(0.0,0.0,0.0), SVector{3,T}(0.0,0.0,0.0))
382373
# check if centers of shapes are overlapping
@@ -391,7 +382,6 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T,F}, shapeA::Co
391382
### Phase 1.3: construction of initial r2 ###
392383
n2 = cross(r0.n, r1.p)
393384
n2abs = norm(n2)
394-
# println("norm(n2) = ", norm(n2))
395385
## TERMINATION CONDITION 1 ##
396386
if n2abs <= neps
397387
# r0 || r1, r0 ist parallel zu r1
@@ -400,7 +390,6 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T,F}, shapeA::Co
400390
# e.g. any collision/or distance between two spheres
401391
#println("TC 1")
402392
distance = dot(r1.p,normalize(r0.p))
403-
#println("distance ", distance)
404393
return (distance, r1.a, r1.b, r1.n, false, Modia3D.ZeroVector3D(T), Modia3D.ZeroVector3D(T), Modia3D.ZeroVector3D(T), Modia3D.ZeroVector3D(T), Modia3D.ZeroVector3D(T), Modia3D.ZeroVector3D(T) )
405394
else
406395
# normalize n2

0 commit comments

Comments
 (0)