Skip to content

Commit 53b4117

Browse files
GerhardHippmannGerhardHippmann
authored
Merge pull request #55 from ModiaSim/an_treatEllipsoidsProperly
An treat ellipsoids properly
2 parents 9bfda34 + 32d717c commit 53b4117

File tree

6 files changed

+66
-27
lines changed

6 files changed

+66
-27
lines changed

‎src/Composition/_module.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,7 @@ export InteractionBehavior, Gripper, Movable, Lockable, NoInteraction
7070

7171
export rot123fromR, rot132fromR, Rfromrot123, Rfromrot132
7272

73-
export supportPoint, boundingBox!
73+
export supportPoint, boundingBox!, contactPointIsLocallyBijectiveToNormal
7474

7575
@enum InteractionBehavior Gripper Movable Lockable NoInteraction
7676

‎src/Composition/supportPoints.jl‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -144,3 +144,13 @@ function boundingBox!(obj::Composition.Object3D, AABB::Basics.BoundingBox; tight
144144
end
145145
return AABB
146146
end
147+
148+
149+
function contactPointIsLocallyBijectiveToNormal(obj::Composition.Object3D)
150+
isBijective = false
151+
shapeKind = obj.shapeKind
152+
if shapeKind == Modia3D.SphereKind || shapeKind == Modia3D.EllipsoidKind
153+
isBijective = true
154+
end
155+
return isBijective
156+
end

‎src/contactDetection/ContactDetectionMPR/mpr.jl‎

Lines changed: 13 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -416,7 +416,7 @@ function mprGeneral(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Comp
416416
end
417417

418418

419-
function mprTwoSpheres(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D,
419+
function distanceTwoSpheres(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Composition.Object3D, shapeB::Modia3D.Composition.Object3D,
420420
sphereA::Shapes.Sphere, sphereB::Shapes.Sphere) where {T}
421421
neps = Modia3D.nepsType(T)
422422
radiusA = T(sphereA.diameter*0.5)
@@ -445,31 +445,22 @@ function mpr(ch::Composition.ContactDetectionMPR_handler{T}, shapeA::Composition
445445
sphereA::Shapes.Sphere = shapeA.shape
446446
sphereB::Shapes.Sphere = shapeB.shape
447447
(distance, contactPoint1, contactPoint2, normal, supportPointsDefined,
448-
support1A, support1B, support2A, support2B, support3A, support3B) = mprTwoSpheres(ch, shapeA, shapeB, sphereA, sphereB)
449-
elseif shapeKindA != Modia3D.SphereKind && shapeKindB != Modia3D.SphereKind
450-
(distance, contactPoint1, contactPoint2, normal, supportPointsDefined,
451-
support1A, support1B, support2A, support2B, support3A, support3B) = mprGeneral(ch, shapeA, shapeB)
448+
support1A, support1B, support2A, support2B, support3A, support3B) = distanceTwoSpheres(ch, shapeA, shapeB, sphereA, sphereB)
452449
else
453450
(distance, contactPoint1, contactPoint2, normal, supportPointsDefined,
454451
support1A, support1B, support2A, support2B, support3A, support3B) = mprGeneral(ch, shapeA, shapeB)
455452

456-
if shapeKindA == Modia3D.SphereKind
457-
centroidSphere = getCentroid(shapeA)
458-
sphereA1::Shapes.Sphere = shapeA.shape
459-
radius = T(sphereA1.diameter*0.5)
460-
contactPointSphere = centroidSphere + radius*normal
461-
contactPointOtherShape = contactPointSphere + distance*normal
462-
contactPoint1 = contactPointSphere
463-
contactPoint2 = contactPointOtherShape
464-
elseif shapeKindB == Modia3D.SphereKind
465-
normalLocal = -normal
466-
centroidSphere = getCentroid(shapeB)
467-
sphereB1::Shapes.Sphere = shapeB.shape
468-
radius = T(sphereB1.diameter*0.5)
469-
contactPointSphere = centroidSphere + radius*normalLocal
470-
contactPointOtherShape = contactPointSphere + distance*normalLocal # distance is negative (otherwise direction of normal must be changed)
471-
contactPoint1 = contactPointOtherShape
472-
contactPoint2 = contactPointSphere
453+
if Modia3D.contactPointIsLocallyBijectiveToNormal(shapeA) && Modia3D.contactPointIsLocallyBijectiveToNormal(shapeB)
454+
algebraicSign = sign(distance)
455+
contactPoint1 = Modia3D.supportPoint(shapeA, normal)
456+
contactPoint2 = Modia3D.supportPoint(shapeB, -normal)
457+
distance = algebraicSign * norm(contactPoint2 - contactPoint1)
458+
elseif Modia3D.contactPointIsLocallyBijectiveToNormal(shapeA) && !Modia3D.contactPointIsLocallyBijectiveToNormal(shapeB)
459+
contactPoint1 = Modia3D.supportPoint(shapeA, normal)
460+
contactPoint2 = contactPoint1 + distance*normal
461+
elseif !Modia3D.contactPointIsLocallyBijectiveToNormal(shapeA) && Modia3D.contactPointIsLocallyBijectiveToNormal(shapeB)
462+
contactPoint2 = Modia3D.supportPoint(shapeB, -normal)
463+
contactPoint1 = contactPoint2 + distance*-normal
473464
end
474465
end
475466
return (Float64(distance), SVector{3,Float64}(contactPoint1), SVector{3,Float64}(contactPoint2), SVector{3,Float64}(normal), supportPointsDefined,

‎test/Collision/BouncingEllipsoid.jl‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -29,13 +29,11 @@ BouncingEllipsoid = Model(
2929

3030
bouncingEllipsoid = @instantiateModel(buildModia3D(BouncingEllipsoid), unitless=true, log=false, logStateSelection=false, logCode=false)
3131

32-
#@show bouncingEllipsoid.parameterExpressions
33-
#@show bouncingEllipsoid.parameters
3432

3533
stopTime = 2.0
3634
tolerance = 1e-8
37-
requiredFinalStates = [-0.45515799147473335, 0.05676993769652407, 1.178459912041914, -0.10966302200496965, -0.367365395565088, 0.2903115082177484, 8.759390647284498, -1.3086676277009546, -2.341163220615624, -0.10934417069415246, -1.1736634425032262, -5.6404159425069835]
38-
simulate!(bouncingEllipsoid, stopTime=stopTime, tolerance=tolerance, log=true, logStates=true, logEvents=false, requiredFinalStates=requiredFinalStates)
35+
requiredFinalStates = [-0.4545597005152044, 0.05683453468299216, 1.1778521103823525, -0.1092425125321821, -0.3622646259841156, 0.28841460521917095, 8.7681878487027, -1.30949181179306, -2.334787231141352, -0.08949785426141968, -1.1664892577220993, -5.628568253333571]
36+
simulate!(bouncingEllipsoid, stopTime=stopTime, tolerance=tolerance, log=true, requiredFinalStates=requiredFinalStates)
3937

4038
@usingModiaPlot
4139
plot(bouncingEllipsoid, ["free.r" "free.rot"; "free.v" "free.w"], figure=1)

‎test/Collision/BouncingEllipsoidOnSphere.jl‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
module BouncingEllipsoidSimulation
2+
3+
using ModiaLang
4+
import Modia3D
5+
using Modia3D.ModiaInterface
6+
7+
BouncingEllipsoid = Model(
8+
boxHeigth = 0.1,
9+
gravField = UniformGravityField(g=9.81, n=[0, -1, 0]),
10+
world = Object3D(feature=Scene(gravityField=:gravField,
11+
mprTolerance=1.0e-19,
12+
defaultFrameLength=0.2,
13+
enableContactDetection=true)),
14+
worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.5))),
15+
ground = Object3D(parent=:world,
16+
translation=:[0.0,-boxHeigth/2,0.0],
17+
feature=Solid(shape=Sphere(diameter=1.5),
18+
visualMaterial=VisualMaterial(color="DarkGreen", transparency=0.5),
19+
solidMaterial="Steel",
20+
collision=true)),
21+
ellipsoid = Object3D(feature=Solid(shape=Ellipsoid(lengthX=0.1, lengthY=0.2, lengthZ=0.3),
22+
visualMaterial=VisualMaterial(color="Blue"),
23+
solidMaterial="Steel",
24+
collision=true)),
25+
free = FreeMotion(obj1=:world, obj2=:ellipsoid, r=Var(init=[0.0, 1.0, 0.0]), w=Var(init=[5.0, 0.0, -2.0]))
26+
)
27+
28+
bouncingEllipsoid = @instantiateModel(buildModia3D(BouncingEllipsoid), unitless=true, log=false, logStateSelection=false, logCode=false)
29+
30+
31+
stopTime = 2.0
32+
tolerance = 1e-8
33+
requiredFinalStates = [1.9013751140911312, -5.182612613347552, -0.6285338998457255, 1.3889960373284438, -10.631628266236548, -0.35235710867283077, 2.6873629925159035, 0.23896916015991412, -8.030636545239265, 6.713810967152868, -1.2032068837594854, -12.984033346958665]
34+
simulate!(bouncingEllipsoid, stopTime=stopTime, tolerance=tolerance, log=true, requiredFinalStates=requiredFinalStates)
35+
36+
@usingModiaPlot
37+
plot(bouncingEllipsoid, ["free.r" "free.rot"; "free.v" "free.w"], figure=1)
38+
39+
end

‎test/includeTests.jl‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ Test.@testset "Collision" begin
4444
include(joinpath("Collision", "BouncingSphere.jl"))
4545
include(joinpath("Collision", "BouncingSphereFreeMotion.jl"))
4646
include(joinpath("Collision", "BouncingEllipsoid.jl"))
47+
include(joinpath("Collision", "BouncingEllipsoidOnSphere.jl"))
4748
include(joinpath("Collision", "TwoCollidingBalls.jl"))
4849
include(joinpath("Collision", "TwoCollidingBoxes.jl"))
4950
include(joinpath("Collision", "CollidingCylinders.jl"))

0 commit comments

Comments
 (0)