From bab4a046182456b8a3c2f98d49aa3ce23004efd1 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Mon, 2 Aug 2021 11:29:08 +0200 Subject: [PATCH 1/2] introduce flag visualizeBoundingBox --- src/Composition/handler.jl | 3 + src/Composition/object3D.jl | 56 ++++++++++--------- src/Composition/scene.jl | 17 +++++- .../ContactDetectionMPR/handler.jl | 3 +- test/Collision/BouncingBeams.jl | 2 +- test/Collision/BouncingCapsules.jl | 2 +- test/Collision/BouncingCones.jl | 2 +- test/Collision/BouncingEllipsoid.jl | 2 +- test/Collision/BouncingSphere.jl | 2 +- test/Collision/BouncingSphereFreeMotion.jl | 2 +- test/Collision/CollidingSphereWithBunnies.jl | 2 +- test/Collision/NewtonsCradle.jl | 3 +- test/Profiling/BouncingSphere_with_time.jl | 2 +- 13 files changed, 58 insertions(+), 40 deletions(-) diff --git a/src/Composition/handler.jl b/src/Composition/handler.jl index bf246258..7fbf9d18 100644 --- a/src/Composition/handler.jl +++ b/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 diff --git a/src/Composition/object3D.jl b/src/Composition/object3D.jl index bcc2a937..b27ef5f2 100644 --- a/src/Composition/object3D.jl +++ b/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,22 @@ 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.9)) # , lengthY , lengthZ + push!(world.AABBVisu, Object3D(world, feature, path = name) ) + + end + end + +end function addContactVisuObjToWorld!(world::Object3D, nVisualContSupPoints, defaultContactSphereDiameter) world.contactVisuObj1 = fill(Object3D(), nVisualContSupPoints) diff --git a/src/Composition/scene.jl b/src/Composition/scene.jl index feb16c93..95019314 100644 --- a/src/Composition/scene.jl +++ b/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` diff --git a/src/contactDetection/ContactDetectionMPR/handler.jl b/src/contactDetection/ContactDetectionMPR/handler.jl index 215c6f1b..961db352 100644 --- a/src/contactDetection/ContactDetectionMPR/handler.jl +++ b/src/contactDetection/ContactDetectionMPR/handler.jl @@ -17,7 +17,8 @@ AABB_touching(aabb1::Basics.BoundingBox, aabb2::Basics.BoundingBox) = 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 diff --git a/test/Collision/BouncingBeams.jl b/test/Collision/BouncingBeams.jl index 6a9db56c..3a89e30d 100644 --- a/test/Collision/BouncingBeams.jl +++ b/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))), diff --git a/test/Collision/BouncingCapsules.jl b/test/Collision/BouncingCapsules.jl index 7d879876..89cc9301 100644 --- a/test/Collision/BouncingCapsules.jl +++ b/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))), diff --git a/test/Collision/BouncingCones.jl b/test/Collision/BouncingCones.jl index 89b7e20d..75328a0a 100644 --- a/test/Collision/BouncingCones.jl +++ b/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")), diff --git a/test/Collision/BouncingEllipsoid.jl b/test/Collision/BouncingEllipsoid.jl index f94482a9..f2cbfb73 100644 --- a/test/Collision/BouncingEllipsoid.jl +++ b/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))), diff --git a/test/Collision/BouncingSphere.jl b/test/Collision/BouncingSphere.jl index 99af93be..28c44781 100644 --- a/test/Collision/BouncingSphere.jl +++ b/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))), diff --git a/test/Collision/BouncingSphereFreeMotion.jl b/test/Collision/BouncingSphereFreeMotion.jl index 2fe05f57..0b69e9c1 100644 --- a/test/Collision/BouncingSphereFreeMotion.jl +++ b/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], diff --git a/test/Collision/CollidingSphereWithBunnies.jl b/test/Collision/CollidingSphereWithBunnies.jl index bdeb8a67..0d7daac4 100644 --- a/test/Collision/CollidingSphereWithBunnies.jl +++ b/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", diff --git a/test/Collision/NewtonsCradle.jl b/test/Collision/NewtonsCradle.jl index 486a3753..d0739d99 100644 --- a/test/Collision/NewtonsCradle.jl +++ b/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)), diff --git a/test/Profiling/BouncingSphere_with_time.jl b/test/Profiling/BouncingSphere_with_time.jl index dfed1b08..38596b91 100644 --- a/test/Profiling/BouncingSphere_with_time.jl +++ b/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))), From a4d1560533bc314f5cb76a0e5b6473f8271c9c35 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Mon, 2 Aug 2021 13:43:10 +0200 Subject: [PATCH 2/2] visualizeBoundingBox in several test models --- src/Composition/object3D.jl | 3 +-- .../ContactDetectionMPR/handler.jl | 20 +++++++++++++++++-- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/Composition/object3D.jl b/src/Composition/object3D.jl index b27ef5f2..2bf4e5e7 100644 --- a/src/Composition/object3D.jl +++ b/src/Composition/object3D.jl @@ -427,9 +427,8 @@ function addAABBVisuToWorld!(world::Object3D, AABB::Array{Array{Basics.BoundingB 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.9)) # , lengthY , lengthZ + visualMaterial = Modia3D.VisualMaterial(color="grey96", transparency=0.8)) push!(world.AABBVisu, Object3D(world, feature, path = name) ) - end end diff --git a/src/contactDetection/ContactDetectionMPR/handler.jl b/src/contactDetection/ContactDetectionMPR/handler.jl index 961db352..f5dba30d 100644 --- a/src/contactDetection/ContactDetectionMPR/handler.jl +++ b/src/contactDetection/ContactDetectionMPR/handler.jl @@ -8,8 +8,7 @@ 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 @@ -110,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 @@ -250,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