Neumayr visualize aabb

src/Composition/handler.jl

@@ -392,6 +392,9 @@ function chooseAndBuildUpTree(world::Object3D, scene::Scene)
                 append!(scene.allVisuElements, world.supportVisuObj2B)
                 append!(scene.allVisuElements, world.supportVisuObj3B)
             end
+            if length(world.AABBVisu) > 0
+                append!(scene.allVisuElements, world.AABBVisu)
+            end
         end
         initializeMassComputation!(scene)
     else

src/Composition/object3D.jl

@@ -18,34 +18,16 @@ const emptyTwoObject3DObject = EmptyTwoObject3DObject()
 
 
 mutable struct InteractionManner
-    gripper::Bool                    # For objects which gripp movable objects
-    movable::Bool                    # For Object3Ds which can be gripped and displaced
-    lockable::Bool                   # For Object3Ds which lock movable Object3Ds to other Objs
-    movablePos::Union{Int64,Nothing} # each object which belongs to a movable super object, knows its order in the buffer
-    originPos::Int64  # each object knows its origin order in the buffer
-    actualPos::Int64  # each object knows its actual order in the buffer (for movable objs)
-
+    gripper::Bool
+    movable::Bool
+    lockable::Bool
+    movablePos::Union{Int64,Nothing}
+    originPos::Int64
+    actualPos::Int64
     InteractionManner() = new(false, false, false, nothing, 0, 0)
 
     function InteractionManner(interactionBehavior::InteractionBehavior)
-        if interactionBehavior == Modia3D.Gripper
-            gripper = true
-            movable = false
-            lockable = false
-        elseif interactionBehavior == Modia3D.Movable
-            gripper = false
-            movable = true
-            lockable = false
-        elseif interactionBehavior == Modia3D.Lockable
-            gripper = false
-            movable = false
-            lockable = true
-        elseif interactionBehavior == Modia3D.NoInteraction
-            gripper = false
-            movable = false
-            lockable = false
-        end
-        new(gripper, movable, lockable, nothing, 0, 0)
+        new(false, false, false, nothing, 0, 0)
     end
 end
 
@@ -95,7 +77,7 @@ mutable struct Object3D <: Modia3D.AbstractObject3D
     parent::Object3D                 # Parent Object3D (if parent===Object3D, no parent is yet defined)
     children::Vector{Object3D}       # All Object3Ds, where Object3D is the parent
     isRootObj::Bool                  # = true, if it is a root obj of a super obj
-    interactionManner::InteractionManner # stores interaction behavior for gripping
+    interactionManner::InteractionManner
 
     # Joint properties, defining the relative motion from parent to Object3D
     joint::Modia3D.AbstractJoint     # ::Fix, ::Revolute, ...
@@ -120,7 +102,6 @@ mutable struct Object3D <: Modia3D.AbstractObject3D
     # Mass properties.
     #   The root of each super object has potentially hasMass=true. All other Object3Ds have hasMass=false.
     #   The initial (fixed) mass properties defined in the ModiaLang model are stored in feature.
-    #   Mass properties might change at events (gripping process) - not yet available in the released Modia3D version.
     hasMass::Bool                 # = false, if m and I_CM are zero. = true, otherwise.
     m::Float64                    # Mass in [kg]
     r_CM::SVector{3,Float64}      # Position vector from Object3D to Center of Mass resolved in Object3D in [m]
@@ -148,6 +129,8 @@ mutable struct Object3D <: Modia3D.AbstractObject3D
     # additional 3D Shapes
     fileMeshConvexShapes::Vector{Object3D} # a graphical decomposition of a 3D mesh must be stored additionally
 
+    # if enabled, all AABBs are visualized and stored in world Object3D
+    AABBVisu::Vector{Object3D}
     # if enabled, contact and support points are set to world Object3D
     # for visualizing contact points (each contact has two points)
     contactVisuObj1::Vector{Object3D}
