Skip to content

Commit 9bc5158

Browse files
AndreaNeumayrAndreaNeumayr
committed
MPRFloatType for supportPoints.jl and boundingBoxes.jl
1 parent d63fee3 commit 9bc5158

File tree

2 files changed

+25
-25
lines changed

2 files changed

+25
-25
lines changed

‎src/Composition/supportPoints.jl‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,32 +1,32 @@
1-
function supportPoint(obj::Composition.Object3D, e::SVector{3,Float64})
1+
function supportPoint(obj::Composition.Object3D, e::SVector{3,T}) where {T}
22
shapeKind = obj.shapeKind
33
solid::Modia3D.Solid = obj.feature
4-
collisionSmoothingRadius = solid.collisionSmoothingRadius
4+
collisionSmoothingRadius = T(solid.collisionSmoothingRadius)
55

66
if shapeKind == Modia3D.SphereKind
77
#sphere::Modia3D.Sphere = obj.shape
8-
return Modia3D.supportPoint_Sphere(obj.shape, obj.r_abs, obj.R_abs, e)
8+
return Modia3D.supportPoint_Sphere(obj.shape, SVector{3, T}(obj.r_abs), SMatrix{3,3,T,9}(obj.R_abs), e)
99
elseif shapeKind == Modia3D.EllipsoidKind
1010
#ellipsoid::Modia3D.Ellipsoid = obj.shape
11-
return Modia3D.supportPoint_Ellipsoid(obj.shape, obj.r_abs, obj.R_abs, e)
11+
return Modia3D.supportPoint_Ellipsoid(obj.shape, SVector{3, T}(obj.r_abs), SMatrix{3,3,T,9}(obj.R_abs), e)
1212
elseif shapeKind == Modia3D.BoxKind
1313
#box::Modia3D.Box = obj.shape
14-
return Modia3D.supportPoint_Box(obj.shape, obj.r_abs, obj.R_abs, e, collisionSmoothingRadius)
14+
return Modia3D.supportPoint_Box(obj.shape, SVector{3, T}(obj.r_abs), SMatrix{3,3,T,9}(obj.R_abs), e, collisionSmoothingRadius)
1515
elseif shapeKind == Modia3D.CylinderKind
1616
#cylinder::Modia3D.Cylinder = obj.shape
17-
return Modia3D.supportPoint_Cylinder(obj.shape, obj.r_abs, obj.R_abs, e, collisionSmoothingRadius)
17+
return Modia3D.supportPoint_Cylinder(obj.shape, SVector{3, T}(obj.r_abs), SMatrix{3,3,T,9}(obj.R_abs), e, collisionSmoothingRadius)
1818
elseif shapeKind == Modia3D.ConeKind
1919
#cone::Modia3D.Cone = obj.shape
20-
return Modia3D.supportPoint_Cone(obj.shape, obj.r_abs, obj.R_abs, e, collisionSmoothingRadius)
20+
return Modia3D.supportPoint_Cone(obj.shape, SVector{3, T}(obj.r_abs), SMatrix{3,3,T,9}(obj.R_abs), e, collisionSmoothingRadius)
2121
elseif shapeKind == Modia3D.CapsuleKind
2222
#capsule::Modia3D.Capsule = obj.shape
23-
return Modia3D.supportPoint_Capsule(obj.shape, obj.r_abs, obj.R_abs, e)
23+
return Modia3D.supportPoint_Capsule(obj.shape, SVector{3, T}(obj.r_abs), SMatrix{3,3,T,9}(obj.R_abs), e)
2424
elseif shapeKind == Modia3D.BeamKind
2525
#beam::Modia3D.Beam = obj.shape
26-
return Modia3D.supportPoint_Beam(obj.shape, obj.r_abs, obj.R_abs, e, collisionSmoothingRadius)
26+
return Modia3D.supportPoint_Beam(obj.shape, SVector{3, T}(obj.r_abs), SMatrix{3,3,T,9}(obj.R_abs), e, collisionSmoothingRadius)
2727
elseif shapeKind == Modia3D.FileMeshKind
2828
#fileMesh::Modia3D.FileMesh = obj.shape
29-
return Modia3D.supportPoint_FileMesh(obj.shape, obj.r_abs, obj.R_abs, e)
29+
return Modia3D.supportPoint_FileMesh(obj.shape, SVector{3, T}(obj.r_abs), SMatrix{3,3,T,9}(obj.R_abs), e)
3030
else
3131
error("not supported shape for support points")
3232
end

