Skip to content

Commit a190d90

Browse files
MartinOtterMartinOtter
authored
Merge pull request #95 from ModiaSim/gh_fixConeContactDetection
Fix Cone contact detection
2 parents e5f35a3 + ccaafa5 commit a190d90

File tree

6 files changed

+125
-53
lines changed

6 files changed

+125
-53
lines changed

‎src/Shapes/boundingBoxes.jl‎

Lines changed: 38 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -120,90 +120,94 @@ end
120120
# for frustum of a cone: A. Neumayr, G. Hippmann
121121
@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)
124+
shapeLength = T(shape.length)
123125
rightCone = T(shape.topDiameter) == T(0.0)
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-
131126
if rightCone
132-
baseRadius = Rright
133-
shapeLength = Hright
134-
sin_phi = T(baseRadius/sqrt(baseRadius^2 + shapeLength^2)) # sin of base angle
127+
slantHeight = T(sqrt(baseRadius^2 + shapeLength^2))
128+
sin_phi = T(baseRadius/slantHeight) # sin of base angle
135129
else
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
130+
topRadius = T(0.5*shape.topDiameter)
131+
diffRadius = T(baseRadius - topRadius)
132+
slantHeight = T(sqrt(diffRadius^2 + shapeLength^2))
133+
sin_phi = T(diffRadius/slantHeight) # sin of base angle
145134
end
146135
if shape.axis == 1
147136
value = e_abs[1] / norm(SVector(e_abs[1], e_abs[2], e_abs[3]))
148137
if value >= sin_phi
149138
if rightCone
150-
return SVector(shapeLength, 0.0, 0.0) # apex is support point
139+
hPos = T(shapeLength - collisionSmoothingRadius*slantHeight/baseRadius) # shapeLength - r/sin(phi)
140+
return SVector(hPos, 0.0, 0.0) # apex is support point
151141
else # frustum of a cone
142+
hPos = T(shapeLength - collisionSmoothingRadius)
152143
enorm = norm(SVector(e_abs[2], e_abs[3]))
153144
if enorm > Modia3D.nepsType(T)
154-
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
145+
topRadius = T(topRadius - collisionSmoothingRadius*(slantHeight - baseRadius)/shapeLength) # topRadius - r*(1/cos(phi) - tan(phi))
146+
return SVector(hPos, 0.0, 0.0) + SVector(0.0, topRadius*e_abs[2], topRadius*e_abs[3]) / enorm # point on top circle is support point
155147
else
156-
return SVector(shapeLength, 0.0, 0.0) # top circle center is support point
148+
return SVector(hPos, 0.0, 0.0) # top circle center is support point
157149
end
158150
end
159151
else
152+
hPos = collisionSmoothingRadius
160153
enorm = norm(SVector(e_abs[2], e_abs[3]))
161154
if enorm > Modia3D.nepsType(T)
162-
return SVector(0.0, baseRadius*e_abs[2], baseRadius*e_abs[3]) / enorm # point on base circle is support point
155+
baseRadius = T(baseRadius - collisionSmoothingRadius*(slantHeight + baseRadius)/shapeLength) # topRadius - r*(1/cos(phi) + tan(phi))
156+
return SVector(hPos, baseRadius*e_abs[2], baseRadius*e_abs[3]) / enorm # point on base circle is support point
163157
else
164-
return SVector{3,T}(0.0, 0.0, 0.0) # base circle center is support point
158+
return SVector{3,T}(hPos, 0.0, 0.0) # base circle center is support point
165159
end
166160
end
167161
elseif shape.axis == 2
168162
value = e_abs[2] / norm(SVector(e_abs[1], e_abs[2], e_abs[3]))
169163
if value >= sin_phi
170164
if rightCone
171-
return SVector(0.0, shapeLength, 0.0) # apex is support point
165+
hPos = T(shapeLength - collisionSmoothingRadius*slantHeight/baseRadius) # shapeLength - r/sin(phi)
166+
return SVector(0.0, hPos, 0.0) # apex is support point
172167
else # frustum of a cone
168+
hPos = T(shapeLength - collisionSmoothingRadius)
173169
enorm = norm(SVector(e_abs[3], e_abs[1]))
174170
if enorm > Modia3D.nepsType(T)
175-
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
171+
topRadius = T(topRadius - collisionSmoothingRadius*(slantHeight - baseRadius)/shapeLength) # topRadius - r*(1/cos(phi) - tan(phi))
172+
return SVector(0.0, hPos, 0.0) + SVector(topRadius*e_abs[1], 0.0, topRadius*e_abs[3]) / enorm # point on top circle is support point
176173
else
177-
return SVector(0.0, shapeLength, 0.0) # top circle center is support point
174+
return SVector(0.0, hPos, 0.0) # top circle center is support point
178175
end
179176
end
180177
else
178+
hPos = collisionSmoothingRadius
181179
enorm = norm(SVector(e_abs[3], e_abs[1]))
182180
if enorm > Modia3D.nepsType(T)
183-
return SVector(baseRadius*e_abs[1], 0.0, baseRadius*e_abs[3]) / enorm # point on base circle is support point
181+
baseRadius = T(baseRadius - collisionSmoothingRadius*(slantHeight + baseRadius)/shapeLength) # topRadius - r*(1/cos(phi) + tan(phi))
182+
return SVector(baseRadius*e_abs[1], hPos, baseRadius*e_abs[3]) / enorm # point on base circle is support point
184183
else
185-
return SVector{3,T}(0.0, 0.0, 0.0) # base circle center is support point
184+
return SVector{3,T}(0.0, hPos, 0.0) # base circle center is support point
186185
end
187186
end
188187
else
189188
value = e_abs[3] / norm(SVector(e_abs[1], e_abs[2], e_abs[3]))
190189
if value >= sin_phi
191190
if rightCone
192-
return SVector(0.0, 0.0, shapeLength) # apex is support point
191+
hPos = T(shapeLength - collisionSmoothingRadius*slantHeight/baseRadius) # shapeLength - r/sin(phi)
192+
return SVector(0.0, 0.0, hPos) # apex is support point
193193
else # frustum of a cone
194+
hPos = T(shapeLength - collisionSmoothingRadius)
194195
enorm = norm(SVector(e_abs[1], e_abs[2]))
195196
if enorm > Modia3D.nepsType(T)
196-
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
197+
topRadius = T(topRadius - collisionSmoothingRadius*(slantHeight - baseRadius)/shapeLength) # topRadius - r*(1/cos(phi) - tan(phi))
198+
return SVector(0.0, 0.0, hPos) + SVector(topRadius*e_abs[1], topRadius*e_abs[2], 0.0) / enorm # point on top circle is support point
197199
else
198-
return SVector(0.0, 0.0, shapeLength) # top circle center is support point
200+
return SVector(0.0, 0.0, hPos) # top circle center is support point
199201
end
200202
end
201203
else
204+
hPos = collisionSmoothingRadius
202205
enorm = norm(SVector(e_abs[1], e_abs[2]))
203206
if enorm > Modia3D.nepsType(T)
204-
return SVector(baseRadius*e_abs[1], baseRadius*e_abs[2], 0.0) / enorm # point on base circle is support point
207+
baseRadius = T(baseRadius - collisionSmoothingRadius*(slantHeight + baseRadius)/shapeLength) # topRadius - r*(1/cos(phi) + tan(phi))
208+
return SVector(baseRadius*e_abs[1], baseRadius*e_abs[2], hPos) / enorm # point on base circle is support point
205209
else
206-
return SVector{3,T}(0.0, 0.0, 0.0) # base circle center is support point
210+
return SVector{3,T}(0.0, 0.0, hPos) # base circle center is support point
207211
end
208212
end
209213
end

