diff --git a/src/Basics/_module.jl b/src/Basics/_module.jl index 4c7a6d64..bcaa47b4 100644 --- a/src/Basics/_module.jl +++ b/src/Basics/_module.jl @@ -13,6 +13,8 @@ Andrea Neumayr and Martin Otter, [DLR - Institute of System Dynamics and Control """ module Basics +import Modia3D + export trailingPartOfName export neps, sign_eps, radToDeg diff --git a/src/Basics/constantsAndFunctions.jl b/src/Basics/constantsAndFunctions.jl index 29d58e75..a7041de2 100644 --- a/src/Basics/constantsAndFunctions.jl +++ b/src/Basics/constantsAndFunctions.jl @@ -7,34 +7,37 @@ # Epsilon and sign -const neps = sqrt( eps() ) -sign_eps(value::Float64; seps::Float64 = 100*neps)::Float64 = value > seps ? 1.0 : (value < -seps ? -1.0 : 0.0) - -function normalizeVector(n::SVector{3,Float64})::SVector{3,Float64} - nabs = norm(n) - if nabs <= neps - println("neps ", neps) - println("nabs ", nabs) - @assert(nabs > neps) # && norm(vec) > eps() - # return nothing - else +const neps = sqrt( eps(Modia3D.MPRFloatType) ) + +function sign_eps(value::T; ) where {T} + seps::T = 100.0*neps + return value > seps ? T(1.0) : (value < -seps ? T(-1.0) : T(0.0)) +end + +function normalizeVector(n::SVector{3,T}) where {T} + nabs = norm(n) + if nabs <= neps + println("neps ", neps) + println("nabs ", nabs) + @assert(nabs > neps) # && norm(vec) > eps() + # return nothing + end return n/nabs - end end # Standard constants -const radToDeg = 180.0/pi +const radToDeg = 180.0/pi """ mutable struct BoundingBox - Smallest box that contains a visual element""" mutable struct BoundingBox - x_min::Float64 - x_max::Float64 - y_min::Float64 - y_max::Float64 - z_min::Float64 - z_max::Float64 - BoundingBox() = new(0.0,0.0,0.0,0.0,0.0,0.0) - BoundingBox(x_min,x_max,y_min,y_max,z_min,z_max) = new(x_min,x_max,y_min,y_max,z_min,z_max) + x_min::Float64 + x_max::Float64 + y_min::Float64 + y_max::Float64 + z_min::Float64 + z_max::Float64 + BoundingBox() = new(0.0,0.0,0.0,0.0,0.0,0.0) + BoundingBox(x_min,x_max,y_min,y_max,z_min,z_max) = new(x_min,x_max,y_min,y_max,z_min,z_max) end @@ -44,17 +47,17 @@ linearMovement(delta_x, tStart, tEnd, time) = delta_x*(time-tStart)/(tEnd-tStart # Trailing part of type name function trailingPartOfTypeAsString(obj)::String - name = string( typeof(obj) ) + name = string( typeof(obj) ) - # Determine trailing solid (after last ".") - i = first(something(findlast(".", name), 0:-1)) - return i > 0 && i < length(name) ? name[i+1:end] : name + # Determine trailing solid (after last ".") + i = first(something(findlast(".", name), 0:-1)) + return i > 0 && i < length(name) ? name[i+1:end] : name end function trailingPartOfName(name::AbstractString)::String - # Determine trailing part of name (after last ".") - i = first(something(findlast(".", name), 0:-1)) - return i > 0 && i < length(name) ? name[i+1:end] : name + # Determine trailing part of name (after last ".") + i = first(something(findlast(".", name), 0:-1)) + return i > 0 && i < length(name) ? name[i+1:end] : name end diff --git a/src/Composition/supportPoints.jl b/src/Composition/supportPoints.jl index 32039fc2..a85df548 100644 --- a/src/Composition/supportPoints.jl +++ b/src/Composition/supportPoints.jl @@ -1,32 +1,34 @@ -function supportPoint(obj::Composition.Object3D, e::SVector{3,Float64}) +function supportPoint(obj::Composition.Object3D, e::SVector{3,T}) where {T} shapeKind = obj.shapeKind solid::Modia3D.Solid = obj.feature - collisionSmoothingRadius = solid.collisionSmoothingRadius + collisionSmoothingRadius = T(solid.collisionSmoothingRadius) + obj_r_abs = SVector{3, T}(obj.r_abs) + obj_R_abs = SMatrix{3,3,T,9}(obj.R_abs) if shapeKind == Modia3D.SphereKind #sphere::Modia3D.Sphere = obj.shape - return Modia3D.supportPoint_Sphere(obj.shape, obj.r_abs, obj.R_abs, e) + return Modia3D.supportPoint_Sphere(obj.shape, obj_r_abs, obj_R_abs, e) elseif shapeKind == Modia3D.EllipsoidKind #ellipsoid::Modia3D.Ellipsoid = obj.shape - return Modia3D.supportPoint_Ellipsoid(obj.shape, obj.r_abs, obj.R_abs, e) + return Modia3D.supportPoint_Ellipsoid(obj.shape, obj_r_abs, obj_R_abs, e) elseif shapeKind == Modia3D.BoxKind #box::Modia3D.Box = obj.shape - return Modia3D.supportPoint_Box(obj.shape, obj.r_abs, obj.R_abs, e, collisionSmoothingRadius) + return Modia3D.supportPoint_Box(obj.shape, obj_r_abs, obj_R_abs, e, collisionSmoothingRadius) elseif shapeKind == Modia3D.CylinderKind #cylinder::Modia3D.Cylinder = obj.shape - return Modia3D.supportPoint_Cylinder(obj.shape, obj.r_abs, obj.R_abs, e, collisionSmoothingRadius) + return Modia3D.supportPoint_Cylinder(obj.shape, obj_r_abs, obj_R_abs, e, collisionSmoothingRadius) elseif shapeKind == Modia3D.ConeKind #cone::Modia3D.Cone = obj.shape - return Modia3D.supportPoint_Cone(obj.shape, obj.r_abs, obj.R_abs, e, collisionSmoothingRadius) + return Modia3D.supportPoint_Cone(obj.shape, obj_r_abs, obj_R_abs, e, collisionSmoothingRadius) elseif shapeKind == Modia3D.CapsuleKind #capsule::Modia3D.Capsule = obj.shape - return Modia3D.supportPoint_Capsule(obj.shape, obj.r_abs, obj.R_abs, e) + return Modia3D.supportPoint_Capsule(obj.shape, obj_r_abs, obj_R_abs, e) elseif shapeKind == Modia3D.BeamKind #beam::Modia3D.Beam = obj.shape - return Modia3D.supportPoint_Beam(obj.shape, obj.r_abs, obj.R_abs, e, collisionSmoothingRadius) + return Modia3D.supportPoint_Beam(obj.shape, obj_r_abs, obj_R_abs, e, collisionSmoothingRadius) elseif shapeKind == Modia3D.FileMeshKind #fileMesh::Modia3D.FileMesh = obj.shape - return Modia3D.supportPoint_FileMesh(obj.shape, obj.r_abs, obj.R_abs, e) + return Modia3D.supportPoint_FileMesh(obj.shape, obj_r_abs, obj_R_abs, e) else error("not supported shape for support points") end diff --git a/src/Modia3D.jl b/src/Modia3D.jl index a210d853..1e0a3a05 100644 --- a/src/Modia3D.jl +++ b/src/Modia3D.jl @@ -94,7 +94,8 @@ convertAndStripUnit(TargetType, requiredUnit, value) = numberType(value) <: Unitful.AbstractQuantity && unit.(value) != Unitful.NoUnits ? convert(TargetType, ustrip.( uconvert.(requiredUnit, value))) : convert(TargetType, value) - +# MPRFloatType is used to change betweeen Double64 and Float64 for mpr calculations +const MPRFloatType = Float64 # Include sub-modules include(joinpath("Frames" , "_module.jl")) diff --git a/src/Shapes/boundingBoxes.jl b/src/Shapes/boundingBoxes.jl index 3d77b4bc..8715f013 100644 --- a/src/Shapes/boundingBoxes.jl +++ b/src/Shapes/boundingBoxes.jl @@ -17,28 +17,28 @@ that is the most extreme in direction of unit vector `e`. - `R_abs::AbstractMatrix`: Rotation matrix to rotate world frame in `shape` reference frame. - `e::AbstractVector`: Unit vector pointing into the desired direction. """ -@inline supportPoint_Box(shape::Box, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}, collisionSmoothingRadius::Float64) = +@inline supportPoint_Box(shape::Box, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}, collisionSmoothingRadius::T) where {T} = r_abs + R_abs'*supportPoint_abs_Box(shape, R_abs*e, collisionSmoothingRadius) + e*collisionSmoothingRadius # -@inline supportPoint_Cylinder( shape::Cylinder, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}, collisionSmoothingRadius::Float64) = +@inline supportPoint_Cylinder( shape::Cylinder, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}, collisionSmoothingRadius::T) where {T} = r_abs + R_abs'*supportPoint_abs_Cylinder(shape, R_abs*e) + e*collisionSmoothingRadius -@inline supportPoint_Cone(shape::Cone, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}, collisionSmoothingRadius::Float64) = +@inline supportPoint_Cone(shape::Cone, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}, collisionSmoothingRadius::T) where {T} = r_abs + R_abs'*supportPoint_abs_Cone(shape, R_abs*e) + e*collisionSmoothingRadius -@inline supportPoint_Capsule(shape::Capsule, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}) = +@inline supportPoint_Capsule(shape::Capsule, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}) where {T} = r_abs + R_abs'*supportPoint_abs_Capsule(shape, R_abs*e) -@inline supportPoint_Beam(shape::Beam, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}, collisionSmoothingRadius::Float64) = +@inline supportPoint_Beam(shape::Beam, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}, collisionSmoothingRadius::T) where {T} = r_abs + R_abs'*supportPoint_abs_Beam(shape, R_abs*e) + e*collisionSmoothingRadius -@inline supportPoint_Sphere(shape::Sphere, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}) = +@inline supportPoint_Sphere(shape::Sphere, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}) where {T} = r_abs + (shape.diameter/2)*e -@inline supportPoint_Ellipsoid(shape::Ellipsoid, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}) = +@inline supportPoint_Ellipsoid(shape::Ellipsoid, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}) where {T} = r_abs + R_abs'*supportPoint_abs_Ellipsoid(shape, R_abs*e) -@inline supportPoint_FileMesh(shape::FileMesh, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}) = +@inline supportPoint_FileMesh(shape::FileMesh, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}) where {T} = r_abs + R_abs'*supportPoint_abs_FileMesh(shape, R_abs*e) @@ -49,78 +49,98 @@ that is the most extreme in direction of unit vector `e`. # or Andrea Neumayr took ideas for computing based on those books # [Gino v.d. Bergen, p. 135] -@inline supportPoint_abs_Ellipsoid(shape::Ellipsoid, e_abs::SVector{3,Float64}) = - @inbounds SVector( (shape.lengthX/2)^2*e_abs[1], (shape.lengthY/2)^2*e_abs[2], (shape.lengthZ/2)^2*e_abs[3] )/norm(SVector((shape.lengthX/2)*e_abs[1], (shape.lengthY/2)*e_abs[2], (shape.lengthZ/2)*e_abs[3]) ) +@inline function supportPoint_abs_Ellipsoid(shape::Ellipsoid, e_abs::SVector{3,T}) where {T} + @inbounds begin + halfLengthX = T(0.5*shape.lengthX) + halfLengthY = T(0.5*shape.lengthY) + halfLengthZ = T(0.5*shape.lengthZ) + return SVector{3,T}( (halfLengthX)^2*e_abs[1], (halfLengthY)^2*e_abs[2], (halfLengthZ)^2*e_abs[3] )/norm(SVector{3,T}((halfLengthX)*e_abs[1], (halfLengthY)*e_abs[2], (halfLengthZ)*e_abs[3]) ) + end +end # [Gino v.d. Bergen, p. 135] -@inline supportPoint_abs_Box(shape::Box, e_abs::SVector{3,Float64}, collisionSmoothingRadius::Float64) = - @inbounds SVector( Basics.sign_eps(e_abs[1])*(shape.lengthX/2-collisionSmoothingRadius), Basics.sign_eps(e_abs[2])*(shape.lengthY/2-collisionSmoothingRadius), Basics.sign_eps(e_abs[3])*(shape.lengthZ/2-collisionSmoothingRadius) ) +@inline function supportPoint_abs_Box(shape::Box, e_abs::SVector{3,T}, collisionSmoothingRadius::T) where {T} + @inbounds begin + halfLengthX = T(0.5*shape.lengthX) + halfLengthY = T(0.5*shape.lengthY) + halfLengthZ = T(0.5*shape.lengthZ) + return SVector{3,T}( + Basics.sign_eps(e_abs[1])*(halfLengthX-collisionSmoothingRadius), + Basics.sign_eps(e_abs[2])*(halfLengthY-collisionSmoothingRadius), + Basics.sign_eps(e_abs[3])*(halfLengthZ-collisionSmoothingRadius) ) + end +end # [Gino v.d. Bergen, p. 136, XenoCollide, p. 168, 169] -@inline function supportPoint_abs_Cylinder(shape::Cylinder, e_abs::SVector{3,Float64}) +@inline function supportPoint_abs_Cylinder(shape::Cylinder, e_abs::SVector{3,T}) where {T} @inbounds begin + halfLength = T(0.5*shape.length) + halfDiameter = T(0.5*shape.diameter) if shape.axis == 1 enorm = norm(SVector(e_abs[2], e_abs[3])) if enorm <= Modia3D.neps - return Basics.sign_eps(e_abs[1])*SVector(shape.length/2, 0.0, 0.0) + return Basics.sign_eps(e_abs[1])*SVector(halfLength, 0.0, 0.0) else - return Basics.sign_eps(e_abs[1])*SVector(shape.length/2, 0.0, 0.0) + SVector(0.0, (shape.diameter/2)*e_abs[2], (shape.diameter/2)*e_abs[3])/enorm + return Basics.sign_eps(e_abs[1])*SVector(halfLength, 0.0, 0.0) + SVector(0.0, (halfDiameter)*e_abs[2], (halfDiameter)*e_abs[3])/enorm end elseif shape.axis == 2 enorm = norm(SVector(e_abs[3], e_abs[1])) if enorm <= Modia3D.neps - return Basics.sign_eps(e_abs[2])*SVector(0.0, shape.length/2, 0.0) + return Basics.sign_eps(e_abs[2])*SVector(0.0, halfLength, 0.0) else - return Basics.sign_eps(e_abs[2])*SVector(0.0, shape.length/2, 0.0) + SVector((shape.diameter/2)*e_abs[1], 0.0, (shape.diameter/2)*e_abs[3])/enorm + return Basics.sign_eps(e_abs[2])*SVector(0.0, halfLength, 0.0) + SVector((halfDiameter)*e_abs[1], 0.0, (halfDiameter)*e_abs[3])/enorm end else enorm = norm(SVector(e_abs[1], e_abs[2])) if enorm <= Modia3D.neps - return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, shape.length/2) + return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, halfLength) else - return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, shape.length/2) + SVector((shape.diameter/2)*e_abs[1], (shape.diameter/2)*e_abs[2], 0.0)/enorm + return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, halfLength) + SVector((halfDiameter)*e_abs[1], (halfDiameter)*e_abs[2], 0.0)/enorm end end end end # G. Hippmann: Cylinder + Sphere as bottom and top -@inline function supportPoint_abs_Capsule(shape::Capsule, e_abs::SVector{3,Float64}) +@inline function supportPoint_abs_Capsule(shape::Capsule, e_abs::SVector{3,T}) where {T} @inbounds begin + halfLength = T(0.5*shape.length) + halfDiameter = T(0.5*shape.diameter) if shape.axis == 1 - return Basics.sign_eps(e_abs[1])*SVector(0.5*shape.length, 0.0, 0.0) + 0.5*shape.diameter*e_abs + return Basics.sign_eps(e_abs[1])*SVector(halfLength, 0.0, 0.0) + halfDiameter*e_abs elseif shape.axis == 2 - return Basics.sign_eps(e_abs[2])*SVector(0.0, 0.5*shape.length, 0.0) + 0.5*shape.diameter*e_abs + return Basics.sign_eps(e_abs[2])*SVector(0.0, halfLength, 0.0) + halfDiameter*e_abs else - return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, 0.5*shape.length) + 0.5*shape.diameter*e_abs + return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, halfLength) + halfDiameter*e_abs end end end # for cone: [Gino v.d. Bergen, p. 136]] # for frustum of a cone: A. Neumayr, G. Hippmann -@inline function supportPoint_abs_Cone(shape::Cone, e_abs::SVector{3,Float64}) +@inline function supportPoint_abs_Cone(shape::Cone, e_abs::SVector{3,T}) where {T} @inbounds begin - baseRadius = shape.diameter/2 - rightCone = shape.topDiameter == 0.0 + baseRadius = T(0.5*shape.diameter) + rightCone = T(shape.topDiameter) == T(0.0) + shapeLength = T(shape.length) if rightCone - sin_phi = baseRadius/sqrt(baseRadius^2 + shape.length^2) # sin of base angle + sin_phi = T(baseRadius/sqrt(baseRadius^2 + shapeLength^2)) # sin of base angle else - topRadius = shape.topDiameter/2 - diffRadius = baseRadius - topRadius - sin_phi = diffRadius/sqrt(diffRadius^2 + shape.length^2) # sin of base angle + topRadius = T(0.5*shape.topDiameter) + diffRadius = T(baseRadius - topRadius) + sin_phi = T(diffRadius/sqrt(diffRadius^2 + shapeLength^2)) # sin of base angle end if shape.axis == 1 value = e_abs[1] / norm(SVector(e_abs[1], e_abs[2], e_abs[3])) if value >= sin_phi if rightCone - return SVector(shape.length, 0.0, 0.0) # apex is support point + return SVector(shapeLength, 0.0, 0.0) # apex is support point else # frustum of a cone enorm = norm(SVector(e_abs[2], e_abs[3])) if enorm > Modia3D.neps - return SVector(shape.length, 0.0, 0.0) + SVector(0.0, topRadius*e_abs[2], topRadius*e_abs[3]) / enorm # point on top circle is support point + return SVector(shapeLength, 0.0, 0.0) + SVector(0.0, topRadius*e_abs[2], topRadius*e_abs[3]) / enorm # point on top circle is support point else - return SVector(shape.length, 0.0, 0.0) # top circle center is support point + return SVector(shapeLength, 0.0, 0.0) # top circle center is support point end end else @@ -128,20 +148,20 @@ end if enorm > Modia3D.neps return SVector(0.0, baseRadius*e_abs[2], baseRadius*e_abs[3]) / enorm # point on base circle is support point else - return SVector(0.0, 0.0, 0.0) # base circle center is support point + return SVector{3,T}(0.0, 0.0, 0.0) # base circle center is support point end end elseif shape.axis == 2 value = e_abs[2] / norm(SVector(e_abs[1], e_abs[2], e_abs[3])) if value >= sin_phi if rightCone - return SVector(0.0, shape.length, 0.0) # apex is support point + return SVector(0.0, shapeLength, 0.0) # apex is support point else # frustum of a cone enorm = norm(SVector(e_abs[3], e_abs[1])) if enorm > Modia3D.neps - return SVector(0.0, shape.length, 0.0) + SVector(topRadius*e_abs[1], 0.0, topRadius*e_abs[3]) / enorm # point on top circle is support point + return SVector(0.0, shapeLength, 0.0) + SVector(topRadius*e_abs[1], 0.0, topRadius*e_abs[3]) / enorm # point on top circle is support point else - return SVector(0.0, shape.length, 0.0) # top circle center is support point + return SVector(0.0, shapeLength, 0.0) # top circle center is support point end end else @@ -149,20 +169,20 @@ end if enorm > Modia3D.neps return SVector(baseRadius*e_abs[1], 0.0, baseRadius*e_abs[3]) / enorm # point on base circle is support point else - return SVector(0.0, 0.0, 0.0) # base circle center is support point + return SVector{3,T}(0.0, 0.0, 0.0) # base circle center is support point end end else value = e_abs[3] / norm(SVector(e_abs[1], e_abs[2], e_abs[3])) if value >= sin_phi if rightCone - return SVector(0.0, 0.0, shape.length) # apex is support point + return SVector(0.0, 0.0, shapeLength) # apex is support point else # frustum of a cone enorm = norm(SVector(e_abs[1], e_abs[2])) if enorm > Modia3D.neps - return SVector(0.0, 0.0, shape.length) + SVector(topRadius*e_abs[1], topRadius*e_abs[2], 0.0) / enorm # point on top circle is support point + return SVector(0.0, 0.0, shapeLength) + SVector(topRadius*e_abs[1], topRadius*e_abs[2], 0.0) / enorm # point on top circle is support point else - return SVector(0.0, 0.0, shape.length) # top circle center is support point + return SVector(0.0, 0.0, shapeLength) # top circle center is support point end end else @@ -170,7 +190,7 @@ end if enorm > Modia3D.neps return SVector(baseRadius*e_abs[1], baseRadius*e_abs[2], 0.0) / enorm # point on base circle is support point else - return SVector(0.0, 0.0, 0.0) # base circle center is support point + return SVector{3,T}(0.0, 0.0, 0.0) # base circle center is support point end end end @@ -178,39 +198,42 @@ end end # G. Hippmann: Outer half circles of beam -@inline function supportPoint_abs_Beam(shape::Beam, e_abs::SVector{3,Float64}) +@inline function supportPoint_abs_Beam(shape::Beam, e_abs::SVector{3,T}) where {T} @inbounds begin + halfLength = T(0.5*shape.length) + halfWidth = T(0.5*shape.width) + halfThickness = T(0.5*shape.thickness) if shape.axis == 1 enorm = norm(SVector(e_abs[1], e_abs[2])) if enorm <= Modia3D.neps - return SVector(Basics.sign_eps(e_abs[1])*shape.length/2, 0.0, Basics.sign_eps(e_abs[3])*shape.thickness/2) + return SVector(Basics.sign_eps(e_abs[1])*halfLength, 0.0, Basics.sign_eps(e_abs[3])*halfThickness) else - return SVector(Basics.sign_eps(e_abs[1])*shape.length/2, 0.0, Basics.sign_eps(e_abs[3])*shape.thickness/2) + SVector(shape.width/2*e_abs[1], shape.width/2*e_abs[2], 0.0)/enorm + return SVector(Basics.sign_eps(e_abs[1])*halfLength, 0.0, Basics.sign_eps(e_abs[3])*halfThickness) + SVector(halfWidth*e_abs[1], halfWidth*e_abs[2], 0.0)/enorm end elseif shape.axis == 2 enorm = norm(SVector(e_abs[2], e_abs[3])) if enorm <= Modia3D.neps - return SVector(Basics.sign_eps(e_abs[1])*shape.thickness/2, Basics.sign_eps(e_abs[2])*shape.length/2, 0.0) + return SVector(Basics.sign_eps(e_abs[1])*halfThickness, Basics.sign_eps(e_abs[2])*halfLength, 0.0) else - return SVector(Basics.sign_eps(e_abs[1])*shape.thickness/2, Basics.sign_eps(e_abs[2])*shape.length/2, 0.0) + SVector(0.0, shape.width/2*e_abs[2], shape.width/2*e_abs[3])/enorm + return SVector(Basics.sign_eps(e_abs[1])*halfThickness, Basics.sign_eps(e_abs[2])*halfLength, 0.0) + SVector(0.0, halfWidth*e_abs[2], halfWidth*e_abs[3])/enorm end else enorm = norm(SVector(e_abs[3], e_abs[1])) if enorm <= Modia3D.neps - return SVector(0.0, Basics.sign_eps(e_abs[2])*shape.thickness/2, Basics.sign_eps(e_abs[3])*shape.length/2) + return SVector(0.0, Basics.sign_eps(e_abs[2])*halfThickness, Basics.sign_eps(e_abs[3])*halfLength) else - return SVector(0.0, Basics.sign_eps(e_abs[2])*shape.thickness/2, Basics.sign_eps(e_abs[3])*shape.length/2) + SVector(shape.width/2*e_abs[1], 0.0, shape.width/2*e_abs[3])/enorm + return SVector(0.0, Basics.sign_eps(e_abs[2])*halfThickness, Basics.sign_eps(e_abs[3])*halfLength) + SVector(halfWidth*e_abs[1], 0.0, halfWidth*e_abs[3])/enorm end end end end # [Gino v.d. Bergen, p. 131] -@inline function supportPoint_abs_FileMesh(shape::FileMesh, e_abs::SVector{3,Float64}) +@inline function supportPoint_abs_FileMesh(shape::FileMesh, e_abs::SVector{3,T}) where {T} e_absVec = Vector{SVector{3,Float64}}(undef, 1) e_absVec[1] = e_abs (max_value, position) = findmax(broadcast(dot, shape.objPoints, e_absVec)) - return shape.objPoints[position] + return SVector{3,T}(shape.objPoints[position]) end diff --git a/src/contactDetection/ContactDetectionMPR/ContactDetectionMPR_handler.jl b/src/contactDetection/ContactDetectionMPR/ContactDetectionMPR_handler.jl index fe12aad9..edcd2bcc 100644 --- a/src/contactDetection/ContactDetectionMPR/ContactDetectionMPR_handler.jl +++ b/src/contactDetection/ContactDetectionMPR/ContactDetectionMPR_handler.jl @@ -108,37 +108,31 @@ about the contact situation. - `neps`: Small number used to check whether a floating number is close to zero (> 0.0). """ -mutable struct ContactDetectionMPR_handler <: Modia3D.AbstractContactDetection - contactPairs::Composition.ContactPairs - distanceComputed::Bool - - lastContactDict::Dict{PairID,ContactPair} - contactDict::Dict{ PairID,ContactPair} - noContactMinVal::Float64 - - tol_rel::Float64 - niter_max::Int - neps::Float64 - - # Visualization options - visualizeContactPoints::Bool - visualizeSupportPoints::Bool - defaultContactSphereDiameter::Float64 - - function ContactDetectionMPR_handler(;tol_rel = 1.0e-7, - niter_max = 100 , - neps = sqrt(eps())) - @assert(tol_rel > 0.0) - @assert(niter_max > 0) - @assert(neps > 0.0) - handler = new() - handler.distanceComputed = false - handler.lastContactDict = Dict{PairID,ContactPair}() - handler.contactDict = Dict{PairID,ContactPair}() - handler.noContactMinVal = 42.0 - handler.tol_rel = tol_rel - handler.niter_max = niter_max - handler.neps = neps - return handler - end +mutable struct ContactDetectionMPR_handler{T} <: Modia3D.AbstractContactDetection + distanceComputed::Bool + + lastContactDict::Dict{PairID,ContactPair} + contactDict::Dict{ PairID,ContactPair} + noContactMinVal::Float64 + + tol_rel::T + niter_max::Int + neps::T + + contactPairs::Composition.ContactPairs + + # Visualization options + visualizeContactPoints::Bool + visualizeSupportPoints::Bool + defaultContactSphereDiameter::Float64 + + function ContactDetectionMPR_handler{T}(;tol_rel = 1.0e-7, + niter_max = 100 , + neps = sqrt(eps(T))) where {T} + @assert(tol_rel > 0.0) + @assert(niter_max > 0) + @assert(neps > 0.0) + new(false, Dict{PairID,ContactPair}(), Dict{PairID,ContactPair}(), 42.0, tol_rel, niter_max, neps) + end end +ContactDetectionMPR_handler() = ContactDetectionMPR_handler{Modia3D.MPRFloatType}() diff --git a/src/contactDetection/ContactDetectionMPR/handler.jl b/src/contactDetection/ContactDetectionMPR/handler.jl index 1873614a..7463222c 100644 --- a/src/contactDetection/ContactDetectionMPR/handler.jl +++ b/src/contactDetection/ContactDetectionMPR/handler.jl @@ -12,8 +12,8 @@ AABB_touching(aabb1::Basics.BoundingBox, aabb2::Basics.BoundingBox) = aabb1.x_ma function Composition.initializeContactDetection!(world::Composition.Object3D, scene::Composition.Scene)::Nothing - if typeof(scene.options.contactDetection) == Modia3D.ContactDetectionMPR_handler - ch::Modia3D.ContactDetectionMPR_handler = scene.options.contactDetection + if typeof(scene.options.contactDetection) == Modia3D.ContactDetectionMPR_handler{Modia3D.MPRFloatType} + ch = scene.options.contactDetection ch.contactPairs = Composition.ContactPairs(world, scene, ch.visualizeContactPoints, ch.visualizeSupportPoints, ch.defaultContactSphereDiameter) if ch.contactPairs.nz == 0 diff --git a/src/contactDetection/ContactDetectionMPR/mpr.jl b/src/contactDetection/ContactDetectionMPR/mpr.jl index 5790e41f..7251ca02 100644 --- a/src/contactDetection/ContactDetectionMPR/mpr.jl +++ b/src/contactDetection/ContactDetectionMPR/mpr.jl @@ -4,21 +4,21 @@ # Collision detection algorithm based on the MPR algorithm -mutable struct SupportPoint - p::SVector{3,Float64} # support point - n::SVector{3,Float64} # support normal unit vector - a::SVector{3,Float64} # point on shapeA - b::SVector{3,Float64} # point on shapeB - function SupportPoint(p::SVector{3,Float64},n::SVector{3,Float64},a::SVector{3,Float64},b::SVector{3,Float64}) +mutable struct SupportPoint{T} + p::SVector{3,T} # support point + n::SVector{3,T} # support normal unit vector + a::SVector{3,T} # point on shapeA + b::SVector{3,T} # point on shapeB + function SupportPoint{T}(p::SVector{3,T},n::SVector{3,T},a::SVector{3,T},b::SVector{3,T}) where {T} new(p,n,a,b) end end -function getSupportPoint(shapeA::Modia3D.Composition.Object3D, shapeB::Composition.Object3D, n::SVector{3,Float64}; scale::Float64=1.0) +function getSupportPoint(shapeA::Modia3D.Composition.Object3D, shapeB::Composition.Object3D, n::SVector{3,T}; scale::T=T(1.0)) where {T} a = Modia3D.supportPoint(shapeA, n) b = Modia3D.supportPoint(shapeB, -n) - return SupportPoint((a-b).*scale,n,a,b) + return SupportPoint{T}((a-b).*scale,n,a,b) end @@ -60,20 +60,20 @@ end ########### Phase 1, Minkowski Portal Refinement ################### -@inline getCentroid(obj::Composition.Object3D) = (obj.r_abs + obj.R_abs'*obj.centroid) +@inline getCentroid(obj::Composition.Object3D) = SVector{3,Modia3D.MPRFloatType}(obj.r_abs + obj.R_abs'*obj.centroid) # checks if centers of shapeA and shapeB are overlapping # belongs to construction of r0 -function checkCentersOfShapesOverlapp(r0::SupportPoint, neps::Float64, shapeA::Composition.Object3D, shapeB::Composition.Object3D) +function checkCentersOfShapesOverlapp(r0::SupportPoint, neps::T, shapeA::Composition.Object3D, shapeB::Composition.Object3D) where {T} if norm(r0.p) <= neps error("MPR: Too large penetration (prerequisite of MPR violated). Centers are overlapping. Look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)).") end end -function checkIfShapesArePlanar(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,n2::SVector{3,Float64}, neps::Float64, - shapeA::Composition.Object3D,shapeB::Composition.Object3D) +function checkIfShapesArePlanar(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,n2::SVector{3,T}, neps::T, + shapeA::Composition.Object3D,shapeB::Composition.Object3D) where {T} # r3 is in the direction of plane normal that contains triangle r0-r1-r2 n3 = cross(r1.p-r0.p, r2.p-r0.p) # the triangle r0-r1-r2 has degenerated into a line segment @@ -120,12 +120,12 @@ end # Der Ursprung muss nicht enthalten sein!!! function tetrahedronEncloseOrigin(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, - neps::Float64, niter_max::Int64, - shapeA::Composition.Object3D, shapeB::Composition.Object3D, scale::Float64) + neps::T, niter_max::Int64, + shapeA::Composition.Object3D, shapeB::Composition.Object3D, scale::T) where {T} r1org = r1 r2org = r2 r3org = r3 - aux = Modia3D.ZeroVector3D + aux = SVector{3, T}(Modia3D.ZeroVector3D) success = false for i in 1:niter_max aux = cross(r1.p-r0.p,r3.p-r0.p) @@ -158,7 +158,7 @@ end ########### Phase 3, Minkowski Portal Refinement ################### # construction of r4 function constructR4(r0::SupportPoint,r1::SupportPoint,r2::SupportPoint,r3::SupportPoint, - neps::Float64, shapeA::Composition.Object3D,shapeB::Composition.Object3D, scale::Float64) + neps::T, shapeA::Composition.Object3D,shapeB::Composition.Object3D, scale::T) where {T} n4 = cross(r2.p-r1.p, r3.p-r1.p) if norm(n4) <= neps r3 = getSupportPoint(shapeA, shapeB, -r3.n, scale=scale) # change search direction @@ -244,11 +244,11 @@ function finalTC3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::Supp end -function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, neps::Float64, niter_max::Int64,tol_rel::Float64, shapeA::Composition.Object3D,shapeB::Composition.Object3D, scale::Float64) +function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::SupportPoint, neps::T, niter_max::Int64, tol_rel::T, shapeA::Composition.Object3D, shapeB::Composition.Object3D, scale::T) where {T} r1org = r1 r2org = r2 r3org = r3 - new_tol = 42.0 + new_tol = T(42.0) isTC2 = false isTC3 = false r1_new::SupportPoint = r0 @@ -269,12 +269,12 @@ function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::Suppor ## TERMINATION CONDITION 2 ## if TC2 < tol_rel (distance,r1,r2,r3,r4) = finalTC2(r1,r2,r3,r4) - return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b) + return distance, r1, r2, r3, r4 ## TERMINATION CONDITION 3 ## elseif TC3 < tol_rel (distance,r1,r2,r3,r4) = finalTC3(r0, r1, r2, r3, r4) - return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b) + return distance, r1, r2, r3, r4 else if TC2 < new_tol new_tol = TC2 @@ -304,11 +304,11 @@ function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::Suppor @warn("MPR (phase 3): Numerical issues with distance computation between $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)). tol_rel increased locally for this computation to $new_tol.") if isTC2 (distance,r1,r2,r3,r4) = finalTC2(r1_new,r2_new,r3_new,r4_new) - return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b) + return distance, r1, r2, r3, r4 end if isTC3 (distance,r1,r2,r3,r4) = finalTC3(r0, r1_new, r2_new, r3_new, r4_new) - return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b) + return distance, r1, r2, r3, r4 end end end @@ -319,13 +319,15 @@ function phase3(r0::SupportPoint, r1::SupportPoint, r2::SupportPoint, r3::Suppor @warn("MPR (phase 3): Max. number of iterations (= $niter_max) is reached and $niter_max > 100, look at $(Modia3D.fullName(shapeA)) and $(Modia3D.fullName(shapeB)). tol_rel increased locally for this computation to $new_tol.") if isTC2 (distance,r1,r2,r3,r4) = finalTC2(r1_new,r2_new,r3_new,r4_new) - return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b) + return distance, r1, r2, r3, r4 end if isTC3 (distance,r1,r2,r3,r4) = finalTC3(r0, r1_new, r2_new, r3_new, r4_new) - return (distance, r4.a, r4.b, r4.n, true, r1.a, r1.b, r2.a, r2.b, r3.a, r3.b) + return distance, r1, r2, r3, r4 end end + @error("passiert das?!?!?") + return distance, r1, r2, r3, r4 # needed for a unique return type end # MPR - Minkowski Portal Refinement algorithm @@ -352,7 +354,7 @@ end # Termination Condition 2 # Termination Condition 3 # Phase 3.3: construct baby tetrahedrons with r1,r2,r3,r4 and create a new portal -function mprGeneral(ch::Composition.ContactDetectionMPR_handler, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D) +function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D) where {T} tol_rel = ch.tol_rel niter_max = ch.niter_max neps = ch.neps @@ -365,7 +367,7 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler, shapeA::Composi # the direction of the origin ray r0 is -r0.p centroidA = getCentroid(shapeA) centroidB = getCentroid(shapeB) - r0 = SupportPoint(centroidA-centroidB, -(centroidA-centroidB), SVector{3,Float64}(0.0,0.0,0.0), SVector{3,Float64}(0.0,0.0,0.0)) + r0 = SupportPoint{T}(centroidA-centroidB, -(centroidA-centroidB), SVector{3,T}(0.0,0.0,0.0), SVector{3,T}(0.0,0.0,0.0)) # check if centers of shapes are overlapping checkCentersOfShapesOverlapp(r0, neps, shapeA, shapeB) @@ -386,7 +388,7 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler, shapeA::Composi # e.g. any collision/or distance between two spheres #println("TC 1") distance = dot(r1.p,normalize(r0.p)) - return (distance, r1.a, r1.b, r1.n, false, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D) + 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) else # normalize n2 n2 = n2/n2abs @@ -407,15 +409,16 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler, shapeA::Composi ########### Phase 3, Minkowski Portal Refinement ################### - phase3(r0, r1, r2, r3, neps, niter_max, tol_rel, shapeA, shapeB, scale) + (distance, r1, r2, r3, r4) = phase3(r0, r1, r2, r3, neps, 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) ) end -function mprTwoSpheres(ch::Composition.ContactDetectionMPR_handler, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D, - sphereA::Shapes.Sphere, sphereB::Shapes.Sphere) +function mprTwoSpheres(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D, + sphereA::Shapes.Sphere, sphereB::Shapes.Sphere) where {T} neps = ch.neps - radiusA = sphereA.diameter*0.5 - radiusB = sphereB.diameter*0.5 + radiusA = T(sphereA.diameter*0.5) + radiusB = T(sphereB.diameter*0.5) centroidSphereA = getCentroid(shapeA) centroidSphereB = getCentroid(shapeB) n = centroidSphereB - centroidSphereA @@ -428,11 +431,11 @@ function mprTwoSpheres(ch::Composition.ContactDetectionMPR_handler, shapeA::Comp distance = distanceCentroids - radiusA - radiusB contactPointShapeA = centroidSphereA + normal*radiusA contactPointShapeB = centroidSphereB - normal*radiusB - return (distance, contactPointShapeA, contactPointShapeB, normal, false, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D, Modia3D.ZeroVector3D) + 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) end -function mpr(ch::Composition.ContactDetectionMPR_handler, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D) +function mpr(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D) where {T} shapeKindA = shapeA.shapeKind shapeKindB = shapeB.shapeKind @@ -449,7 +452,7 @@ function mpr(ch::Composition.ContactDetectionMPR_handler, shapeA::Composition.Ob support1A, support1B, support2A, support2B, support3A, support3B) = mprGeneral(ch, shapeA, shapeB) if shapeKindA == Modia3D.SphereKind - centroidSphere = getCentroid(shapeA) + centroidSphere = SVector{3,Float64}(getCentroid(shapeA)) sphereA1::Shapes.Sphere = shapeA.shape radius = sphereA1.diameter*0.5 contactPointSphere = centroidSphere + radius*normal @@ -458,7 +461,7 @@ function mpr(ch::Composition.ContactDetectionMPR_handler, shapeA::Composition.Ob contactPoint2 = contactPointOtherShape elseif shapeKindB == Modia3D.SphereKind normalLocal = -normal - centroidSphere = getCentroid(shapeB) + centroidSphere = SVector{3,Float64}(getCentroid(shapeB)) sphereB1::Shapes.Sphere = shapeB.shape radius = sphereB1.diameter*0.5 contactPointSphere = centroidSphere + radius*normalLocal diff --git a/src/contactDetection/ContactDetectionMPR/utilitiesMPR.jl b/src/contactDetection/ContactDetectionMPR/utilitiesMPR.jl index be9119ff..6b9100cf 100644 --- a/src/contactDetection/ContactDetectionMPR/utilitiesMPR.jl +++ b/src/contactDetection/ContactDetectionMPR/utilitiesMPR.jl @@ -120,9 +120,9 @@ end - r4: Point on line r1 + lambda*r21 that is closest to the origin with 0<=lambda<=1, or r4_old, if it is closer. - d4: Signed distance of r4 to the origin or d4_old if abs(d4_old) < norm(r1 + lambda*r21) """ -function signedDistanceToLineSegmentWithR4(r1::SVector{3,Float64}, r2::SVector{3,Float64}, - r21::SVector{3,Float64}, e::SVector{3,Float64}, - r4_old::SVector{3,Float64}, d4_old::Float64=0.0) +function signedDistanceToLineSegmentWithR4(r1::SVector{3,T}, r2::SVector{3,T}, + r21::SVector{3,T}, e::SVector{3,T}, + r4_old::SVector{3,T}, d4_old::T=T(0.0)) where {T} # r = r1 + lambda*r21 (0 <= lambda <= 1) # # The closest point to the line is point r4, such that r21*r4 = 0 @@ -146,8 +146,8 @@ end - r4: Point on line r1 + lambda*r21 that is closest to the origin with 0<=lambda<=1. - d4: Signed distance of r4 to the origin """ -function signedDistanceToLineSegment(r1::SVector{3,Float64}, r2::SVector{3,Float64}, - r21::SVector{3,Float64}, e::SVector{3,Float64}) +function signedDistanceToLineSegment(r1::SVector{3,T}, r2::SVector{3,T}, + r21::SVector{3,T}, e::SVector{3,T}) where {T} # r = r1 + lambda*r21 (0 <= lambda <= 1) # # The closest point to the line is point r4, such that r21*r4 = 0 @@ -168,8 +168,8 @@ end - r4: Point on triangle portal r1, r2, r3 that is closest to the origin. - d : Signed distance of r4 to the origin: d > 0: origin is outside of tetrahedron r0,r1,r2,r3, otherwise inside. """ -function signedDistanceToPortal(r0::SVector{3,Float64}, r1::SVector{3,Float64}, - r2::SVector{3,Float64}, r3::SVector{3,Float64}) +function signedDistanceToPortal(r0::SVector{3,T}, r1::SVector{3,T}, + r2::SVector{3,T}, r3::SVector{3,T}) where {T} # Determine unit normal on portal r21 = r2 - r1 r31 = r3 - r1