Merge pull request #42 from ModiaSim/an_avoidCollisionErrors

An avoid collision errors

Author: GerhardHippmann

src/contactDetection/ContactDetectionMPR/mpr.jl

@@ -21,14 +21,6 @@ function getSupportPoint(shapeA::Modia3D.Composition.Object3D, shapeB::Compositi
     return SupportPoint((a-b).*scale,n,a,b)
 end
 
-#=
-function getSupportPoint(shapeA::Modia3D.Composition.Object3D, shapeB::Composition.Object3D, n::Nothing; scale::Float64=1.0)
-    println(Modia3D.fullName(shapeA), " r_abs ", shapeA.r_abs, " R_abs ", shapeA.R_abs)
-    println(Modia3D.fullName(shapeB), " r_abs ", shapeB.r_abs, " R_abs ", shapeB.R_abs)
-    error("numerical issues computing normalized vector")
-    return nothing
-end
-=#
 
 function barycentric(r1::SupportPoint,r2::SupportPoint,r3::SupportPoint,r4::SupportPoint)
     r21 = r2.p - r1.p
@@ -75,7 +67,7 @@ end
 # belongs to construction of r0
 function checkCentersOfShapesOverlapp(r0::SupportPoint, neps::Float64, shapeA::Composition.Object3D, shapeB::Composition.Object3D)
     if norm(r0.p) <= neps
-        error("MPR: Too large penetration (prerequisite of MPR violated). Centers are overlapping. Look at shapeA = ", shapeA, " shapeB = ", shapeB)
+        error("MPR: Too large penetration (prerequisite of MPR violated). Centers are overlapping. Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
     end
 end
 
@@ -94,7 +86,7 @@ function checkIfShapesArePlanar(r0::SupportPoint,r1::SupportPoint,r2::SupportPoi
             # Shape is purely planar. Computing the shortest distance for a planar shape
             # requires an MPR 2D algorithm (using lines instead of triangles as portals).
             # However, this is not implemented and therefore the shortest distance cannot be computed
-            error("MPR: Shapes are planar and MPR2D is not supported. abs(dot((r2.p-r1.p),n2)). Look at shapeA = ", shapeA, " shapeB = ", shapeB)
+            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)).")
         end
         # new normal to the triangle plane (r0-r1-r2_new)
         n3 = cross(r1.p-r0.p, r2.p-r0.p)   # |n3| > 0 guaranteed, due to construction
@@ -114,7 +106,7 @@ function checkIfShapesArePlanar(r0::SupportPoint,r1::SupportPoint,r2::SupportPoi
             # Shape is purely planar. Computing the shortest distance for a planar shape
             # requires an MPR 2D algorithm (using lines instead of triangles as portals).
             # However, this is not implemented and therefore the shortest distance cannot be computed
-            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 shapeA = ", shapeA, " shapeB = ", shapeB)
+            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)).")
         end
     end
 
@@ -126,9 +118,13 @@ end
 # loop around to "ensure" the tetrahedron r0,r1,r2 and r3 encloses the origin
 # stimmt so nicht wirklich, muss ich nochmal nachlesen!!!
 # Der Ursprung muss nicht enthalten sein!!!
-function tetrahedronEncloseOrigin(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,r3::SupportPoint,
-                                  neps::Float64, niter_max::Int64,
-                                  shapeA::Composition.Object3D,shapeB::Composition.Object3D, scale::Float64)
+function tetrahedronEncloseOrigin(r0::SupportPoint, r1::SupportPoint,
+            r2::SupportPoint, r3::SupportPoint,
+            neps::Float64, niter_max::Int64,
+            shapeA::Composition.Object3D, shapeB::Composition.Object3D, scale::Float64)
+    r1org = r1
+    r2org = r2
+    r3org = r3
     aux = Modia3D.ZeroVector3D
     success = false
     for i in 1:niter_max
