Skip to content

Commit 2ab70ed

Browse files
AndreaNeumayrAndreaNeumayr
committed
change mpr.jl
1 parent 3929842 commit 2ab70ed

File tree

1 file changed

+39
-36
lines changed
  • src/contactDetection/ContactDetectionMPR

1 file changed

+39
-36
lines changed

‎src/contactDetection/ContactDetectionMPR/mpr.jl‎

Lines changed: 39 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -4,21 +4,21 @@
44

55
# Collision detection algorithm based on the MPR algorithm
66

7-
mutable struct SupportPoint
8-
p::SVector{3,Float64} # support point
9-
n::SVector{3,Float64} # support normal unit vector
10-
a::SVector{3,Float64} # point on shapeA
11-
b::SVector{3,Float64} # point on shapeB
12-
function SupportPoint(p::SVector{3,Float64},n::SVector{3,Float64},a::SVector{3,Float64},b::SVector{3,Float64})
7+
mutable struct SupportPoint{T}
8+
p::SVector{3,T} # support point
9+
n::SVector{3,T} # support normal unit vector
10+
a::SVector{3,T} # point on shapeA
11+
b::SVector{3,T} # point on shapeB
12+
function SupportPoint{T}(p::SVector{3,T},n::SVector{3,T},a::SVector{3,T},b::SVector{3,T}) where {T}
1313
new(p,n,a,b)
1414
end
1515
end
1616

1717

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

2424

@@ -60,20 +60,20 @@ end
6060

6161

6262
########### Phase 1, Minkowski Portal Refinement ###################
63-
@inline getCentroid(obj::Composition.Object3D) = (obj.r_abs + obj.R_abs'*obj.centroid)
63+
@inline getCentroid(obj::Composition.Object3D) = SVector{3,Modia3D.MPRFloatType}(obj.r_abs + obj.R_abs'*obj.centroid)
6464

6565

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

7474

75-
function checkIfShapesArePlanar(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,n2::SVector{3,Float64}, neps::Float64,
76-
shapeA::Composition.Object3D,shapeB::Composition.Object3D)
75+
function checkIfShapesArePlanar(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,n2::SVector{3,T}, neps::T,
76+
shapeA::Composition.Object3D,shapeB::Composition.Object3D) where {T}
7777
# r3 is in the direction of plane normal that contains triangle r0-r1-r2
7878
n3 = cross(r1.p-r0.p, r2.p-r0.p)
7979
# the triangle r0-r1-r2 has degenerated into a line segment
@@ -120,12 +120,12 @@ end
120120
# Der Ursprung muss nicht enthalten sein!!!
121121
function tetrahedronEncloseOrigin(r0::SupportPoint, r1::SupportPoint,
122122
r2::SupportPoint, r3::SupportPoint,
123-
neps::Float64, niter_max::Int64,
124-
shapeA::Composition.Object3D, shapeB::Composition.Object3D, scale::Float64)
123+
neps::T, niter_max::Int64,
124+
shapeA::Composition.Object3D, shapeB::Composition.Object3D, scale::T) where {T}
125125
r1org = r1
126126
r2org = r2
127127
r3org = r3
128-
aux = Modia3D.ZeroVector3D
128+
aux = SVector{3, T}(Modia3D.ZeroVector3D)
129129
success = false
130130
for i in 1:niter_max
131131
aux = cross(r1.p-r0.p,r3.p-r0.p)
@@ -158,7 +158,7 @@ end
158158
########### Phase 3, Minkowski Portal Refinement ###################
159159
# construction of r4
160160
function constructR4(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,r3::SupportPoint,
161-
neps::Float64, shapeA::Composition.Object3D,shapeB::Composition.Object3D, scale::Float64)
161+
neps::T, shapeA::Composition.Object3D,shapeB::Composition.Object3D, scale::T) where {T}
162162
n4 = cross(r2.p-r1.p, r3.p-r1.p)
163163
if norm(n4) <= neps
164164
r3 = getSupportPoint(shapeA, shapeB, -r3.n, scale=scale) # change search direction
@@ -244,11 +244,11 @@ function finalTC3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::Supp
244244
end
245245