@@ -270,6 +253,7 @@ mutable struct Object3D <: Modia3D.AbstractObject3D
               false, false, false, false,
               shapeKind, shape, visualMaterial,
               visualizeFrame2,
+              Vector{Object3D}[],
               Vector{Object3D}[], Vector{Object3D}[],
               Vector{Object3D}[], Vector{Object3D}[], Vector{Object3D}[], Vector{Object3D}[],
               Vector{Object3D}[], Vector{Object3D}[], Vector{Object3D}[], Vector{Object3D}[],
@@ -315,6 +299,7 @@ mutable struct Object3D <: Modia3D.AbstractObject3D
         false, 0.0, Modia3D.ZeroVector3D, SMatrix{3,3,Float64,9}(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
         feature, Modia3D.AbstractTwoObject3DObject[],
         false, false, false, false, shapeKind, shape, visualMaterial, visualizeFrame,
+        Vector{Object3D}[],
         Vector{Object3D}[], Vector{Object3D}[],
         Vector{Object3D}[], Vector{Object3D}[], Vector{Object3D}[], Vector{Object3D}[],
         Vector{Object3D}[], Vector{Object3D}[], Vector{Object3D}[], Vector{Object3D}[],
@@ -375,6 +360,7 @@ function Object3DWithoutParent(obj::Object3D;
     obj.visualizeFrame     = typeof(visualizeFrame) == Modia3D.Ternary ? visualizeFrame : (visualizeFrame ? Modia3D.True : Modia3D.False)
     obj.visualizationFrame = Vector{Object3D}[]
     obj.fileMeshConvexShapes = Vector{Object3D}[]
+    obj.AABBVisu           = Vector{Object3D}[]
     obj.contactVisuObj1    = Vector{Object3D}[]
     obj.contactVisuObj2    = Vector{Object3D}[]
     obj.supportVisuObj1A   = Vector{Object3D}[]
@@ -432,6 +418,21 @@ function createFileFeature(feature::Shapes.Solid, fileMesh)
     return Modia3D.Solid(shape=fileMesh, massProperties=nothing, solidMaterial=feature.solidMaterial, collision=feature.collision, contactMaterial=feature.contactMaterial, collisionSmoothingRadius=feature.collisionSmoothingRadius, visualMaterial=feature.visualMaterial)
 end
 
+function addAABBVisuToWorld!(world::Object3D, AABB::Array{Array{Basics.BoundingBox}})
+    k = 0
+    @inbounds for i = 1:length(AABB)
+        for j = 1:length(AABB[i])
+            k = k + 1
+            name = String(Symbol(world.path, ".", "AABBVisu", "[", i, "][",j,"]"))
+            aabb = AABB[i][j]
+            feature = Modia3D.Visual(shape = Modia3D.Box(
+                    lengthX = abs(aabb.x_max - aabb.x_min), lengthY = abs(aabb.y_max - aabb.y_min), lengthZ = abs(aabb.z_max - aabb.z_min)),
+                visualMaterial = Modia3D.VisualMaterial(color="grey96", transparency=0.8))
+            push!(world.AABBVisu,  Object3D(world, feature, path = name) )
+        end
+    end
+
+end
 
 function addContactVisuObjToWorld!(world::Object3D, nVisualContSupPoints, defaultContactSphereDiameter)
     world.contactVisuObj1 = fill(Object3D(), nVisualContSupPoints)

src/Composition/scene.jl

@@ -34,8 +34,8 @@ mutable struct ContactPairs
     nz::Int                                         # length(z)
     nzContact::Int                                  # length(z | z has contact) length of z where zi has contact
 
-    function ContactPairs(world::Composition.Object3D, superObjs::Array{SuperObjsRow,1},
-                        allowedToMove::Array{Union{Bool,Nothing},1},  nVisualContSupPoints::Int,
+    function ContactPairs(world::Composition.Object3D, AABB::Array{Array{Basics.BoundingBox}}, superObjs::Array{SuperObjsRow,1},
+                        allowedToMove::Array{Union{Bool,Nothing},1}, visualizeBoundingBox::Bool, nVisualContSupPoints::Int,
                         visualizeContactPoints::Bool, visualizeSupportPoints::Bool, defaultContactSphereDiameter::Float64)
         @assert(length(superObjs) > 0)
         @assert(nVisualContSupPoints > 0)
@@ -62,6 +62,11 @@ mutable struct ContactPairs
             addSupportVisuObjToWorld!(world, nVisualContSupPoints, defaultContactSphereDiameter)
         end
 
+        if visualizeBoundingBox
+            addAABBVisuToWorld!(world, AABB)
+        end
+
+
         new(allowedToMove, dummyObject3D, lengthCollSuperObjs, nz, nzContact)
     end
 end
@@ -266,7 +271,7 @@ struct SceneOptions <: Modia3D.AbstractSceneOptions
     # Contact detection
     contactDetection::Modia3D.AbstractContactDetection
     nVisualContSupPoints::Int               # amount of visual contact or support points
-    gap::Float64                            # defines the gap between two lockable objects
+    gap::Float64
     enableContactDetection::Bool            # = true, if contact detection is enabled
     defaultContactSphereDiameter::Float64   # = true, if contact points are visualized
     elasticContactReductionFactor::Float64  # c_res_used = c_res * elasticContactReductionFactor (> 0)
@@ -290,6 +295,7 @@ struct SceneOptions <: Modia3D.AbstractSceneOptions
     visualizeGravity::Bool                # = true, if gravity field shall be visualized (acceleration vector or field center)
     visualizeFrames::Bool                 # = true, if all frames shall be visualized
     visualizeConvexHulls::Bool            # = true, if convex hulls (used for contact detection) shall be visualized
+    visualizeBoundingBox::Bool            # = true, if AABB's are visualized
     visualizeContactPoints::Bool          # = true, if contact points shall be visualized
     visualizeSupportPoints::Bool          # = true, if support points shall be visualized
     cameraDistance::Float64               # Distance between world frame and camera position
@@ -323,6 +329,7 @@ struct SceneOptions <: Modia3D.AbstractSceneOptions
                            visualizeGravity              = true,
                            visualizeFrames               = false,
                            visualizeConvexHulls          = true,
+                           visualizeBoundingBox          = false,
                            visualizeContactPoints        = false,
                            visualizeSupportPoints        = false,
                            cameraDistance                = 10.0*nominalLength,
@@ -371,6 +378,7 @@ struct SceneOptions <: Modia3D.AbstractSceneOptions
             visualizeGravity,
             visualizeFrames,
             visualizeConvexHulls,
+            visualizeBoundingBox,
             visualizeContactPoints,
             visualizeSupportPoints,
             cameraDistance,
@@ -415,6 +423,7 @@ Defines global properties of the system, such as the gravity field. Exactly one
 | `enableContactDetection`        | true                                    |
 | `contactDetection`              | [`ContactDetectionMPR_handler`](@ref)`()`|
 | `elasticContactReductionFactor` | 1.0                                     |
+| `visualizeBoundingBox`          | false                                   |
 | `visualizeContactPoints`        | false                                   |
 | `visualizeSupportPoints`        | false                                   |
 | `nVisualContSupPoints`          | 5                                       |
@@ -451,6 +460,8 @@ Defines global properties of the system, such as the gravity field. Exactly one
 - `elasticContactReductionFactor::Float64`: (> 0.0)
   - ``usedContactCompliance = contactCompliance * elasticContactReductionFactor``
 
+- `visualizeBoundingBox::Bool`: Flag enabled for visualizing Axis Aligned Bounding Box (AABB) for all solid shapes allowed to collide
+
 - `visualizeContactPoints::Bool`: Flag enabled for visualizing contact points, and `enableVisualization = true`
 
 - `visualizeSupportPoints::Bool`: Flag enabled for visualizing support points, and `enableVisualization = true`

src/contactDetection/ContactDetectionMPR/handler.jl

@@ -8,16 +8,16 @@ using LinearAlgebra
 EYE3() = Matrix(1.0I,3,3)
 
 # returns true, if AABB's are touching
-AABB_touching(aabb1::Basics.BoundingBox, aabb2::Basics.BoundingBox) =
-    aabb1.x_max >= aabb2.x_min && aabb1.x_min <= aabb2.x_max &&
+AABB_touching(aabb1::Basics.BoundingBox, aabb2::Basics.BoundingBox) = aabb1.x_max >= aabb2.x_min && aabb1.x_min <= aabb2.x_max &&
     aabb1.y_max >= aabb2.y_min && aabb1.y_min <= aabb2.y_max &&
     aabb1.z_max >= aabb2.z_min && aabb1.z_min <= aabb2.z_max
 
 
 
 function Composition.initializeContactDetection!(world::Composition.Object3D, scene::Composition.Scene)
     ch = scene.options.contactDetection
-    ch.contactPairs = Composition.ContactPairs(world,  scene.superObjs, scene.allowedToMove,
+    ch.contactPairs = Composition.ContactPairs(world, scene.AABB, scene.superObjs, scene.allowedToMove,
+        scene.options.visualizeBoundingBox,
         scene.options.nVisualContSupPoints, ch.visualizeContactPoints,
         ch.visualizeSupportPoints, ch.defaultContactSphereDiameter)
     if ch.contactPairs.nz == 0
@@ -109,12 +109,17 @@ function computeDistances(scene::Composition.Scene, ch::Composition.ContactDetec
     superObjs = scene.superObjs
     AABB = scene.AABB
     if length(superObjs) > 1
+        k = 0
         @inbounds for i = 1:length(superObjs)
             superObj = superObjs[i].superObjCollision.superObj
             for j = 1:length(superObj)
+                k = k +1
                 obj = superObj[j]
                 AABB[i][j] = Modia3D.boundingBox!(obj,
                     AABB[i][j]; tight=false, scaleFactor=0.01)
+                if scene.options.visualizeBoundingBox
+                    updateVisualBoundingBox!(world.AABBVisu[k], AABB[i][j])
+                end
             end
         end
 
@@ -249,3 +254,15 @@ function Composition.closeContactDetection!(ch::Composition.ContactDetectionMPR_
     empty!(ch.contactDict)
     ch.noContactMinVal =  42.0
 end
+
+
+function updateVisualBoundingBox!(obj::Composition.Object3D, aabb::Basics.BoundingBox)
+    box::Shapes.Box = obj.shape
+    box.lengthX = abs(aabb.x_max - aabb.x_min)
+    box.lengthY = abs(aabb.y_max - aabb.y_min)
+    box.lengthZ = abs(aabb.z_max - aabb.z_min)
+
+    obj.r_abs = SVector((aabb.x_max + aabb.x_min)/2.0,
+                        (aabb.y_max + aabb.y_min)/2.0,
+                        (aabb.z_max + aabb.z_min)/2.0)
+end

test/Collision/BouncingBeams.jl

@@ -15,7 +15,7 @@ BouncingBeams = Model(
     world = Object3D(feature=Scene(gravityField=UniformGravityField(g=9.81, n=[0, 0, -1]),
                                    visualizeFrames=false,
                                    defaultFrameLength=0.2,
-                                   gap=0.2,
+                                   visualizeBoundingBox = true,
                                    enableContactDetection=true,
                                    visualizeContactPoints=false)),
     worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.5))),

test/Collision/BouncingCapsules.jl

@@ -16,7 +16,7 @@ BouncingCapsules = Model(
     world = Object3D(feature=Scene(gravityField=:gravField,
                                    visualizeFrames=false,
                                    defaultFrameLength=0.2,
-                                   gap=0.2,
+                                   visualizeBoundingBox = true,
                                    enableContactDetection=true,
                                    visualizeContactPoints=false)),
     worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.5))),

test/Collision/BouncingCones.jl

@@ -16,7 +16,7 @@ BouncingCones = Model(
     world = Object3D(feature=Scene(gravityField=:gravField,
                                    visualizeFrames=false,
                                    defaultFrameLength=0.2,
-                                   gap=0.2,
+                                   visualizeBoundingBox = true,
                                    enableContactDetection=true,
                                    visualizeContactPoints=false,
                                    animationFile="BouncingCones.json")),

test/Collision/BouncingEllipsoid.jl

@@ -10,7 +10,7 @@ BouncingEllipsoid = Model(
     world = Object3D(feature=Scene(gravityField=:gravField,
                                    visualizeFrames=false,
                                    defaultFrameLength=0.2,
-                                   gap=0.2,
+                                   visualizeBoundingBox = true,
                                    enableContactDetection=true,
                                    visualizeContactPoints=false)),
     worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.5))),

test/Collision/BouncingSphere.jl

@@ -11,7 +11,7 @@ BouncingSphere = Model(
     world = Object3D(feature=Scene(gravityField=:gravField,
                                           visualizeFrames=false,
                                           defaultFrameLength=0.2,
-                                          gap=0.2,
+                                          visualizeBoundingBox = true,
                                           enableContactDetection=true,
                                           visualizeContactPoints=false)),
     worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.5))),

