Merge pull request #102 from ModiaSim/an_allocCollision

An alloc collision

Author: GerhardHippmann

src/Composition/dynamicCollision.jl

@@ -5,9 +5,10 @@
 # (see file ...\ContactDetectionMPR\handler.jl)
 # after computing all distances between colliding shapes response calculations are done
 function computeContactForcesAndTorques(sim::Modia.SimulationModel, scene, world, time, file)
-  ch = scene.options.contactDetection
-  # Compute signed distances of all contact shapes during zero-crossing computation
-    #try
+    if typeof(scene.options.contactDetection) <: Modia3D.ContactDetectionMPR_handler
+        ch::Modia3D.ContactDetectionMPR_handler = scene.options.contactDetection
+        # Compute signed distances of all contact shapes during zero-crossing computation
+            #try
         setComputationFlag(ch)
         if Modia.isEvent(sim)             # with Event
             # handle event
@@ -20,9 +21,14 @@ function computeContactForcesAndTorques(sim::Modia.SimulationModel, scene, world
             TimerOutputs.@timeit sim.timer "Modia3D_1 getDistances!"  getDistances!(scene, ch, world)
         end
         TimerOutputs.@timeit sim.timer "Modia3D_1 dealWithContacts!"  dealWithContacts!(sim, scene, ch, world, time, file)
+        # dealWithContacts!(sim, scene, ch, world, time, file)
     #catch e
         #println("Bug in Modia3D/src/Composition/dynamicCollision.jl: error curing collision detection at time = ", time)
     #end
+    else
+        error("No contact detection handler is defined")
+    end
+    return nothing
 end
 
 # each shape pair in contact is stored in ch.contactDict
@@ -33,27 +39,53 @@ end
 # (see file ...\Composition\responseCalculation\elasticCollisionResponse.jl)
 # further, at an event simulation status is updated, contact material is replaced
 # and the actual contactDict is stored
-function dealWithContacts!(sim, scene::Scene{F}, ch, world, time, file) where F <: Modia3D.VarFloatType
-    simh = sim.eventHandler
+function dealWithContacts!(sim::Modia.SimulationModel{F, T}, scene::Scene{F}, ch::Composition.ContactDetectionMPR_handler{M,F}, world::Composition.Object3D{F}, time::Float64, file::Nothing)::Nothing where {F <: Modia3D.VarFloatType, T, M}
+
+    simh::Modia.EventHandler{F,T} = sim.eventHandler
     f1::SVector{3,F}=Modia3D.ZeroVector3D(F)
     f2::SVector{3,F}=Modia3D.ZeroVector3D(F)
     t1::SVector{3,F}=Modia3D.ZeroVector3D(F)
     t2::SVector{3,F}=Modia3D.ZeroVector3D(F)
 
-    for (pairID, pair) in ch.contactDict
+    for (pairID::Int64, pair::ContactPair{F}) in ch.contactDict
         obj1 = pair.obj1
-        obj2 = pair.obj2
-        rContact      = (pair.contactPoint1 + pair.contactPoint2)/F(2.0)
+        obj2= pair.obj2
+        rContact= (pair.contactPoint1 + pair.contactPoint2)/F(2.0)
         contactNormal = pair.contactNormal
         if Modia.isEvent(sim)
             # println("$(sim.time): ", obj1.path, " ", obj2.path)
             getMaterialContactStart(scene, ch, simh, pair, pairID, obj1, obj2, rContact, contactNormal)
             visualizeContactAndSupportPoints(ch, world)
         end
         if scene.options.enableContactDetection
-            (f1,f2,t1,t2) = responseCalculation(pair.contactPairMaterial, obj1, obj2,
+            if pair.pairKind == Modia3D.NoContactPairKind
+                noContactPairMaterial::Shapes.NoContactPairMaterial = pair.contactPairMaterial
+                (f1,f2,t1,t2) = responseCalculation(noContactPairMaterial, obj1, obj2,
                             rContact, contactNormal,
                             pair.distanceWithHysteresis, time, file, sim)
+            elseif pair.pairKind == Modia3D.ElasticContactPairKind
+                elasticContactPairMaterial::Composition.ElasticContactPairResponseMaterial = pair.contactPairMaterial
+                (f1,f2,t1,t2) = responseCalculation(elasticContactPairMaterial, obj1, obj2,
+                    rContact, contactNormal,
+                    pair.distanceWithHysteresis, time, file, sim)
+            elseif pair.pairKind == Modia3D.ObserverContactPairKind
+                observerContactPairMaterial::Shapes.ObserverContactPairMaterial = pair.contactPairMaterial
+                (f1,f2,t1,t2) = responseCalculation(observerContactPairMaterial, obj1, obj2,
+                rContact, contactNormal,
+                pair.distanceWithHysteresis, time, file, sim)
+            elseif pair.pairKind == Modia3D.ImpulseContactPairKind
+                impulseContactPairMaterial::Shapes.ImpulseContactPairMaterial = pair.contactPairMaterial
+                (f1,f2,t1,t2) = responseCalculation(impulseContactPairMaterial, obj1, obj2,
+                rContact, contactNormal,
+                pair.distanceWithHysteresis, time, file, sim)
+            elseif pair.pairKind == Modia3D.WheelRailContactPairKind
+                wheelRailContactPairMaterial::Shapes.WheelRailContactPairMaterial = pair.contactPairMaterial
+                (f1,f2,t1,t2) = responseCalculation(wheelRailContactPairMaterial, obj1, obj2,
+                rContact, contactNormal,
+                pair.distanceWithHysteresis, time, file, sim)
+            else
+                error("not supported contact pair material")
+            end
 
             # Transform forces/torques in local part frames
             obj1.f += obj1.R_abs*f1
@@ -70,7 +102,6 @@ function dealWithContacts!(sim, scene::Scene{F}, ch, world, time, file) where F
 end
 
 
-
 # mainly its for assigning contact materials.
 # therefore, lastContactDict stores information of shape pairs in contact at last step
 # if the actual shape pair was in contact at the last step as well, the already assigned contact material is taken
@@ -79,12 +110,13 @@ function getMaterialContactStart(scene, ch, simh, pair, pairID, obj1, obj2, rCon
     if haskey(ch.lastContactDict, pairID)
         # use material (reference) from previous event
         if scene.options.enableContactDetection
-        pair.contactPairMaterial = ch.lastContactDict[pairID].contactPairMaterial    # improve later (should avoid to inquire pairID twice)
+            pair.contactPairMaterial = ch.lastContactDict[pairID].contactPairMaterial    # improve later (should avoid to inquire pairID twice)
+            pair.pairKind = ch.lastContactDict[pairID].pairKind
         end
     else
         # determine contact pair material
         if scene.options.enableContactDetection
-            pair.contactPairMaterial = contactStart(obj1, obj2, rContact, contactNormal,
+            (pair.contactPairMaterial, pair.pairKind) = contactStart(obj1, obj2, rContact, contactNormal,
                 scene.options.elasticContactReductionFactor,
                 scene.options.maximumContactDamping)
         end

src/Composition/massPropertiesComputation.jl

@@ -44,7 +44,7 @@ end
 # It sums up common mass, inertia tensor and center of mass.
 # The results are stored twice in actual root of super object:
 #       massProperties: container for actual values
-function addMassPropertiesOfAllSuperObjChildsToRootSuperObj!(rootSuperObj::Object3D, actualMassSuperObject::Vector{Object3D{F}}) where F <: Modia3D.VarFloatType
+function addMassPropertiesOfAllSuperObjChildsToRootSuperObj!(rootSuperObj::Object3D{F}, actualMassSuperObject::Vector{Object3D{F}}) where F <: Modia3D.VarFloatType
     if length(actualMassSuperObject) > 0
         for i=1:length(actualMassSuperObject)
             obj = actualMassSuperObject[i]
@@ -74,7 +74,8 @@ end
 
 # function computeInertiaTensorForTwoBodies!(massPropParent, massPropObj, obj; add=true)
 function addOrSubtractMassPropertiesOfChildToRoot!(obj_root::Object3D{F}, obj_child::Object3D{F}; add=true)::Nothing where F <: Modia3D.VarFloatType
-    massProperties_child = obj_child.feature.massProperties
+    solid::Shapes.Solid{F} = obj_child.feature
+    massProperties_child::Shapes.MassProperties{F} = solid.massProperties
 
     R_child   = obj_child.R_rel
     r_child   = obj_child.r_rel
@@ -94,7 +95,7 @@ function addOrSubtractMassPropertiesOfChildToRoot!(obj_root::Object3D{F}, obj_ch
     #                 (no need of rotation matrices)
     I_child_steiner = Modia3D.NullRotation(F) * I_child * Modia3D.NullRotation(F)' +
                     m_child * Modia3D.skew(rCM_child_new)' * Modia3D.skew(rCM_child_new)
-    I_root_steiner = I_root +
+    I_root_steiner  = I_root +
                     m_root * Modia3D.skew(rCM_root)' * Modia3D.skew(rCM_root)
 
     if add
@@ -108,13 +109,12 @@ function addOrSubtractMassPropertiesOfChildToRoot!(obj_root::Object3D{F}, obj_ch
 
         # I: substract new common mass multiplied with skew matrices of
         #    center of mass from the sum of I_root_steiner and I_child_steiner
-        I = I_root_steiner + I_child_steiner - m * Modia3D.skew(rCM)' * Modia3D.skew(rCM)
+        I =I_root_steiner + I_child_steiner - m * Modia3D.skew(rCM)' * Modia3D.skew(rCM)
 
         # Assign to obj_root
         obj_root.m    = m
         obj_root.r_CM = rCM
         obj_root.I_CM = I
-        return nothing
 
         #= # other way to compute inertia tensor
         a1 = rCM_new - rCM_child_new
@@ -143,5 +143,6 @@ function addOrSubtractMassPropertiesOfChildToRoot!(obj_root::Object3D{F}, obj_ch
         obj_root.m    = m
         obj_root.r_CM = rCM
         obj_root.I_CM = I
-        return nothing
-end; end
+    end
+    return nothing
+end

src/Composition/responseCalculation/elasticCollisionResponse.jl

@@ -36,22 +36,22 @@ function resultantDampingCoefficient(cor, abs_vreln, vsmall, maximumContactDampi
     return d_res
 end
 
-function elasticContactPairCoefficients(obj1::Object3D, obj2::Object3D)
-    solid1::Shapes.Solid = obj1.feature
-    solid2::Shapes.Solid = obj2.feature
-
+function elasticContactPairCoefficients(obj1::Object3D{F}, obj2::Object3D{F}) where F <: Modia3D.VarFloatType
+    solid1::Shapes.Solid{F} = obj1.feature
+    solid2::Shapes.Solid{F} = obj2.feature
+    mu_r_geo = F(0.0)
     if !solid1.isFlat && solid2.isFlat
         mu_r_geo = solid1.contactSphereRadius
     elseif solid1.isFlat && !solid2.isFlat
         mu_r_geo = solid2.contactSphereRadius
     else # (solid1.isFlat && solid2.isFlat) || (!solid1.isFlat && !solid2.isFlat)
-        r1 = solid1.contactSphereRadius
-        r2 = solid2.contactSphereRadius
+        r1::F = solid1.contactSphereRadius
+        r2::F = solid2.contactSphereRadius
         mu_r_geo = r1*r2/(r1 + r2)
     end
 
-    n_geo = 1.5
-    c_geo = 4/3*sqrt(mu_r_geo)
+    n_geo = F(1.5)
+    c_geo = F(4/3*sqrt(mu_r_geo))
 
     return (c_geo, n_geo, mu_r_geo)
 end
@@ -105,7 +105,7 @@ function contactStart(matPair::Shapes.ElasticContactPairMaterial,
                                 matPair.rotationalResistanceCoefficient, mu_r_geo,
                                 matPair.vsmall, matPair.wsmall)
     end
-    return responseMaterial
+    return (responseMaterial, Shapes.ElasticContactPairKind)
 end
 
 

src/Composition/responseCalculation/othersCollisionResponse.jl

@@ -1,7 +1,7 @@
 contactStart(matPair::Shapes.NoContactPairMaterial, obj1::Object3D{F}, obj2::Object3D{F}, rContact::SVector{3,F}, contactNormal::SVector{3,F}, elasticContactReductionFactor::F, maximumContactDamping::F) where F <: Modia3D.VarFloatType =
-matPair
+(matPair, Shapes.NoContactPairKind)
 
-contactStart(matPair::Shapes.ObserverContactPairMaterial, obj1::Object3D{F}, obj2::Object3D{F}, rContact::SVector{3,F}, contactNormal::SVector{3,F}, elasticContactReductionFactor::F, maximumContactDamping::F) where F <: Modia3D.VarFloatType = matPair
+contactStart(matPair::Shapes.ObserverContactPairMaterial, obj1::Object3D{F}, obj2::Object3D{F}, rContact::SVector{3,F}, contactNormal::SVector{3,F}, elasticContactReductionFactor::F, maximumContactDamping::F) where F <: Modia3D.VarFloatType = (matPair, Shapes.ObserverContactPairKind)
 
 
 contactStart(matPair::Shapes.ImpulseContactPairMaterial, obj1::Object3D{F}, obj2::Object3D{F}, rContact::SVector{3,F}, contactNormal::SVector{3,F}, elasticContactReductionFactor::F, maximumContactDamping::F) where F <: Modia3D.VarFloatType =

src/Composition/scene.jl

@@ -202,7 +202,7 @@ struct SceneOptions{F <: Modia3D.VarFloatType}
 
     ### Contact detection ###
     enableContactDetection::Bool            # = true, if contact detection is enabled
-    contactDetection::Modia3D.AbstractContactDetection
+    contactDetection::ContactDetectionMPR_handler{Modia3D.MPRFloatType, F}
     elasticContactReductionFactor::F  # c_res_used = c_res * elasticContactReductionFactor (> 0)
     maximumContactDamping::F
     gap::Float64

src/Shapes/_module.jl

@@ -66,6 +66,10 @@ export ShapeKind, UndefinedShapeKind, SphereKind, EllipsoidKind, BoxKind, Cylind
 
 @enum ShapeKind UndefinedShapeKind SphereKind EllipsoidKind BoxKind CylinderKind ConeKind CapsuleKind BeamKind FileMeshKind CoordinateSystemKind GridKind SpringKind GearWheelKind ModelicaKind TextKind
 
+export PairMaterialKind, ElasticContactPairKind, NoContactPairKind, ObserverContactPairKind, ImpulseContactPairKind, WheelRailContactPairKind
+
+@enum PairMaterialKind ElasticContactPairKind NoContactPairKind ObserverContactPairKind ImpulseContactPairKind WheelRailContactPairKind
+
 include("color.jl")
 include("visualMaterial.jl")
 include("text.jl")

src/contactDetection/ContactDetectionMPR/ContactDetectionMPR_handler.jl

@@ -36,6 +36,7 @@ mutable struct ContactPair{F <: Modia3D.VarFloatType}
     support2B::SVector{3,F}
     support3B::SVector{3,F}
 
+    pairKind::Shapes.PairMaterialKind
     contactPairMaterial::Union{Modia3D.AbstractContactPairMaterial,Nothing}  # only if contact = true, otherwise not defined
 
     ContactPair{F}(contactPoint1::SVector{3,F}, contactPoint2::SVector{3,F},

src/contactDetection/ContactDetectionMPR/handler.jl

@@ -10,21 +10,22 @@ AABB_touching(aabb1::Basics.BoundingBox, aabb2::Basics.BoundingBox) = aabb1.x_ma
 
 
 
-function Composition.initializeContactDetection!(world::Composition.Object3D, scene::Composition.Scene)::Nothing
+function Composition.initializeContactDetection!(world::Composition.Object3D{F}, scene::Composition.Scene{F})::Nothing where F <: Modia3D.VarFloatType
     if typeof(scene.options.contactDetection) <: Modia3D.ContactDetectionMPR_handler
-        ch = scene.options.contactDetection
+        ch::Modia3D.ContactDetectionMPR_handler = scene.options.contactDetection
         ch.contactPairs = Composition.ContactPairs(world, scene, ch.visualizeContactPoints,
             ch.visualizeSupportPoints, ch.defaultContactSphereDiameter)
         if ch.contactPairs.nz == 0
             Composition.closeContactDetection!(ch)
             scene.collide = false
                 @warn "... From Modia3D collision handler: Collision handling switched off, since no contacts can take place (nz=0).\n" *
                     "... You might need to set canCollide=true at joints.\n"
-            return
+            return nothing
         end
         @assert(ch.contactPairs.ne > 0)
         @assert(ch.contactPairs.nz > 0)
     end
+    return nothing
 end
 
 

src/contactDetection/ContactDetectionMPR/utilitiesPairID.jl

@@ -2,35 +2,35 @@
 # Copyright 2017-2018, DLR Institute of System Dynamics and Control
 
 
-const i16max = Int64(typemax(Int16))
-const i32max = Int64(typemax(Int32))
+i16max()::Int64 = Int64(typemax(Int16))
+i32max()::Int64 = Int64(typemax(Int32))
 
-pack16(i1::Integer, i2::Integer)::Integer = Int64(i1) + i16max*Int64(i2)
+pack16(i1::Int64, i2::Int64)::Int64 = Int64(i1) + i16max()*Int64(i2)
 
-function pack(i1::Integer, i2::Integer,i3::Integer,i4::Integer)::Integer
+function pack(i1::Int64, i2::Int64,i3::Int64,i4::Int64)::Int64
     @assert(i1 >= 0 && i1 <= typemax(Int16))
     @assert(i2 >= 0 && i2 <= typemax(Int16))
     @assert(i3 >= 0 && i3 <= typemax(Int16))
     @assert(i4 >= 0 && i4 <= typemax(Int16))
-    return pack16(i1,i2) + i32max*pack16(i3,i4)
+    return pack16(i1,i2) + i32max()*pack16(i3,i4)
 end
 
 
-function unpack16(i::Int64)
-    i1 = rem(i,i16max)
-    i2 = div(i-i1, i16max)
+function unpack16(i::Int64)::Tuple{Int64, Int64}
+    i1 = rem(i,i16max())
+    i2 = div(i-i1, i16max())
     return (i1,i2)
 end
 
 
-function unpack32(i::Int64)
-    i1 = rem(i,i32max)
-    i2 = div(i-i1, i32max)
+function unpack32(i::Int64)::Tuple{Int64, Int64}
+    i1 = rem(i,i32max())
+    i2 = div(i-i1, i32max())
     return (i1,i2)
 end
 
 
-function unpack(i::Int64)
+function unpack(i::Int64)::Tuple{Int64, Int64, Int64, Int64}
     @assert(i >= 0 && i <= typemax(Int64))
     (tmp1,tmp2) = unpack32(i)
     (i1,i2) = unpack16(tmp1)
@@ -41,14 +41,17 @@ end
 
 ### -------------------computation of pairID -----------------------------------
 ### it returns a unique ID
-function orderPositions(is,i,js,j)::Integer
+function orderPositions(is::Int64, i::Int64, js::Int64, j::Int64)::Int64
+    p = 0
     if is < js
-        return pack(is,i,js,j)
+        p = pack(is,i,js,j)
     elseif is > js
-        return pack(js,j,is,i)
+        p = pack(js,j,is,i)
     else
         error("from orderPositions: is == js.")
-end; end
+    end
+    return p
+end
 
 #getPositionsOfObj(scene::Composition.Scene, obj::Composition.Object3D,
 #                  movablePos::Nothing) = (false, 0, 0)
@@ -57,25 +60,29 @@ function getPositionsOfObj(scene::Composition.Scene,
     if movablePos == 0
         return (false, 0, 0)
     else
-        return (true, movablePos, findall(x->x==obj, scene.superObjs[movablePos].superObjMovable.superObj)[1] )
+        # findall(x->x==obj, scene.superObjs[movablePos].superObjMovable.superObj)[1] # needs to be improved
+        return (true, movablePos, 0 )
     end
 end
 
-function computePairID(scene::Composition.Scene,
-        actObj::Composition.Object3D, nextObj::Composition.Object3D,
-        is, i, js, j)::Integer
+function computePairID(scene::Composition.Scene{F},
+        actObj::Composition.Object3D{F}, nextObj::Composition.Object3D{F},
+        is::Int64, i::Int64, js::Int64, j::Int64)::Int64 where F
     # is: actual super - object
     # js: subsequent super - object
     # i: Object3D of is_th super - object
     # j: Object3D of js_th super - object
     (isSetAct, isPosActSuper, iPosActObj)    = getPositionsOfObj(scene, actObj,  actObj.interactionManner.movablePos)
     (isSetNext, jsPosNextSuper, jPosNextObj) = getPositionsOfObj(scene, nextObj, nextObj.interactionManner.movablePos)
+    pairID = 0
     if isSetAct && isSetNext
-        return orderPositions(isPosActSuper,iPosActObj,jsPosNextSuper,jPosNextObj)
+        pairID = orderPositions(isPosActSuper,iPosActObj,jsPosNextSuper,jPosNextObj)
     elseif isSetAct && !isSetNext
-        return orderPositions(isPosActSuper,iPosActObj,js,j)
+        pairID = orderPositions(isPosActSuper,iPosActObj,js,j)
     elseif isSetNext && !isSetAct
-        return orderPositions(is,i,jsPosNextSuper,jPosNextObj)
+        pairID = orderPositions(is,i,jsPosNextSuper,jPosNextObj)
     else
-        return orderPositions(is,i,js,j)
-end; end
+        pairID = orderPositions(is,i,js,j)
+    end
+    return pairID
+end