diff --git a/src/Composition/supportPoints.jl b/src/Composition/supportPoints.jl index 4c71ab2b..32039fc2 100644 --- a/src/Composition/supportPoints.jl +++ b/src/Composition/supportPoints.jl @@ -20,7 +20,7 @@ function supportPoint(obj::Composition.Object3D, e::SVector{3,Float64}) 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, collisionSmoothingRadius) + 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) @@ -96,12 +96,12 @@ function boundingBox!(obj::Composition.Object3D, AABB::Basics.BoundingBox; tight zmax = Modia3D.supportPoint_i_Cone(cone, obj.r_abs[3], SVector(obj.R_abs[:,3]), +1, collisionSmoothingRadius) elseif shapeKind == Modia3D.CapsuleKind capsule::Modia3D.Capsule = obj.shape - xmin = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[1], SVector(obj.R_abs[:,1]), -1, collisionSmoothingRadius) - xmax = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[1], SVector(obj.R_abs[:,1]), +1, collisionSmoothingRadius) - ymin = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[2], SVector(obj.R_abs[:,2]), -1, collisionSmoothingRadius) - ymax = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[2], SVector(obj.R_abs[:,2]), +1, collisionSmoothingRadius) - zmin = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[3], SVector(obj.R_abs[:,3]), -1, collisionSmoothingRadius) - zmax = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[3], SVector(obj.R_abs[:,3]), +1, collisionSmoothingRadius) + xmin = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[1], SVector(obj.R_abs[:,1]), -1) + xmax = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[1], SVector(obj.R_abs[:,1]), +1) + ymin = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[2], SVector(obj.R_abs[:,2]), -1) + ymax = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[2], SVector(obj.R_abs[:,2]), +1) + zmin = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[3], SVector(obj.R_abs[:,3]), -1) + zmax = Modia3D.supportPoint_i_Capsule(capsule, obj.r_abs[3], SVector(obj.R_abs[:,3]), +1) elseif shapeKind == Modia3D.BeamKind beam::Modia3D.Beam = obj.shape xmin = Modia3D.supportPoint_i_Beam(beam, obj.r_abs[1], SVector(obj.R_abs[:,1]), -1, collisionSmoothingRadius) diff --git a/src/Shapes/boundingBoxes.jl b/src/Shapes/boundingBoxes.jl index 86dc832c..3d77b4bc 100644 --- a/src/Shapes/boundingBoxes.jl +++ b/src/Shapes/boundingBoxes.jl @@ -26,8 +26,8 @@ that is the most extreme in direction of unit vector `e`. @inline supportPoint_Cone(shape::Cone, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}, collisionSmoothingRadius::Float64) = 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}, collisionSmoothingRadius::Float64) = - r_abs + R_abs'*supportPoint_abs_Capsule(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}) = + 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) = r_abs + R_abs'*supportPoint_abs_Beam(shape, R_abs*e) + e*collisionSmoothingRadius @@ -248,8 +248,8 @@ Returns the *Axis Aligned Bounding Box* of solid `shape` in argument `AABB`. @inline supportPoint_i_Cone(shape::Cone, r_absi::Float64, R_absi::SVector{3,Float64}, isign::Int, collisionSmoothingRadius) = r_absi + R_absi'*supportPoint_abs_Cone(shape, isign*R_absi) + isign*collisionSmoothingRadius -@inline supportPoint_i_Capsule(shape::Capsule, r_absi::Float64, R_absi::SVector{3,Float64}, isign::Int, collisionSmoothingRadius) = - r_absi + R_absi'*supportPoint_abs_Capsule(shape, isign*R_absi) + isign*collisionSmoothingRadius +@inline supportPoint_i_Capsule(shape::Capsule, r_absi::Float64, R_absi::SVector{3,Float64}, isign::Int) = + r_absi + R_absi'*supportPoint_abs_Capsule(shape, isign*R_absi) # @inline supportPoint_i_Beam(shape::Beam, r_absi::Float64, R_absi::SVector{3,Float64}, isign::Int, collisionSmoothingRadius) = r_absi + R_absi'*supportPoint_abs_Beam(shape, isign*R_absi) + isign*collisionSmoothingRadius diff --git a/src/Shapes/setCollisionSmoothingRadius.jl b/src/Shapes/setCollisionSmoothingRadius.jl index 8cb33da2..002911d2 100644 --- a/src/Shapes/setCollisionSmoothingRadius.jl +++ b/src/Shapes/setCollisionSmoothingRadius.jl @@ -25,12 +25,6 @@ function setCollisionSmoothingRadius(shape::Cone, collisionSmoothingRadius) return collisionSmoothingRadius2 end -function setCollisionSmoothingRadius(shape::Capsule, collisionSmoothingRadius) - @assert(collisionSmoothingRadius >= 0.0) - collisionSmoothingRadius2 = min(collisionSmoothingRadius, 0.1*min(shape.diameter, shape.length)) # at most 10% of the smallest edge length - return collisionSmoothingRadius2 -end - function setCollisionSmoothingRadius(shape::Beam, collisionSmoothingRadius) @assert(collisionSmoothingRadius >= 0.0) collisionSmoothingRadius2 = min(collisionSmoothingRadius, 0.1*min(shape.length, shape.width, shape.thickness)) # at most 10% of the smallest edge length