You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
@@ -364,6 +365,8 @@ Defines global properties of the system, such as the gravity field. Exactly one
364
365
- `mprTolerance::1.0e-20`: Local tolerance used for terminating the mpr algorithm (that computes the distances between shapes). Changing this value might improve speed.
365
366
For integrators with step-size control, a value is needed that is much smaller as the relative tolerance used for the integration.
366
367
368
+
- `mprIterMax::120`: Local maximum amount of iterations used for mpr algorithm. If more iterations are needed a message is printed.
369
+
367
370
- `visualizeFrames::Bool`: = true, to visualize the coordinate system of every [Object3D](@ref) that is not explicitly switched off.
368
371
369
372
- `visualizeBoundingBox::Bool`: Flag enabled for visualizing Axis Aligned Bounding Box (AABB) for all solid shapes allowed to collide
Copy file name to clipboardExpand all lines: src/contactDetection/ContactDetectionMPR/mpr.jl
+47-61Lines changed: 47 additions & 61 deletions
Original file line number
Diff line number
Diff line change
@@ -15,7 +15,7 @@ struct SupportPoint{T}
15
15
end
16
16
17
17
18
-
functiongetSupportPoint(shapeA::Modia3D.Composition.Object3D, shapeB::Composition.Object3D, n::SVector{3,T}; scale::T=T(1.0)) where {T}
18
+
functiongetSupportPoint(shapeA::Modia3D.Composition.Object3D{F}, shapeB::Composition.Object3D{F}, n::SVector{3,T}; scale::T=T(1.0)) where {T,F}
19
19
a = Modia3D.supportPoint(shapeA, n)
20
20
b = Modia3D.supportPoint(shapeB, -n)
21
21
returnSupportPoint{T}((a-b).*scale,n,a,b)
@@ -66,15 +66,15 @@ end
66
66
67
67
# checks if centers of shapeA and shapeB are overlapping
68
68
# belongs to construction of r0
69
-
functioncheckCentersOfShapesOverlapp(r0::SupportPoint{T}, shapeA::Composition.Object3D, shapeB::Composition.Object3D) where {T}
69
+
functioncheckCentersOfShapesOverlapp(r0::SupportPoint{T}, shapeA::Composition.Object3D{F}, shapeB::Composition.Object3D{F}) where {T,F}
70
70
ifnorm(r0.p) <= Modia3D.nepsType(T)
71
71
error("MPR: Too large penetration (prerequisite of MPR violated). Centers are overlapping. Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
shapeA::Composition.Object3D{F},shapeB::Composition.Object3D{F}) where {T,F}
78
78
r3 = SupportPoint{T}
79
79
80
80
# r3 is in the direction of plane normal that contains triangle r0-r1-r2
@@ -86,14 +86,13 @@ function checkIfShapesArePlanar(r0::SupportPoint,r1::SupportPoint,r2::SupportPoi
86
86
# because we are still interested in distances if shapes are not intersecting
87
87
n2 =-n2
88
88
r2 =getSupportPoint(shapeA, shapeB, n2)
89
-
ifabs(dot((r2.p-r1.p),n2)) <= neps
89
+
n3 =cross(r1.p-r0.p, r2.p-r0.p) # new normal to the triangle plane (r0-r1-r2_new)
90
+
ifnorm(n3) <= neps
90
91
# Shape is purely planar. Computing the shortest distance for a planar shape
91
92
# requires an MPR 2D algorithm (using lines instead of triangles as portals).
92
93
# However, this is not implemented and therefore the shortest distance cannot be computed
93
-
error("MPR: Shapes are planar and MPR2D is not supported. abs(dot((r2.p-r1.p),n2)). Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
94
+
error("MPR: Shapes are planar and MPR2D is not supported. norm(cross(r1.p-r0.p, r2.p-r0.p)). Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
94
95
end
95
-
# new normal to the triangle plane (r0-r1-r2_new)
96
-
n3 =cross(r1.p-r0.p, r2.p-r0.p) # |n3| > 0 guaranteed, due to construction
97
96
end
98
97
99
98
ifdot(n3,r0.p) >=0.0
@@ -106,11 +105,11 @@ function checkIfShapesArePlanar(r0::SupportPoint,r1::SupportPoint,r2::SupportPoi
106
105
ifnorm(n3b) <= neps
107
106
# change search direction for r3
108
107
r3 =getSupportPoint(shapeA, shapeB, -r3.n)
109
-
ifabs(dot((r3.p-r1.p),r3.n)) <= neps
108
+
ifnorm(cross(r2.p-r1.p, r3.p-r1.p)) <= neps
110
109
# Shape is purely planar. Computing the shortest distance for a planar shape
111
110
# requires an MPR 2D algorithm (using lines instead of triangles as portals).
112
111
# However, this is not implemented and therefore the shortest distance cannot be computed
113
-
error("MPR: Shapes are planar and MPR2D is not supported. r1, r2, r3 are on the same ray. abs(dot((r3.p-r1.p),r3.n)) <= neps. Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
112
+
error("MPR: Shapes are planar and MPR2D is not supported. r1, r2, r3 are on the same ray. norm(cross(r2.p-r1.p, r3.p-r1.p)) <= neps. Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
114
113
end
115
114
end
116
115
@@ -122,10 +121,10 @@ end
122
121
# loop around to "ensure" the tetrahedron r0,r1,r2 and r3 encloses the origin
123
122
# stimmt so nicht wirklich, muss ich nochmal nachlesen!!!
@@ -149,33 +148,28 @@ function tetrahedronEncloseOrigin(r0::SupportPoint, r1::SupportPoint,
149
148
break
150
149
end
151
150
if success !=true
152
-
if niter_max <100
153
-
@warn("MPR (phase 2): Max. number of iterations (= $niter_max) is reached. niter_max increased locally by 10 and phase 2 is rerun. Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
error("MPR (phase 2): Max. number of iterations (= $niter_max) is reached and $niter_max > 100, look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
157
-
end
151
+
error("MPR (phase 2): Max. number of iterations (mprIterMax = $niter_max) is reached. Please, increase mprIterMax. Look at shapes: $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
shapeA::Composition.Object3D{F},shapeB::Composition.Object3D{F}, scale::T) where {T,F}
167
161
r4 = SupportPoint{T}
168
162
n4 =cross(r2.p-r1.p, r3.p-r1.p)
169
163
neps = Modia3D.nepsType(T)
170
164
ifnorm(n4) <= neps
171
165
r3 =getSupportPoint(shapeA, shapeB, -r3.n, scale=scale) # change search direction
172
-
ifabs(dot((r3.p-r1.p),r3.n)) <= neps
166
+
n4 =cross(r2.p-r1.p, r3.p-r1.p)
167
+
ifnorm(n4) <= neps
173
168
# Shape is purely planar. Computing the shortest distance for a planar shape
174
169
# requires an MPR 2D algorithm (using lines instead of triangles as portals).
175
170
# However, this is not implemented and therefore the shortest distance cannot be computed
176
-
error("MPR: Shapes are planar and MPR2D is not supported. abs(dot((r3.p-r1.p),r3.n)). Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
171
+
error("MPR: Shapes are planar and MPR2D is not supported. norm(n4). Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
177
172
end
178
-
n4 =cross(r2.p-r1.p, r3.p-r1.p) # |n4| > 0 guaranteed, due to construction
@@ -311,32 +313,11 @@ function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::Suppor
311
313
312
314
if!nextPortal # createBabyTetrahedrons failed
313
315
@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.")
@warn("MPR (phase 3): Numerical issues with distance computation between $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)). Max. number of iterations (= $niter_max) is reached. niter_max increased locally by 10 and phase 3 is rerun.")
@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.")
return distance, r1, r2, r3, r4 # needed for a unique return type
319
+
@warn("MPR (phase 3): Max. number of iterations (mprIterMax = $niter_max) is reached. Please, increase mprIterMax. tol_rel increased locally for this computation to $new_tol. Look at shapes $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
# Phase 3.3: construct baby tetrahedrons with r1,r2,r3,r4 and create a new portal
366
-
functionmprGeneral(ch::Composition.ContactDetectionMPR_handler{T,F}, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D) where {T,F}
347
+
functionmprGeneral(ch::Composition.ContactDetectionMPR_handler{T,F}, shapeA::Composition.Object3D{F}, shapeB::Modia3D.Composition.Object3D{F}) where {T,F}
367
348
tol_rel = ch.tol_rel
368
349
niter_max = ch.niter_max
369
350
neps = Modia3D.nepsType(T)
@@ -379,6 +360,11 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T,F}, shapeA::Co
379
360
# the direction of the origin ray r0 is -r0.p
380
361
centroidA =getCentroid(shapeA)
381
362
centroidB =getCentroid(shapeB)
363
+
ifisnan(centroidA[1]) ||isnan(centroidB[1]) ||
364
+
isnan(centroidA[2]) ||isnan(centroidB[2]) ||
365
+
isnan(centroidA[3]) ||isnan(centroidB[3])
366
+
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).")
0 commit comments