@@ -148,9 +144,13 @@ function tetrahedronEncloseOrigin(r0::SupportPoint,r1::SupportPoint,r2::SupportP
         break
     end
     if success != true
-        error("MPR: Max. number of iterations is reached in phase2. Look at shapeA = ", shapeA, " shapeB = ", shapeB)
+        if niter_max <= 100
+            @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)).")
+            tetrahedronEncloseOrigin(r0, r1org, r2org, r3org, neps, niter_max + 10, shapeA, shapeB, scale)
+        else
+            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)).")
+        end
     end
-    # pointInTetrahedron(r0.p,r1.p,r2.p,r3.p,SVector(0.0, 0.0, 0.0))
     return (r1, r2, r3)
 end
 
@@ -166,7 +166,7 @@ function constructR4(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,r3::Supp
             # Shape is purely planar. Computing the shortest distance for a planar shape
             # requires an MPR 2D algorithm (using lines instead of triangles as portals).
             # However, this is not implemented and therefore the shortest distance cannot be computed
-            error("MPR: Shapes are planar and MPR2D is not supported. abs(dot((r3.p-r1.p),r3.n)). Look at shapeA = ", shapeA, " shapeB = ", shapeB)
+            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)).")
         end
         n4 = cross(r2.p-r1.p, r3.p-r1.p)   # |n4| > 0 guaranteed, due to construction
     end