246246

247-
function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, neps::Float64, niter_max::Int64,tol_rel::Float64, shapeA::Composition.Object3D,shapeB::Composition.Object3D, scale::Float64)
247+
function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, neps::T, niter_max::Int64, tol_rel::T, shapeA::Composition.Object3D, shapeB::Composition.Object3D, scale::T) where {T}
248248
r1org = r1
249249
r2org = r2
250250
r3org = r3
251-
new_tol = 42.0
251+
new_tol = T(42.0)
252252
isTC2 = false
253253
isTC3 = false
254254
r1_new::SupportPoint = r0
@@ -269,12 +269,12 @@ function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::Suppor
269269
## TERMINATION CONDITION 2 ##
270270
if TC2 < tol_rel
271271
(distance,r1,r2,r3,r4) = finalTC2(r1,r2,r3,r4)
272-
return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
272+
return distance, r1, r2, r3, r4
273273

274274
## TERMINATION CONDITION 3 ##
275275
elseif TC3 < tol_rel
276276
(distance,r1,r2,r3,r4) = finalTC3(r0, r1, r2, r3, r4)
277-
return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
277+
return distance, r1, r2, r3, r4
278278
else
279279
if TC2 < new_tol
280280
new_tol = TC2
@@ -304,11 +304,11 @@ function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::Suppor
304304
@warn("MPR (phase 3): Numerical issues with distance computation between $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)). tol_rel increased locally for this computation to $new_tol.")
305305
if isTC2
306306
(distance,r1,r2,r3,r4) = finalTC2(r1_new,r2_new,r3_new,r4_new)
307-
return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
307+
return distance, r1, r2, r3, r4
308308
end
309309
if isTC3
310310
(distance,r1,r2,r3,r4) = finalTC3(r0, r1_new, r2_new, r3_new, r4_new)
311-
return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
311+
return distance, r1, r2, r3, r4
312312
end
313313
end
314314
end
@@ -319,13 +319,15 @@ function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::Suppor
319319
@warn("MPR (phase 3): Max. number of iterations (= $niter_max) is reached and $niter_max > 100, look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)). tol_rel increased locally for this computation to $new_tol.")
320320
if isTC2
321321
(distance,r1,r2,r3,r4) = finalTC2(r1_new,r2_new,r3_new,r4_new)
322-
return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
322+
return distance, r1, r2, r3, r4
323323
end
324324
if isTC3
325325
(distance,r1,r2,r3,r4) = finalTC3(r0, r1_new, r2_new, r3_new, r4_new)
326-
return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
326+
return distance, r1, r2, r3, r4
327327
end
328328
end
329+
@error("passiert das?!?!?")
330+
return distance, r1, r2, r3, r4 # needed for a unique return type
329331
end
330332

331333
# MPR - Minkowski Portal Refinement algorithm
@@ -352,7 +354,7 @@ end
352354
# Termination Condition 2
353355
# Termination Condition 3
354356
# Phase 3.3: construct baby tetrahedrons with r1,r2,r3,r4 and create a new portal
355-
function mprGeneral(ch::Composition.ContactDetectionMPR_handler, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D)
357+
function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D) where {T}
356358
tol_rel = ch.tol_rel
357359
niter_max = ch.niter_max
358360
neps = ch.neps
@@ -365,7 +367,7 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler, shapeA::Composi
365367
# the direction of the origin ray r0 is -r0.p
366368
centroidA = getCentroid(shapeA)
367369
centroidB = getCentroid(shapeB)
368-
r0 = SupportPoint(centroidA-centroidB, -(centroidA-centroidB), SVector{3,Float64}(0.0,0.0,0.0), SVector{3,Float64}(0.0,0.0,0.0))
370+
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))
369371
# check if centers of shapes are overlapping
370372
checkCentersOfShapesOverlapp(r0, neps, shapeA, shapeB)
371373