‎src/Shapes/boundingBoxes.jl‎

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -17,28 +17,28 @@ that is the most extreme in direction of unit vector `e`.
1717
- `R_abs::AbstractMatrix`: Rotation matrix to rotate world frame in `shape` reference frame.
1818
- `e::AbstractVector`: Unit vector pointing into the desired direction.
1919
"""
20-
@inline supportPoint_Box(shape::Box, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}, collisionSmoothingRadius::Float64) =
20+
@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} =
2121
r_abs + R_abs'*supportPoint_abs_Box(shape, R_abs*e, collisionSmoothingRadius) + e*collisionSmoothingRadius
2222
#
23-
@inline supportPoint_Cylinder( shape::Cylinder, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}, collisionSmoothingRadius::Float64) =
23+
@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} =
2424
r_abs + R_abs'*supportPoint_abs_Cylinder(shape, R_abs*e) + e*collisionSmoothingRadius
2525

26-
@inline supportPoint_Cone(shape::Cone, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}, collisionSmoothingRadius::Float64) =
26+
@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} =
2727
r_abs + R_abs'*supportPoint_abs_Cone(shape, R_abs*e) + e*collisionSmoothingRadius
2828

29-
@inline supportPoint_Capsule(shape::Capsule, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}) =
29+
@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

32-
@inline supportPoint_Beam(shape::Beam, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}, collisionSmoothingRadius::Float64) =
32+
@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} =
3333
r_abs + R_abs'*supportPoint_abs_Beam(shape, R_abs*e) + e*collisionSmoothingRadius
3434

35-
@inline supportPoint_Sphere(shape::Sphere, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}) =
35+
@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
3737

38-
@inline supportPoint_Ellipsoid(shape::Ellipsoid, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}) =
38+
@inline supportPoint_Ellipsoid(shape::Ellipsoid, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}) where {T} =
3939
r_abs + R_abs'*supportPoint_abs_Ellipsoid(shape, R_abs*e)
4040

41-
@inline supportPoint_FileMesh(shape::FileMesh, r_abs::SVector{3,Float64}, R_abs::SMatrix{3,3,Float64,9}, e::SVector{3,Float64}) =
41+
@inline supportPoint_FileMesh(shape::FileMesh, r_abs::SVector{3,T}, R_abs::SMatrix{3,3,T,9}, e::SVector{3,T}) where {T} =
4242
r_abs + R_abs'*supportPoint_abs_FileMesh(shape, R_abs*e)
4343

4444

@@ -49,15 +49,15 @@ that is the most extreme in direction of unit vector `e`.
4949
# or Andrea Neumayr took ideas for computing based on those books
5050

5151
# [Gino v.d. Bergen, p. 135]
52-
@inline supportPoint_abs_Ellipsoid(shape::Ellipsoid, e_abs::SVector{3,Float64}) =
52+
@inline supportPoint_abs_Ellipsoid(shape::Ellipsoid, e_abs::SVector{3,T}) where {T} =
5353
@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]) )
5454

5555
# [Gino v.d. Bergen, p. 135]
56-
@inline supportPoint_abs_Box(shape::Box, e_abs::SVector{3,Float64}, collisionSmoothingRadius::Float64) =
56+
@inline supportPoint_abs_Box(shape::Box, e_abs::SVector{3,T}, collisionSmoothingRadius::T) where {T} =
5757
@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) )
5858

5959
# [Gino v.d. Bergen, p. 136, XenoCollide, p. 168, 169]
60-
@inline function supportPoint_abs_Cylinder(shape::Cylinder, e_abs::SVector{3,Float64})
60+
@inline function supportPoint_abs_Cylinder(shape::Cylinder, e_abs::SVector{3,T}) where {T}
6161
@inbounds begin
6262
if shape.axis == 1
6363
enorm = norm(SVector(e_abs[2], e_abs[3]))
@@ -85,7 +85,7 @@ that is the most extreme in direction of unit vector `e`.
8585
end
8686

8787
# G. Hippmann: Cylinder + Sphere as bottom and top
88-
@inline function supportPoint_abs_Capsule(shape::Capsule, e_abs::SVector{3,Float64})
88+
@inline function supportPoint_abs_Capsule(shape::Capsule, e_abs::SVector{3,T}) where {T}
8989
@inbounds begin
9090
if shape.axis == 1
9191
return Basics.sign_eps(e_abs[1])*SVector(0.5*shape.length, 0.0, 0.0) + 0.5*shape.diameter*e_abs
@@ -99,7 +99,7 @@ end
9999

100100
# for cone: [Gino v.d. Bergen, p. 136]]
101101
# for frustum of a cone: A. Neumayr, G. Hippmann
102-
@inline function supportPoint_abs_Cone(shape::Cone, e_abs::SVector{3,Float64})
102+
@inline function supportPoint_abs_Cone(shape::Cone, e_abs::SVector{3,T}) where {T}
103103
@inbounds begin
104104
baseRadius = shape.diameter/2
105105
rightCone = shape.topDiameter == 0.0
@@ -178,7 +178,7 @@ end
178178
end
179179

180180
# G. Hippmann: Outer half circles of beam
181-
@inline function supportPoint_abs_Beam(shape::Beam, e_abs::SVector{3,Float64})
181+
@inline function supportPoint_abs_Beam(shape::Beam, e_abs::SVector{3,T}) where {T}
182182
@inbounds begin
183183
if shape.axis == 1
184184
enorm = norm(SVector(e_abs[1], e_abs[2]))
@@ -206,7 +206,7 @@ end
206206
end
207207

208208
# [Gino v.d. Bergen, p. 131]
209-
@inline function supportPoint_abs_FileMesh(shape::FileMesh, e_abs::SVector{3,Float64})
209+
@inline function supportPoint_abs_FileMesh(shape::FileMesh, e_abs::SVector{3,T}) where {T}
210210
e_absVec = Vector{SVector{3,Float64}}(undef, 1)
211211
e_absVec[1] = e_abs
212212
(max_value, position) = findmax(broadcast(dot, shape.objPoints, e_absVec))

0 commit comments

Comments
 (0)