Skip to content

Commit 8a3b2a5

Browse files
AndreaNeumayrAndreaNeumayr
committed
collisionSmoothingRadius is set to 0.001 + adapt supportPoint computation
1 parent 451a717 commit 8a3b2a5

File tree

2 files changed

+40
-27
lines changed

2 files changed

+40
-27
lines changed

‎src/Shapes/boundingBoxes.jl‎

Lines changed: 38 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -21,16 +21,16 @@ that is the most extreme in direction of unit vector `e`.
2121
r_abs + R_abs'*supportPoint_abs_Box(shape, R_abs*e, collisionSmoothingRadius) + e*collisionSmoothingRadius
2222
#
2323
@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} =
24-
r_abs + R_abs'*supportPoint_abs_Cylinder(shape, R_abs*e) + e*collisionSmoothingRadius
24+
r_abs + R_abs'*supportPoint_abs_Cylinder(shape, R_abs*e, collisionSmoothingRadius) + e*collisionSmoothingRadius
2525

2626
@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} =
27-
r_abs + R_abs'*supportPoint_abs_Cone(shape, R_abs*e) + e*collisionSmoothingRadius
27+
r_abs + R_abs'*supportPoint_abs_Cone(shape, R_abs*e, collisionSmoothingRadius) + e*collisionSmoothingRadius
2828

2929
@inline supportPoint_Capsule(shape::Capsule, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}) where {T} =
3030
r_abs + R_abs'*supportPoint_abs_Capsule(shape, R_abs*e)
3131

3232
@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} =
33-
r_abs + R_abs'*supportPoint_abs_Beam(shape, R_abs*e) + e*collisionSmoothingRadius
33+
r_abs + R_abs'*supportPoint_abs_Beam(shape, R_abs*e, collisionSmoothingRadius) + e*collisionSmoothingRadius
3434

3535
@inline supportPoint_Sphere(shape::Sphere, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}) where {T} =
3636
r_abs + (shape.diameter/2)*e
@@ -61,21 +61,21 @@ end
6161
# [Gino v.d. Bergen, p. 135]
6262
@inline function supportPoint_abs_Box(shape::Box, e_abs::SVector{3,T}, collisionSmoothingRadius::T) where {T}
6363
@inbounds begin
64-
halfLengthX = T(0.5*shape.lengthX)
65-
halfLengthY = T(0.5*shape.lengthY)
66-
halfLengthZ = T(0.5*shape.lengthZ)
64+
halfLengthX = T(0.5*shape.lengthX) - collisionSmoothingRadius
65+
halfLengthY = T(0.5*shape.lengthY) - collisionSmoothingRadius
66+
halfLengthZ = T(0.5*shape.lengthZ) - collisionSmoothingRadius
6767
return SVector{3,T}(
68-
Basics.sign_eps(e_abs[1])*(halfLengthX-collisionSmoothingRadius),
69-
Basics.sign_eps(e_abs[2])*(halfLengthY-collisionSmoothingRadius),
70-
Basics.sign_eps(e_abs[3])*(halfLengthZ-collisionSmoothingRadius) )
68+
Basics.sign_eps(e_abs[1])*(halfLengthX),
69+
Basics.sign_eps(e_abs[2])*(halfLengthY),
70+
Basics.sign_eps(e_abs[3])*(halfLengthZ) )
7171
end
7272
end
7373

7474
# [Gino v.d. Bergen, p. 136, XenoCollide, p. 168, 169]
75-
@inline function supportPoint_abs_Cylinder(shape::Cylinder, e_abs::SVector{3,T}) where {T}
75+
@inline function supportPoint_abs_Cylinder(shape::Cylinder, e_abs::SVector{3,T}, collisionSmoothingRadius::T) where {T}
7676
@inbounds begin
77-
halfLength = T(0.5*shape.length)
78-
halfDiameter = T(0.5*shape.diameter)
77+
halfLength = T(0.5*shape.length) - collisionSmoothingRadius
78+
halfDiameter = T(0.5*shape.diameter) - collisionSmoothingRadius
7979
if shape.axis == 1
8080
enorm = norm(SVector(e_abs[2], e_abs[3]))
8181
if enorm <= Modia3D.nepsType(T)
@@ -118,17 +118,30 @@ end
118118