test/Collision/BouncingSphereFreeMotion.jl

@@ -10,7 +10,7 @@ BouncingSphere = Model(
     sphereMaterial = VisualMaterial(color="Red"),
     gravField = UniformGravityField(g=9.81, n=[0, -1, 0]),
     world = Object3D(feature=Scene(gravityField=:gravField,
-                                   visualizeFrames=false, defaultFrameLength=0.2, gap=0.2,
+                                   visualizeFrames=false, defaultFrameLength=0.2, visualizeBoundingBox = true,
                                    enableContactDetection=true, visualizeContactPoints=false)),
     worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.5))),
     ground = Object3D(parent=:world, translation=:[0.0, -boxHeigth/2, 0.0],

test/Collision/CollidingSphereWithBunnies.jl

@@ -14,7 +14,7 @@ massAndGeoSphere = MassPropertiesFromShape()
 # Objects3D
 ConvexPartitions = Model(
     gravField = UniformGravityField(g=9.81, n=[0, -1, 0]),
-    world = Object3D(feature=Scene(gravityField=:gravField)),
+    world = Object3D(feature=Scene(gravityField=:gravField, visualizeBoundingBox = true)),
 
     sphere = Object3D(feature=Solid(shape=Sphere(diameter=0.2),
                       visualMaterial=mat1, solidMaterial="Steel",

test/Collision/NewtonsCradle.jl

@@ -59,7 +59,8 @@ Cradle = Model(
                                    nominalLength=Ly,
                                    enableContactDetection=true,
                                    elasticContactReductionFactor=1.0,
-                                   animationFile="NewtonsCradle.json")),
+                                   animationFile="NewtonsCradle.json",
+                                   visualizeBoundingBox=true)),
     box = Object3D(parent=:world,
                    feature=Visual(shape=Box(lengthX=Lx, lengthY=Ly, lengthZ=Lz),
                                   visualMaterial=vmatVisual)),

test/Profiling/BouncingSphere_with_time.jl

@@ -11,7 +11,7 @@ using  Modia3D.ModiaInterface
         world = Object3D(feature=Scene(gravityField=:gravField,
                                             visualizeFrames=false,
                                             defaultFrameLength=0.2,
-                                            gap=0.2,
+                                            visualizeBoundingBox = true,
                                             enableContactDetection=true,
                                             visualizeContactPoints=false)),
         worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.5))),