Merge pull request #53 from ModiaSim/an_switchToDouble64

An switch to double64

Author: GerhardHippmann

src/Basics/constantsAndFunctions.jl

@@ -9,7 +9,7 @@
 # Epsilon and sign
 const neps = sqrt( eps() )
 
-nepsType(::Type{T}) where {T} = sqrt( eps(T) ) # mpr and bounding box calculation use this
+nepsType(::Type{T}) where {T} = 100.0 * eps(T) # mpr and bounding box calculation use this
 
 function sign_eps(value::T) where {T}
     seps::T = 100.0*nepsType(T)

src/Composition/scene.jl

@@ -308,7 +308,7 @@ Defines global properties of the system, such as the gravity field. Exactly one
 | `gravityField`                  | [`UniformGravityField`](@ref)`()`       |
 | `useOptimizedStructure`         | true                                    |
 | `enableContactDetection`        | true                                    |
-| `contactDetection`              | [`ContactDetectionMPR_handler`](@ref)`()`|
+| `mprTolerance`                   | 1.0e-20                                 |
 | `elasticContactReductionFactor` | 1.0                                     |
 | `enableVisualization`           | true                                    |
 | `animationFile`                 | nothing                                 |
@@ -338,7 +338,7 @@ Defines global properties of the system, such as the gravity field. Exactly one
 
 - `enableContactDetection::Bool`: = true, if contact detection is enable, see [Collision Handling](@ref).
 
-- `contactDetection::Modia3D.AbstractContactDetection`: Handler used for contact detection e.g., to determine the smallest distance between two objects.
+- `mprTolerance::1.0e-20`: Local tolerance used for terminating the mpr algorithm. Changing this value might improve speed.
 
 - `elasticContactReductionFactor::Float64`: (> 0.0)
   - ``usedContactCompliance = contactCompliance * elasticContactReductionFactor``
@@ -421,7 +421,7 @@ mutable struct Scene <: Modia3D.AbstractScene
     function Scene(;gravityField          = UniformGravityField(),
             useOptimizedStructure         = true,
             enableContactDetection        = true,
-            contactDetection              = ContactDetectionMPR_handler(),
+            mprTolerance                  = 1.0e-20,
             elasticContactReductionFactor = 1.0,
             gap                           = 0.001,
             enableVisualization           = true,
@@ -443,7 +443,7 @@ mutable struct Scene <: Modia3D.AbstractScene
 
         sceneOptions = SceneOptions(gravityField = gravityField,
             useOptimizedStructure         = useOptimizedStructure,
-            contactDetection              = contactDetection,
+            contactDetection              = ContactDetectionMPR_handler(tol_rel = mprTolerance),
             nVisualContSupPoints          = nVisualContSupPoints,
             gap                           = gap,
             enableContactDetection        = enableContactDetection,

src/Modia3D.jl

@@ -96,8 +96,8 @@ convertAndStripUnit(TargetType, requiredUnit, value) =
             convert(TargetType, ustrip.( uconvert.(requiredUnit, value))) : convert(TargetType, value)
 
 # MPRFloatType is used to change betweeen Double64 and Float64 for mpr calculations
-# using DoubleFloats
-const MPRFloatType = Float64
+using DoubleFloats
+const MPRFloatType = Double64
 
 # Include sub-modules
 include(joinpath("Frames"           , "_module.jl"))

src/contactDetection/ContactDetectionMPR/ContactDetectionMPR_handler.jl

@@ -123,11 +123,11 @@ mutable struct ContactDetectionMPR_handler{T} <: Modia3D.AbstractContactDetectio
     visualizeSupportPoints::Bool
     defaultContactSphereDiameter::Float64
 
-    function ContactDetectionMPR_handler{T}(;tol_rel  = 1.0e-7,
+    function ContactDetectionMPR_handler{T}(;tol_rel  = 1.0e-20,
                                             niter_max = 100) where {T}
         @assert(tol_rel > 0.0)
         @assert(niter_max > 0)
         new(false, Dict{PairID,ContactPair}(), Dict{PairID,ContactPair}(), 42.0, tol_rel, niter_max)
     end
 end
-ContactDetectionMPR_handler() = ContactDetectionMPR_handler{Modia3D.MPRFloatType}()
+ContactDetectionMPR_handler(; kwargs...) = ContactDetectionMPR_handler{Modia3D.MPRFloatType}(; kwargs...)

src/contactDetection/ContactDetectionMPR/mpr.jl

@@ -146,7 +146,7 @@ function tetrahedronEncloseOrigin(r0::SupportPoint, r1::SupportPoint,
         break
     end
     if success != true
-        if niter_max <= 100
+        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, niter_max + 10, shapeA, shapeB, scale)
         else
@@ -315,7 +315,7 @@ function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::Suppor
             end
         end
     end
-    if niter_max <= 100
+    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, niter_max + 10, tol_rel, shapeA, shapeB, scale)
     else
@@ -391,7 +391,7 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Comp
         # e.g. any collision/or distance between two spheres
         #println("TC 1")
         distance = dot(r1.p,normalize(r0.p))
-        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)
+        return (distance, r1.a, r1.b, r1.n, false, SVector{3,T}(Modia3D.ZeroVector3D), SVector{3,T}(Modia3D.ZeroVector3D), SVector{3,T}(Modia3D.ZeroVector3D), SVector{3,T}(Modia3D.ZeroVector3D), SVector{3,T}(Modia3D.ZeroVector3D), SVector{3,T}(Modia3D.ZeroVector3D) )
     else
         # normalize n2
         n2 = n2/n2abs
@@ -412,7 +412,7 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Comp
 
     ###########      Phase 3, Minkowski Portal Refinement      ###################
     (distance, r1, r2, r3, r4) = phase3(r0, r1, r2, r3, niter_max, tol_rel, shapeA, shapeB, scale)
-    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) )
+    return ( distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b )
 end
 
 