@@ -178,8 +178,8 @@ function constructR4(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,r3::Supp
     return (r3, r4, n4)
 end
 
-
-isNextPortal(r0,r1,r2,r4) = dot( cross( (r2.p-r0.p), (r4.p-r0.p) ), r0.n ) <= 0.0 &&
+# computes twice a parallelepipedial product (Spatprodukt)
+isNextPortal(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r4::SupportPoint) = dot( cross( (r2.p-r0.p), (r4.p-r0.p) ), r0.n ) <= 0.0 &&
                             dot( cross( (r4.p-r0.p), (r1.p-r0.p) ), r0.n ) <= 0.0
 
 
@@ -189,29 +189,25 @@ isNextPortal(r0,r1,r2,r4) = dot( cross( (r2.p-r0.p), (r4.p-r0.p) ), r0.n ) <= 0.
 #   r1r2r4
 #   r2r3r4
 #   r3r1r4
-function createBabyTetrahedrons(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,r3::SupportPoint,r4::SupportPoint,
-                                shapeA::Composition.Object3D,shapeB::Composition.Object3D)
+function createBabyTetrahedrons(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, r4::SupportPoint)
+    nextPortal = true
     if isNextPortal(r0,r1,r2,r4)
         r3 = r4
     elseif isNextPortal(r0,r2,r3,r4)
         r1 = r4
     elseif isNextPortal(r0,r3,r1,r4)
         r2 = r4
     else
-        # Compute the closest distance to the portal and return it:
-        (r4.p, distance) = signedDistanceToPortal(r0.p,r1.p,r2.p,r3.p)
-        barycentric(r1,r2,r3,r4)
-        error("MPR: Numerical issues with distance computation between shapeA = ", shapeA, " and shapeB = ", shapeB,". Used distance = ",distance)
-        return (r1, r2, r3)
+        nextPortal = false # the signs of all tried combinations of parallelepipedial products are not negative
     end
-    return (r1, r2, r3)
+    return (nextPortal, r1, r2, r3)
 end
 
 
 scaleVector(scale, ri) = ri.*scale
 
 
-function skalarization(r0,r1,r2,r3)
+function skalarization(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::SupportPoint)
     x = maximum([abs.(r0.p) abs.(r1.p) abs.(r2.p) abs.(r3.p)])
     scale = 1/x
     r0.p = scaleVector(scale, r0.p)
@@ -221,6 +217,116 @@ function skalarization(r0,r1,r2,r3)
     return (r0, r1, r2, r3, scale)
 end
 
+function finalTC2(r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, r4::SupportPoint)
+    #println("TC 2")
+    #if !analyzeFinalPortal(r1.p, r2.p, r3.p, r4.p, neps)
+        # error("shapeA = ", shapeA, " shapeB = ", shapeB)
+    #end
+    distance = -dot(r4.n, r4.p)
+    barycentric(r1,r2,r3,r4)
+    return distance, r1, r2, r3, r4
+end
+
+
+function finalTC3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, r4::SupportPoint)
+    #println("TC 3")
+
+    #doesRayIntersectPortal(r1.p,r2.p,r3.p, r4.p,neps)
+    #println("r1.p = ", r1.p , " r2.p = ", r2.p ," r3.p = ", r3.p)
+    #println("r4.p = ", r4.p)
+    #println(" ")
+    #if !analyzeFinalPortal(r1.p, r2.p, r3.p, r4.p, neps)
+        # error("shapeA = ", shapeA, " shapeB = ", shapeB)
+    #end
+    (r4.p, distance) = signedDistanceToPortal(r0.p,r1.p,r2.p,r3.p)
+    barycentric(r1,r2,r3,r4)
+    return distance, r1, r2, r3, r4
+end
+
+
+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)
+    r1org = r1
+    r2org = r2
+    r3org = r3
+    new_tol = 42.0
+    isTC2 = false
+    isTC3 = false
+    r1_new::SupportPoint = r0
+    r2_new::SupportPoint = r0
+    r3_new::SupportPoint = r0
+    r4_new::SupportPoint = r0
+    for i in 1:niter_max
+        ### Phase 3.1: construct r4 ###
+        # Find support point using the tetrahedron face
+        (r3,r4,n4) = constructR4(r0,r1,r2,r3,neps,shapeA,shapeB, scale)
+
+
+        ### Phase 3.2: check if r4 is close to the origin ###
+        # check if the new point r4 is already on the plane of the triangle r1,r2,r3,
+        # we're already as close as we can get to the origin
+        TC2 = norm(cross(r4.p,r4.n))    # TC2
+        TC3 = abs(dot(r4.p-r1.p, r4.n)) # TC3
+        ## TERMINATION CONDITION 2 ##
+        if TC2 < tol_rel
+            (distance,r1,r2,r3,r4) = finalTC2(r1,r2,r3,r4)
+            return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
+
+        ## TERMINATION CONDITION 3 ##
+        elseif TC3 < tol_rel
+            (distance,r1,r2,r3,r4) = finalTC3(r0, r1, r2, r3, r4)
+            return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
+        else
+            if TC2 < new_tol
+                new_tol = TC2
+                isTC2 = true
+                isTC3 = false
+                r1_new = r1
+                r2_new = r2
+                r3_new = r3
+                r4_new = r4
+            end
+            if TC3 < new_tol
+                new_tol = TC3
+                isTC2 = false
+                isTC3 = true
+                r1_new = r1
+                r2_new = r2
+                r3_new = r3
+                r4_new = r4
+            end
+        end
+
+        #### Phase 3.3: construct baby tetrahedrons with r1,r2,r3,r4 and create a new portal ###
+        # Construction of three baby tetrahedrons
+        (nextPortal, r1,r2,r3) = createBabyTetrahedrons(r0,r1,r2,r3,r4)
+
+        if !nextPortal # createBabyTetrahedrons failed
+            @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.")
+            if isTC2
+                (distance,r1,r2,r3,r4) = finalTC2(r1_new,r2_new,r3_new,r4_new)
+                return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
+            end
+            if isTC3
+                (distance,r1,r2,r3,r4) = finalTC3(r0, r1_new, r2_new, r3_new, r4_new)
+                return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
+            end
+        end
+    end
+    if niter_max <= 100
+        @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.")
+        phase3(r0, r1org, r2org, r3org, neps, niter_max + 10, tol_rel, shapeA, shapeB, scale)
+    else
+        @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.")
+        if isTC2
+            (distance,r1,r2,r3,r4) = finalTC2(r1_new,r2_new,r3_new,r4_new)
+            return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
+        end
+        if isTC3
+            (distance,r1,r2,r3,r4) = finalTC3(r0, r1_new, r2_new, r3_new, r4_new)
+            return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
+        end
+    end
+end
 
 # MPR - Minkowski Portal Refinement algorithm
 # construction of points r0 is in the interior of Minkowski Difference and points r1,r2,r3,r4 are on the boundary of Minkowski Difference