@@ -386,7 +388,7 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler, shapeA::Composi
386388
# e.g. any collision/or distance between two spheres
387389
#println("TC 1")
388390
distance = dot(r1.p,normalize(r0.p))
389-
return (distance, r1.a, r1.b, r1.n, false, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D)
391+
return (Float64(distance), SVector{3,Float64}(r1.a), SVector{3,Float64}(r1.b), SVector{3,Float64}(r1.n), false, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D)
390392
else
391393
# normalize n2
392394
n2 = n2/n2abs
@@ -407,15 +409,16 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler, shapeA::Composi
407409

408410

409411
########### Phase 3, Minkowski Portal Refinement ###################
410-
phase3(r0, r1, r2, r3, neps, niter_max, tol_rel, shapeA, shapeB, scale)
412+
(distance, r1, r2, r3, r4) = phase3(r0, r1, r2, r3, neps, niter_max, tol_rel, shapeA, shapeB, scale)
413+
return (Float64(distance), SVector{3,Float64}(r4.a), SVector{3,Float64}(r4.b), SVector{3,Float64}(r4.n), true, SVector{3,Float64}(r1.a), SVector{3,Float64}(r1.b), SVector{3,Float64}(r2.a), SVector{3,Float64}(r2.b), SVector{3,Float64}(r3.a), SVector{3,Float64}(r3.b) )
411414
end
412415

413416

414-
function mprTwoSpheres(ch::Composition.ContactDetectionMPR_handler, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D,
415-
sphereA::Shapes.Sphere, sphereB::Shapes.Sphere)
417+
function mprTwoSpheres(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D,
418+
sphereA::Shapes.Sphere, sphereB::Shapes.Sphere) where {T}
416419
neps = ch.neps
417-
radiusA = sphereA.diameter*0.5
418-
radiusB = sphereB.diameter*0.5
420+
radiusA = T(sphereA.diameter*0.5)
421+
radiusB = T(sphereB.diameter*0.5)
419422
centroidSphereA = getCentroid(shapeA)
420423
centroidSphereB = getCentroid(shapeB)
421424
n = centroidSphereB - centroidSphereA
@@ -428,11 +431,11 @@ function mprTwoSpheres(ch::Composition.ContactDetectionMPR_handler, shapeA::Comp
428431
distance = distanceCentroids - radiusA - radiusB
429432
contactPointShapeA = centroidSphereA + normal*radiusA
430433
contactPointShapeB = centroidSphereB - normal*radiusB
431-
return (distance, contactPointShapeA, contactPointShapeB, normal, false, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D)
434+
return (Float64(distance), SVector{3,Float64}(contactPointShapeA), SVector{3,Float64}(contactPointShapeB), SVector{3,Float64}(normal), false, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D)
432435
end
433436

434437

435-
function mpr(ch::Composition.ContactDetectionMPR_handler, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D)
438+
function mpr(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D) where {T}
436439
shapeKindA = shapeA.shapeKind
437440
shapeKindB = shapeB.shapeKind
438441

@@ -449,7 +452,7 @@ function mpr(ch::Composition.ContactDetectionMPR_handler, shapeA::Composition.Ob
449452
support1A, support1B, support2A, support2B, support3A, support3B) = mprGeneral(ch, shapeA, shapeB)
450453

451454
if shapeKindA == Modia3D.SphereKind
452-
centroidSphere = getCentroid(shapeA)
455+
centroidSphere = SVector{3,Float64}(getCentroid(shapeA))
453456
sphereA1::Shapes.Sphere = shapeA.shape
454457
radius = sphereA1.diameter*0.5
455458
contactPointSphere = centroidSphere + radius*normal
@@ -458,7 +461,7 @@ function mpr(ch::Composition.ContactDetectionMPR_handler, shapeA::Composition.Ob
458461
contactPoint2 = contactPointOtherShape
459462
elseif shapeKindB == Modia3D.SphereKind
460463
normalLocal = -normal
461-
centroidSphere = getCentroid(shapeB)
464+
centroidSphere = SVector{3,Float64}(getCentroid(shapeB))
462465
sphereB1::Shapes.Sphere = shapeB.shape
463466
radius = sphereB1.diameter*0.5
464467
contactPointSphere = centroidSphere + radius*normalLocal

0 commit comments

Comments
 (0)