@@ -433,7 +433,7 @@ function mprTwoSpheres(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::C
     distance = distanceCentroids - radiusA - radiusB
     contactPointShapeA = centroidSphereA + normal*radiusA
     contactPointShapeB = centroidSphereB - normal*radiusB
-    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)
+    return (distance, contactPointShapeA, contactPointShapeB, normal, false, SVector{3,T}(Modia3D.ZeroVector3D), SVector{3,T}(Modia3D.ZeroVector3D), SVector{3,T}(Modia3D.ZeroVector3D), SVector{3,T}(Modia3D.ZeroVector3D), SVector{3,T}(Modia3D.ZeroVector3D), SVector{3,T}(Modia3D.ZeroVector3D) )
 end
 
 
@@ -454,24 +454,25 @@ function mpr(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Composition
         support1A, support1B, support2A, support2B, support3A, support3B) = mprGeneral(ch, shapeA, shapeB)
 
         if shapeKindA == Modia3D.SphereKind
-            centroidSphere = SVector{3,Float64}(getCentroid(shapeA))
+            centroidSphere = getCentroid(shapeA)
             sphereA1::Shapes.Sphere = shapeA.shape
-            radius = sphereA1.diameter*0.5
+            radius = T(sphereA1.diameter*0.5)
             contactPointSphere = centroidSphere + radius*normal
             contactPointOtherShape = contactPointSphere + distance*normal
             contactPoint1 = contactPointSphere
             contactPoint2 = contactPointOtherShape
         elseif shapeKindB == Modia3D.SphereKind
             normalLocal = -normal
-            centroidSphere = SVector{3,Float64}(getCentroid(shapeB))
+            centroidSphere = getCentroid(shapeB)
             sphereB1::Shapes.Sphere = shapeB.shape
-            radius = sphereB1.diameter*0.5
+            radius = T(sphereB1.diameter*0.5)
             contactPointSphere = centroidSphere + radius*normalLocal
             contactPointOtherShape = contactPointSphere + distance*normalLocal # distance is negative (otherwise direction of normal must be changed)
             contactPoint1 = contactPointOtherShape
             contactPoint2 = contactPointSphere
         end
     end
-    return (distance, contactPoint1, contactPoint2, normal, supportPointsDefined,
-    support1A, support1B, support2A, support2B, support3A, support3B)
+    return (Float64(distance), SVector{3,Float64}(contactPoint1), SVector{3,Float64}(contactPoint2), SVector{3,Float64}(normal), supportPointsDefined,
+    SVector{3,Float64}(support1A), SVector{3,Float64}(support1B), SVector{3,Float64}(support2A), SVector{3,Float64}(support2B), SVector{3,Float64}(support3A), SVector{3,Float64}(support3B) )
+
 end

test/Collision/Billard16Balls.jl