@@ -301,59 +407,7 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler, shapeA::Composi
 
 
     ###########      Phase 3, Minkowski Portal Refinement      ###################
-    for i in 1:niter_max
-        ### Phase 3.1: construct r4 ###
-        # Find support point using the tetrahedron face
-        (r3,r4,n4) = constructR4(r0,r1,r2,r3,neps,shapeA,shapeB, scale)
-
-
-        ### Phase 3.2: check if r4 is close to the origin ###
-        # check if the new point r4 is already on the plane of the triangle r1,r2,r3,
-        # we're already as close as we can get to the origin
-
-        ## TERMINATION CONDITION 2 ##
-        if norm(cross(r4.p,r4.n)) < tol_rel
-            #println("TC 2")
-            #if !analyzeFinalPortal(r1.p, r2.p, r3.p, r4.p, neps)
-                # error("shapeA = ", shapeA, " shapeB = ", shapeB)
-            #end
-            distance = -dot(r4.n, r4.p)
-            barycentric(r1,r2,r3,r4)
-
-            return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
-
-        ## TERMINATION CONDITION 3 ##
-        elseif abs(dot(r4.p-r1.p, r4.n)) < tol_rel
-            #println("TC 3")
-
-            #doesRayIntersectPortal(r1.p,r2.p,r3.p, r4.p,neps)
-            #println("r1.p = ", r1.p , " r2.p = ", r2.p ," r3.p = ", r3.p)
-            #println("r4.p = ", r4.p)
-            #println(" ")
-            #if !analyzeFinalPortal(r1.p, r2.p, r3.p, r4.p, neps)
-                # error("shapeA = ", shapeA, " shapeB = ", shapeB)
-            #end
-            (r4.p, distance) = signedDistanceToPortal(r0.p,r1.p,r2.p,r3.p)
-            barycentric(r1,r2,r3,r4)
-
-            return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b)
-        end
-
-        #### Phase 3.3: construct baby tetrahedrons with r1,r2,r3,r4 and create a new portal ###
-        # Construction of three baby tetrahedrons
-        (r1,r2,r3) = createBabyTetrahedrons(r0,r1,r2,r3,r4,shapeA,shapeB)
-
-
-    #=
-        r0RayOnPortal = isr0RayOnPortal(r0.p,r1.p,r2.p,r3.p)
-        if r0RayOnPortal[1] != true
-        println("r0RayOnPortal = ", r0RayOnPortal, ": Wrong baby-tetrahedron selected. " , i)
-        end
-        =#
-
-    end
-    error("MPR: Should never get here! Computation failed. Look at shapeA = ", Modia3D.instanceName(shapeA),
-        " shapeB = ",  Modia3D.instanceName(shapeB))
+    phase3(r0, r1, r2, r3, neps, niter_max, tol_rel, shapeA, shapeB, scale)
 end
 
 
@@ -367,7 +421,7 @@ function mprTwoSpheres(ch::Composition.ContactDetectionMPR_handler, shapeA::Comp
     n = centroidSphereB - centroidSphereA
     distanceCentroids = norm(n)
     if distanceCentroids <= neps
-        error("Centers of two spheres are overlapping. SphereA = ", shapeA, " sphereB = ", shapeB)
+        error("Centers of two spheres are overlapping. Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).")
     else
         normal = n/distanceCentroids
     end