‎test/Basic/PendulumWithDamper_MonteCarlo.jl‎

Lines changed: 3 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
module PendulumWithDamper
1+
module PendulumWithDamper_MonteCarloMeasurements
22

33
using Modia3D
44
using Modia3D.MonteCarloMeasurements
@@ -7,7 +7,6 @@ include("$(Modia3D.modelsPath)/Blocks.jl")
77
include("$(Modia3D.modelsPath)/Electric.jl")
88
include("$(Modia3D.modelsPath)/Rotational.jl")
99

10-
1110
Pendulum = Model(
1211
m = 1.0,
1312
g = 9.81,
@@ -16,7 +15,6 @@ Pendulum = Model(
1615
world = Object3D(feature=Scene(enableContactDetection=false)),
1716
worldFrame = Object3D(parent=:world,
1817
feature=Visual(shape=CoordinateSystem(length=0.5))),
19-
2018
frame0 = Object3D(feature=Solid(shape=Beam(axis=1, length=1.0, width=0.2, thickness=0.2),
2119
massProperties=MassProperties(mass=:m),
2220
visualMaterial=:(vmat1))),
@@ -37,14 +35,12 @@ PendulumWithDamp = Model(
3735
(damper.flange_a, fixed.flange)]
3836
)
3937

40-
#@showModel PendulumWithDamp
4138
unsafe_comparisons(:reduction)
4239
pendulumWithDamper = @instantiateModel(PendulumWithDamp, unitless=true, log=false, logStateSelection=false, logCode=false, FloatType =MonteCarloMeasurements.StaticParticles{Float64,100})
4340

4441
stopTime = 10.0
45-
# requiredFinalStates = [-1.578178283450938, 0.061515170100766486]
46-
47-
simulate!(pendulumWithDamper, QBDF(autodiff=false), stopTime=stopTime, log=true, logStates=false, requiredFinalStates=missing)
42+
requiredFinalStates = missing
43+
simulate!(pendulumWithDamper, QBDF(autodiff=false), stopTime=stopTime, log=true, logStates=false, requiredFinalStates=requiredFinalStates)
4844

4945
@usingModiaPlot
5046
plot(pendulumWithDamper, ["pendulum.rev.flange.phi", "pendulum.rev.variables[1]"], figure=1)

‎test/Collision/BouncingCones.jl‎

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ BouncingCones = Model(
2828
translation=:[-1.0, -0.5, 1.0],
2929
rotation=:[-90*u"°", 0.0, -90*u"°"],
3030
feature=Visual(shape=CoordinateSystem(length=0.5))),
31-
coneX = Object3D(feature=Solid(shape=Cone(axis=1, diameter=0.4, length=1.0, topDiameter=0.3),
31+
coneX = Object3D(feature=Solid(shape=Cone(axis=1, diameter=0.4, length=1.0),
3232
visualMaterial=vmatRed,
3333
solidMaterial="DryWood",
3434
collision=true)),
@@ -40,7 +40,7 @@ BouncingCones = Model(
4040
translation=:[0.0, -0.5, 1.0],
4141
rotation=:[90*u"°", 90*u"°", 0.0],
4242
feature=Visual(shape=CoordinateSystem(length=0.5))),
43-
coneY = Object3D(feature=Solid(shape=Cone(axis=2, diameter=0.4, length=1.0, topDiameter=0.3),
43+
coneY = Object3D(feature=Solid(shape=Cone(axis=2, diameter=0.4, length=1.0),
4444
visualMaterial=vmatGreen,
4545
solidMaterial="DryWood",
4646
collision=true)),
@@ -51,7 +51,7 @@ BouncingCones = Model(
5151
frameZ = Object3D(parent=:world,
5252
translation=:[1.0, -0.5, 1.0],
5353
feature=Visual(shape=CoordinateSystem(length=0.5))),
54-
coneZ = Object3D(feature=Solid(shape=Cone(axis=3, diameter=0.4, length=1.0, topDiameter=0.3),
54+
coneZ = Object3D(feature=Solid(shape=Cone(axis=3, diameter=0.4, length=1.0),
5555
visualMaterial=vmatBlue,
5656
solidMaterial="DryWood",
5757
collision=true)),
@@ -63,12 +63,9 @@ BouncingCones = Model(
6363

6464
bouncingCones = @instantiateModel(buildModia3D(BouncingCones), unitless=true, log=false, logStateSelection=false, logCode=false)
6565

66-
#@show bouncingCones.parameterExpressions
67-
#@show bouncingCones.parameters
68-
6966
stopTime = 1.2
7067
tolerance = 1e-8
71-
requiredFinalStates = [-0.7966975786520888, 0.939985832590693, 0.38951809262163817, 0.008283684681695894, 0.6658233515495816, 0.0036360762130771824, 1.5168127712425896, 0.809516683182946, 1.6309255834121132, 3.326610309487532, -0.09442757282865713, 0.1303056174217491, 0.3895556675053645, -0.7966982542294878, 0.939989290934488, 0.0035470907869790593, 0.0092555728573629, 0.6658258352729927, 2.381830853536271, -0.021037308472438263, -1.533543880143283, 0.1309791419088494, 3.326660575823015, -0.09374545764051212, 0.939978985399097, 0.3893676586735939, -0.7967319752002895, 0.6658573796203757, 0.0038700820465069068, 0.005146202892539086, -1.612281246246739, 0.010392303991357257, 2.3818260851771695, -0.09767677033008831, 0.12731399635504168, 3.3267904560140074]
68+
requiredFinalStates = [-0.7999312016374764, 1.0550588860480428, 0.8921254770510955, -0.1896960061863984, 0.8841404598824826, 0.43402607612744604, 1.3606906655246873, 1.1282714887339953, 2.052883317050297, 4.6497266384364115, 0.052126888408507835, 1.009893100393836, 0.8916594826499894, -0.8000400934802586, 1.0551915122440447, 0.43363393980540194, -0.19001673923281473, 0.8846394848571297, 2.735874415024229, -0.2906589834171084, -1.476919610003813, 1.010451091809992, 4.65340945904709, 0.051874592801066474, 1.0551437465113622, 0.8923792400352838, -0.8001536268911364, 0.8848687234167343, 0.4342006445471319, -0.19173561019070964, -1.7767415342672759, -0.2277632915908899, 2.6985438395240826, 0.05325010071392679, 1.012579595034664, 4.656827261542965]
7269
simulate!(bouncingCones, stopTime=stopTime, tolerance=tolerance, log=true, logStates=false, logEvents=false, requiredFinalStates=requiredFinalStates)
7370

7471
@usingModiaPlot

‎test/Collision/BouncingFrustums.jl‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
module BouncingFrustumsSimulation
2+
3+
using Modia3D
4+
5+
vmatRed = VisualMaterial(color="Red")
6+
vmatGreen = VisualMaterial(color="Green")
7+
vmatBlue = VisualMaterial(color="Blue")
8+
vmatGrey = VisualMaterial(color="Grey", transparency=0.5)
9+
10+
BouncingFrustums = Model(
11+
boxHeigth = 0.1,
12+
gravField = UniformGravityField(g=9.81, n=[0, 0, -1]),
13+
world = Object3D(feature=Scene(gravityField=:gravField,
14+
visualizeFrames=false,
15+
defaultFrameLength=0.2,
16+
visualizeBoundingBox = true,
17+
enableContactDetection=true,
18+
visualizeContactPoints=false)),
19+
worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.5))),
20+
ground = Object3D(parent=:world,
21+
translation=:[0.0, 0.0, -boxHeigth/2],
22+
feature=Solid(shape=Box(lengthX=5.0, lengthY=3.0, lengthZ=:boxHeigth),
23+
visualMaterial=vmatGrey,
24+
solidMaterial="DryWood",
25+
collision=true)),
26+
frameX = Object3D(parent=:world,
27+
translation=:[-1.0, -0.5, 1.0],
28+
rotation=:[-90*u"°", 0.0, -90*u"°"],
29+
feature=Visual(shape=CoordinateSystem(length=0.5))),
30+
coneX = Object3D(feature=Solid(shape=Cone(axis=1, diameter=0.4, length=1.0, topDiameter=0.3),
31+
visualMaterial=vmatRed,
32+
solidMaterial="DryWood",
33+
collision=true)),
34+
jointX = FreeMotion(obj1=:frameX, obj2=:coneX,
35+
r=Var(init=[0.0, 0.0, 0.0]),
36+
rot=Var(init=[0.0, -60*u"°", 0.0]),
37+
v=Var(init=[0.0, 1.0, 0.0])),
38+
frameY = Object3D(parent=:world,
39+
translation=:[0.0, -0.5, 1.0],
40+
rotation=:[90*u"°", 90*u"°", 0.0],
41+
feature=Visual(shape=CoordinateSystem(length=0.5))),
42+
coneY = Object3D(feature=Solid(shape=Cone(axis=2, diameter=0.4, length=1.0, topDiameter=0.3),
43+
visualMaterial=vmatGreen,
44+
solidMaterial="DryWood",
45+
collision=true)),
46+
jointY = FreeMotion(obj1=:frameY, obj2=:coneY,
47+
r=Var(init=[0.0, 0.0, 0.0]),
48+
rot=Var(init=[0.0, 0.0, -60*u"°"]),
49+
v=Var(init=[0.0, 0.0, 1.0])),
50+
frameZ = Object3D(parent=:world,
51+
translation=:[1.0, -0.5, 1.0],
52+
feature=Visual(shape=CoordinateSystem(length=0.5))),
53+
coneZ = Object3D(feature=Solid(shape=Cone(axis=3, diameter=0.4, length=1.0, topDiameter=0.3),
54+
visualMaterial=vmatBlue,
55+
solidMaterial="DryWood",
56+
collision=true)),
57+
jointZ = FreeMotion(obj1=:frameZ, obj2=:coneZ,
58+
r=Var(init=[0.0, 0.0, 0.0]),
59+
rot=Var(init=[-60*u"°", 0.0, 0.0]),
60+
v=Var(init=[1.0, 0.0, 0.0]))
61+
)
62+
63+
bouncingFrustums = @instantiateModel(buildModia3D(BouncingFrustums), unitless=true, log=false, logStateSelection=false, logCode=false)
64+
65+
stopTime = 1.2
66+
tolerance = 1e-8
67+
requiredFinalStates = missing
68+
simulate!(bouncingFrustums, stopTime=stopTime, tolerance=tolerance, log=true, logStates=false, logEvents=false, requiredFinalStates=requiredFinalStates)
69+
70+
@usingModiaPlot
71+
plot(bouncingFrustums, [("jointX.r", "jointY.r", "jointZ.r") ("jointX.rot", "jointY.rot", "jointZ.rot")
72+
("jointX.v", "jointY.v", "jointZ.v") ("jointX.w", "jointY.w", "jointZ.w")], figure=1)
73+
74+
end

‎test/Collision/TwoCollidingBoxes.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ interval = 0.001
3838
if Sys.iswindows()
3939
requiredFinalStates = [-4.545411902572032, -0.0004339548925595526, 0.997958461213036, -7.952963881809473, -0.0005737829147323827, 1.0504837525229056, 1.5704114560517215, -0.000736947651219812, 4.178796873901921, -0.0006362640507553737, 0.0016059923768505658, 4.7729118080050466]
4040
else
41-
requiredFinalStates = [-4.539994107706228, -3.9895800164887376e-5, 1.0062426501885768, -7.945240606955788, -5.092515873863734e-5, 1.0612235633669154, 1.570658199884234, 0.000303670527292675, 4.197282711848592, 0.0006744468205883097, -0.0006760290317853639, 4.797671511349969]
41+
requiredFinalStates = [-8.657430783400523, 1.311823798979111e-5, 1.9101646652128834, -12.06565031388969, 1.060893587277513e-5, 1.3537002915036265, 1.5708365835208564, -8.409599774801432e-6, 5.808051018355625, -0.00017413228174145693, 4.0965706583202726e-5, 4.180685839091236]
4242
end
4343

4444
simulate!(twoCollidingBoxes, stopTime=stopTime, tolerance=tolerance, interval=interval, log=true, logStates=false, logEvents=false, requiredFinalStates=requiredFinalStates)

0 commit comments

Comments
 (0)