Fix start index of collision dectection zero crossing function

Author: GerhardHippmann

src/Composition/dynamics.jl

@@ -93,7 +93,7 @@ struct MultibodyData{FloatType}
     jointObjects::Vector{Object3D}  # References to Object3Ds that have a joint
     jointStartIndex::Vector{Int}    # Start index of joint in qdd
     jointNdof::Vector{Int}          # Number-of-degrees-of-freedom of joint
-    zStartIndex::Int                # eventHandler.z[zStartIndex] is first index of crossing function
+    zStartIndex::Int                # eventHandler.z[zStartIndex] is first index of crossing functions for contact detection
                                     # (or zero, if nableContactDetection=false)
     nz::Int                         # Number of used zero crossing functions
     residuals::Vector{FloatType}    # Residuals - length(residuals) = nqdd
@@ -141,6 +141,7 @@ function setModiaJointVariables!(id::Int, _leq_mode, instantiatedModel::ModiaLan
             if scene.options.enableContactDetection
                 nz = 2
                 zStartIndex = ModiaLang.addZeroCrossings(instantiatedModel, nz)
+                scene.zStartIndex = zStartIndex
             else
                 nz = 0
                 zStartIndex = 0

src/Composition/scene.jl

@@ -514,7 +514,8 @@ mutable struct Scene
     noCPairs::Vector{Vector{Int64}}           # Indices of frames (with respect to collSuperObjs) that can't collide in general (e.g. objects are connected via joints)
     noCPairsHelp::Dict{Modia3D.AbstractJoint,Vector{Int64}}
     allowedToMove::Vector{Union{Bool,Nothing}}
-    AABB::Vector{Vector{Basics.BoundingBox}}    # Bounding boxes of elements that can collide
+    AABB::Vector{Vector{Basics.BoundingBox}}  # Bounding boxes of elements that can collide
+    zStartIndex::Int                          # start index of collision zero crossing functions
     #forceElements::Vector{Int64}
     exportAnimation::Bool                     # animation file export is enabled
     animation::Vector{animationStep}          # animation data of visible Object3Ds
@@ -557,10 +558,10 @@ mutable struct Scene
             Dict{Modia3D.AbstractJoint,Vector{Int64}}(),
             Vector{Union{Bool}}[],
             Vector{Vector{Basics.BoundingBox}}[],
+            1,
             exportAnimation,
             Vector{animationStep}[],
             0,
-
             Revolute[],
             Prismatic[],
             FreeMotion[])

src/contactDetection/ContactDetectionMPR/handler.jl

@@ -79,14 +79,14 @@ function selectContactPairs!(sim, scene::Composition.Scene, ch::Composition.Cont
         # z[1] ... zero crossing function from contact to no contact
         # max. distance (= min. penetration) of active contact pairs
         if isempty(ch.contactDict)
-            @inbounds simh.z[1] = -42.0
+            @inbounds simh.z[scene.zStartIndex] = -42.0
         else
             (pair, key)  = findmax(ch.contactDict)
-            @inbounds simh.z[1] = pair.distanceWithHysteresis
+            @inbounds simh.z[scene.zStartIndex] = pair.distanceWithHysteresis
         end
         # z[2] ... zero crossing function from no contact to contact
         # min. distance of inactive contact pairs
-        @inbounds simh.z[2] = ch.noContactMinVal
+        @inbounds simh.z[1+scene.zStartIndex] = ch.noContactMinVal
     end
     ch.distanceComputed = true
     return nothing