@@ -86,10 +86,9 @@ Ball = Model(
 )
 
 Billard = Model(
-    world = Object3D(feature=Scene(gravityField=UniformGravityField(g=9.81, n=[0, 0, -1]),
+    world = Object3D(feature=Scene(gravityField=UniformGravityField(g=9.81, n=[0, 0, -1]), mprTolerance = 1.0e-18,
                                    enableContactDetection=true,
-                                   nominalLength=0.15*TableX,
-                                   animationFile="Billard16Balls.json")),
+                                   nominalLength=0.15*TableX)), # animationFile="Billard16Balls.json"
     table = Table,
     cushion = Cushion,
     ball0 = Ball,
@@ -165,8 +164,8 @@ stopTime = 5.0
 testTime = 1.5
 tolerance = 1e-7
 interval = 0.001
-requiredFinalStates = [0.9336946243629118, -0.18365520174670474, 0.029998496991356247, -0.7011746198909901, 0.6311295647736829, 1.1287610685608916e-6, -13.537433737049488, -0.3858748206785683, -51.74321001761849, 10.647446137520763, -27.86255858188846, -9.957728295667122, 0.32509152711043904, 0.027780203284739517, 0.029997667930081795, -0.04542743305030694, 0.02500170283086513, 7.062399855287814e-8, 0.9625529100121986, -0.5890320882437217, 1.5160600432975857, -0.8636537095656692, -0.04474632782683813, 1.4954319791715307, 0.39301955928437254, 0.31437311545523294, 0.029997559985557257, -0.0295450598735267, 0.25844118748219774, 6.543232669438406e-8, -7.838021100027451, -0.22760408431777734, 4.052129880389039e-5, -8.613971169221236, -0.015367180725959132, 0.9844568913082455, 0.3526376383583441, -0.14575202949834226, 0.029998721807484585, -0.05704334595170738, -0.09087467006012945, -8.648550230091023e-8, 6.463720232117719, 0.5613406381152121, -0.6619441310051621, 3.0283371525415186, -0.011031122590753485, 1.901031014930388, 0.47669958004172114, 0.30988266338557435, 0.029997421574535003, -0.0006779043504506098, 0.22297266655607906, 2.9010253957924976e-8, -6.65506642882509, -0.004145772945978028, 0.0028226662531805786, -7.4316293203593204, -7.298765565151683e-5, 0.022599728211138887, 0.3783007120971173, -0.012766185812833935, 0.029998759358176016, -0.04494536278891231, -0.018632373536778858, -7.215437557767047e-8, 1.6943216570588908, 0.19414146703780452, 3.298010373792468, -0.3471933000846414, -0.13201026537226748, 1.577730264308742, 0.46331163648978807, -0.09389324485553788, 0.029998896770933176, -0.0016188042316731567, -0.0037739593426910373, -2.5503675848459563e-9, 2.6598529884755653, 0.23804363229547723, 0.3872083559790523, 0.1248900591032485, 0.00033968828158966596, 0.05356980525109636, 0.5432807165029493, 0.2953230757760231, 0.02999730653311251, -0.13624873106020685, -0.18205262515847373, 1.9858018516413702e-7, -5.40642875525656, -0.19653356814243264, 0.09370138552443177, 6.331724950526496, -3.5125692661867474, 2.2385800870275046, 0.5280895349790748, 0.02944513078495663, 0.029997345184240368, -3.671897262472209e-15, -3.6295581814805492e-12, -1.2427691187347466e-15, 1.5884011771883515, 0.00047788925223401856, 0.045046404635795685, 8.53157003717906e-11, -3.8442455469304e-12, 1.2714782913701774e-13, 0.5298393427066304, -0.03176029891751437, 0.029999002794942116, 1.297965371269723e-12, -7.63423453026827e-13, 3.722976278613608e-18, 1.6241627081677217, -0.00011555321024985877, -0.0050541325338022604, 1.7967580704217367e-11, -1.5382972866683138e-12, -3.0499570729309535e-11, 0.5382383412758586, -0.09514048819597622, 0.02999893580898303, 1.3155505360211643e-12, -8.821797446529922e-13, -7.043094683356967e-16, 1.7309170721863256, -0.022807794119219644, -0.28238846622282227, 2.063596477748024e-11, 8.485564273716919e-13, -3.1023757752800775e-11, 0.592498670205686, 0.3681019267809226, 0.029997236420195104, 0.15727976960938742, -0.019558871461817822, -2.644518715680437e-7, -6.163065667728738, 0.37981142806747936, -0.058040028743833724, 0.5349948316934143, 5.243907614414163, -0.34169516938674743, 0.6344294601093406, 0.0669304999587428, 0.029997172347095737, -0.7614223443828724, 0.006309955741959036, 1.2183649383003493e-6, 1.558857816712138, 0.0042029171671443275, -0.7644036324349628, -0.0191002690729473, -0.4380898296432406, 25.377516875983996, 0.7646288540762318, -0.10696914113095653, 0.029998770268321002, 0.15911209364889162, -0.09298081830712687, -2.5596316103907114e-7, 1.9073899574938322, -0.10513813819059165, -6.883833037985919, 3.0989440441527623, 3.5085557038592837e-6, -5.30302146889589, 0.7873220523004107, -0.17884965224773064, 0.029998733802261777, 0.18239453271826178, -0.10527370713656753, -2.9340175118245415e-7, 2.1544182053207317, -0.43508216020191737, -7.5519516937515325, 3.508723323843255, -9.174029482795626e-7, -6.0791204764983995, 0.7976238621443168, -0.24490123678673625, 0.02999869521225786, 0.19303854447667773, -0.11142219870901685, -1.791306576517481e-7, 2.1859628103222946, -0.6263874799512511, -7.873271866785609, 3.7136784821321944, -4.152815543679789e-7, -6.433930426076936]
-simulate!(billard, stopTime=testTime, tolerance=tolerance, interval=interval, log=true, logStates=true, logEvents=true, requiredFinalStates=requiredFinalStates)
+requiredFinalStates = [0.9333661393766369, -0.18381562888712827, 0.029998497688391915, -0.7014362417219272, 0.6310009997544462, 1.1290899588689932e-6, -13.565404070825176, -0.381058964756716, -51.75885657438144, 10.537345277549955, -27.735368779239312, -10.430136200221227, 0.3250638168669772, 0.027712197685071317, 0.029997668116143905, -0.04558267651330124, 0.024882899734608332, 7.08708375292526e-8, 0.965416714333456, -0.5889389817950592, 1.5175095706521067, -0.8627353305843296, -0.050338947235934135, 1.4988260115560812, 0.3929917006965645, 0.3144655526735176, 0.02999756004737067, -0.02957166616376027, 0.2585362147819237, 6.546959176824667e-8, -7.841172993119799, -0.22772933488389074, -0.00031937205113484224, -8.617138862383795, -0.0153761661581909, 0.9853435747006696, 0.3526558673248896, -0.14526528920171633, 0.029998721894158267, -0.056974608242212996, -0.09040531529599616, -8.644860926138948e-8, 6.455235000096263, 0.5697009425702483, -0.6662857408704342, 3.012693173834483, -0.010923114750204998, 1.8987373661439921, 0.4766983392310379, 0.30995639701896677, 0.029997421593076824, -0.0006790472397150304, 0.2230484330415847, 2.9007914762965347e-8, -6.657481205058328, -0.004158194262967625, 0.0028237265472410952, -7.4341549376124965, -7.354492897781964e-5, 0.0226378054711174, 0.37858459379997833, -0.01303304126802059, 0.029998759898762013, -0.04454851388162656, -0.018598671965046183, -7.152403071858711e-8, 1.7002544724045368, 0.19886559468786247, 3.2892872463916563, -0.3413889150764648, -0.14290695554767294, 1.5650562024658172, 0.46333085370780364, -0.09385847703246966, 0.029998896916633183, -0.0016023154034459151, -0.003736631228246453, -2.524756751505809e-9, 2.658771952706271, 0.2375361890551339, 0.38685341623759373, 0.12364571680165906, 0.00033631987729715405, 0.05302026303612267, 0.5432830285468065, 0.29528415239802475, 0.029997306522725007, -0.1361340886884427, -0.18217533682898493, 1.9837453065516486e-7, -5.405252316032075, -0.19683033148976267, 0.09405739322728604, 6.335836054778727, -3.508750848458166, 2.2362775359298994, 0.5279576840249166, 0.029411604271573133, 0.029997345534074352, -2.0097480807390374e-20, 6.630238551643466e-21, -7.852186594481353e-16, 1.5894787461282525, 0.0005510984167413897, 0.04940034311617822, -1.4726489653714904e-19, 1.6298099217397075e-20, 4.819327329807566e-19, 0.5298457116337745, -0.031876469837529606, 0.029999002929103884, 6.441996653051123e-21, 2.044078647259606e-21, 7.478575748967392e-16, 1.6280210741379746, -0.00013021384141367968, -0.005265784765458889, -6.303145274641481e-20, -9.155654576004988e-21, -1.540158056949745e-19, 0.5382254132148557, -0.09513309283457251, 0.029998935946514784, -2.4353611016104665e-19, 1.1480757656293768e-19, -3.402512648485731e-16, 1.7306808428774456, -0.022739317690155147, -0.28196399908821645, -2.7292648305085957e-18, 1.860055121430653e-19, 5.876967232455233e-18, 0.592522307113131, 0.368128230829464, 0.029997236390732995, 0.15718315723461718, -0.019429213295171666, -2.642803208328939e-7, -6.163666213388699, 0.38062391948289315, -0.05791773996043013, 0.5307694564582713, 5.240733887108378, -0.3393612241165449, 0.6345581404814613, 0.06693110909072808, 0.029997172271377055, -0.7610317598699818, 0.006309980725063319, 1.2177417150428154e-6, 1.5589113127222334, 0.004251050614008636, -0.7683880059515366, -0.019236388490959964, -0.43789479945224935, 25.364498817249434, 0.7644490357187969, -0.10686411031544148, 0.029998770706948247, 0.15892605579721072, -0.09287215208524814, -2.5564504515598876e-7, 1.904457619305972, -0.1031523714772522, -6.878100110409454, 3.095322174103587, 3.4875825666557394e-6, -5.296819787141439, 0.7873764400470138, -0.17888103685815732, 0.029998733869831, 0.18245039270778482, -0.10530593919198675, -2.934692365195682e-7, 2.154753570394882, -0.4360610751585018, -7.553586183876722, 3.5097973316114093, -1.098206217365888e-6, -6.0809827701210315, 0.7977083789714665, -0.24495001552925505, 0.02999869529308616, 0.19312533440021248, -0.11147228664501829, -1.7918625043209475e-7, 2.1859222338974504, -0.6279787289271236, -7.876052995028591, 3.7153480112389947, -1.3327607189387022e-6, -6.436823566539937]
+simulate!(billard, stopTime=testTime, tolerance=tolerance, interval=interval, log=true, logStates=false, logEvents=false, requiredFinalStates=requiredFinalStates)
 
 @usingModiaPlot
 plot(billard, ["joint0.r" "joint0.rot"; "joint0.v" "joint0.w"], figure=1)

test/Collision/Billard4Balls.jl

@@ -115,7 +115,7 @@ testTime = 2.5
 tolerance = 1e-8
 interval = 0.0001
 requiredFinalStates = [-0.06367465272629756, 0.24238657622364665, 0.02999809758732255, -0.8055793547583174, 0.5049384414649515, -1.2942520100327887e-6, -41.98758798677127, 1.0302592492754234, -61.383962718912315, 7.327199670112295, 30.778026759406714, -1.8364952739701552, -0.15196276181074325, 0.179157995489763, 0.029997955278049118, -0.5749051726066565, 0.19607773653519803, -9.23765746465685e-7, 1.823582864167988, -0.11082435017660355, 18.026385421294833, -6.535777886518464, 3.361853105628269e-5, 19.163080641737785, 0.415730418200003, -0.25313065448897354, 0.02999753001826395, -0.004216104905417898, -0.49464670233658653, 3.901393071314717e-9, 11.54177678780837, 0.005646322377435987, 0.011097198279087365, 16.486263237425984, -0.25596215953403123, -0.026989059565206447, 0.15504140104701092, -0.18658321851036563, 0.029997948409715088, -0.297314286397064, -0.176662447164565, 4.779910373218372e-7, 0.9427627045555219, 0.6640319309374063, 12.884489822750403, -2.1107480248530672, -5.435105655586106, 9.944019158456165]
-simulate!(billard, stopTime=testTime, tolerance=tolerance, interval=interval, log=true, logStates=true, logEvents=true, requiredFinalStates=requiredFinalStates)
+simulate!(billard, stopTime=testTime, tolerance=tolerance, interval=interval, log=true, logStates=false, logEvents=false, requiredFinalStates=requiredFinalStates)
 
 @usingModiaPlot
 plot(billard, ["joint0.r" "joint0.rot"; "joint0.v" "joint0.w"], figure=1)