119119
# for cone: [Gino v.d. Bergen, p. 136]]
120120
# for frustum of a cone: A. Neumayr, G. Hippmann
121-
@inline function supportPoint_abs_Cone(shape::Cone, e_abs::SVector{3,T}) where {T}
121+
@inline function supportPoint_abs_Cone(shape::Cone, e_abs::SVector{3,T}, collisionSmoothingRadius::T) where {T}
122122
@inbounds begin
123-
baseRadius = T(0.5*shape.diameter)
124123
rightCone = T(shape.topDiameter) == T(0.0)
125-
shapeLength = T(shape.length)
124+
R = T(0.5*shape.diameter)
125+
H = T(shape.length)
126+
r = collisionSmoothingRadius
127+
128+
Rright = (R*H-R*r-r*sqrt(H^2 + R^2))/H
129+
Hright = Rright/(R/H)
130+
126131
if rightCone
132+
baseRadius = Rright
133+
shapeLength = Hright
127134
sin_phi = T(baseRadius/sqrt(baseRadius^2 + shapeLength^2)) # sin of base angle
128135
else
129-
topRadius = T(0.5*shape.topDiameter)
130-
diffRadius = T(baseRadius - topRadius)
131-
sin_phi = T(diffRadius/sqrt(diffRadius^2 + shapeLength^2)) # sin of base angle
136+
Rt = T(0.5*shape.topDiameter)
137+
Hcone = H - 2*r
138+
diffRadius = R - Rt
139+
Rcone = diffRadius*Hcone/H
140+
topRadius = Rt - Rright + Rcone
141+
@assert(topRadius > 0.0)
142+
shapeLength = Hcone
143+
baseRadius = topRadius + Rcone
144+
sin_phi = T(Rcone/sqrt(Rcone^2 + shapeLength^2)) # sin of base angle
132145
end
133146
if shape.axis == 1
134147
value = e_abs[1] / norm(SVector(e_abs[1], e_abs[2], e_abs[3]))
@@ -198,11 +211,11 @@ end
198211
end
199212

200213
# G. Hippmann: Outer half circles of beam
201-
@inline function supportPoint_abs_Beam(shape::Beam, e_abs::SVector{3,T}) where {T}
214+
@inline function supportPoint_abs_Beam(shape::Beam, e_abs::SVector{3,T}, collisionSmoothingRadius::T) where {T}
202215
@inbounds begin
203-
halfLength = T(0.5*shape.length)
204-
halfWidth = T(0.5*shape.width)
205-
halfThickness = T(0.5*shape.thickness)
216+
halfLength = T(0.5*shape.length) - collisionSmoothingRadius
217+
halfWidth = T(0.5*shape.width) - collisionSmoothingRadius
218+
halfThickness = T(0.5*shape.thickness) - collisionSmoothingRadius
206219
if shape.axis == 1
207220
enorm = norm(SVector(e_abs[1], e_abs[2]))
208221
if enorm <= Modia3D.nepsType(T)
@@ -266,16 +279,16 @@ Returns the *Axis Aligned Bounding Box* of solid `shape` in argument `AABB`.
266279
r_absi + R_absi'*supportPoint_abs_Box(shape, isign*R_absi, collisionSmoothingRadius) + isign*collisionSmoothingRadius
267280

268281
@inline supportPoint_i_Cylinder(shape::Cylinder{F}, r_absi::F, R_absi::SVector{3,F}, isign::Int, collisionSmoothingRadius) where F <: Modia3D.VarFloatType =
269-
r_absi + R_absi'*supportPoint_abs_Cylinder(shape, isign*R_absi) + isign*collisionSmoothingRadius
282+
r_absi + R_absi'*supportPoint_abs_Cylinder(shape, isign*R_absi, collisionSmoothingRadius) + isign*collisionSmoothingRadius
270283

271284
@inline supportPoint_i_Cone(shape::Cone{F}, r_absi::F, R_absi::SVector{3,F}, isign::Int, collisionSmoothingRadius) where F <: Modia3D.VarFloatType =
272-
r_absi + R_absi'*supportPoint_abs_Cone(shape, isign*R_absi) + isign*collisionSmoothingRadius
285+
r_absi + R_absi'*supportPoint_abs_Cone(shape, isign*R_absi, collisionSmoothingRadius) + isign*collisionSmoothingRadius
273286

274287
@inline supportPoint_i_Capsule(shape::Capsule{F}, r_absi::F, R_absi::SVector{3,F}, isign::Int) where F <: Modia3D.VarFloatType =
275288
r_absi + R_absi'*supportPoint_abs_Capsule(shape, isign*R_absi)
276289
#
277290
@inline supportPoint_i_Beam(shape::Beam{F}, r_absi::F, R_absi::SVector{3,F}, isign::Int, collisionSmoothingRadius) where F <: Modia3D.VarFloatType =
278-
r_absi + R_absi'*supportPoint_abs_Beam(shape, isign*R_absi) + isign*collisionSmoothingRadius
291+
r_absi + R_absi'*supportPoint_abs_Beam(shape, isign*R_absi, collisionSmoothingRadius) + isign*collisionSmoothingRadius
279292

280293
@inline supportPoint_i_Ellipsoid(shape::Ellipsoid{F}, r_absi::F, R_absi::SVector{3,F}, isign::Int) where F <: Modia3D.VarFloatType =
281294
r_absi + dot(R_absi, supportPoint_abs_Ellipsoid(shape, isign*R_absi))

‎src/Shapes/solid.jl‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ Generate a [Solid](@ref) with physical behavior of a rigid body with mass, visua
3333
If undefined `solidMaterial` is used as contact material. Only contact material combinations defined in
3434
palettes/contactPairMaterials.json can be used.
3535
36-
- `collisionSmoothingRadius`: Defines a collision smoothing radius for surface edges.
36+
- `collisionSmoothingRadius`: Defines a collision smoothing radius for surface edges, its default value is `0.001`. It takes the minimum value of your collision smoothing radius and 10% of the smallest shape length, like `min(collisionSmoothingRadius, 0.1 min(shape dimensions))`. If it is set to `0.0` no `collisionSmoothingRadius` is used. A `collisionSmoothingRadius` is used for `Box`, `Cylinder`, `Cone`, and `Beam`.
3737
3838
- `visualMaterial`: Defines the material of the solid used for visualization. A pre-defined [Visual material](@ref)
3939
from palettes/visualMaterials.json (e.g. `"RedTransparent"`) or a user-defined [Visual material](@ref) (e.g.
@@ -66,7 +66,7 @@ struct Solid{F <: Modia3D.VarFloatType} <: Modia3D.AbstractObject3DFeature
6666
massProperties::Union{Modia3D.AbstractMassProperties, Number, AbstractString, SolidMaterial, Nothing} = nothing,
6767
collision::Bool = false,
6868
contactMaterial::AbstractString = "",
69-
collisionSmoothingRadius=F(0.0),
69+
collisionSmoothingRadius=F(0.001),
7070
visualMaterial::Union{Shapes.VisualMaterial,AbstractString,Nothing} = Shapes.VisualMaterial(),
7171
visualMaterialConvexDecomposition::Union{Shapes.VisualMaterial,AbstractString,Nothing} = Shapes.VisualMaterial()) where F <: Modia3D.VarFloatType
7272

0 commit comments

Comments
 (0)