adapt support points

Author: AndreaNeumayr

src/Shapes/boundingBoxes.jl

@@ -49,36 +49,53 @@ that is the most extreme in direction of unit vector `e`.
 # or Andrea Neumayr took ideas for computing based on those books
 
 # [Gino v.d. Bergen, p. 135]
-@inline supportPoint_abs_Ellipsoid(shape::Ellipsoid, e_abs::SVector{3,T}) where {T} =
-    @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]) )
+@inline function supportPoint_abs_Ellipsoid(shape::Ellipsoid, e_abs::SVector{3,T}) where {T}
+    @inbounds begin
+        halfLengthX = T(0.5*shape.lengthX)
+        halfLengthY = T(0.5*shape.lengthY)
+        halfLengthZ = T(0.5*shape.lengthZ)
+        return SVector{3,T}( (halfLengthX)^2*e_abs[1], (halfLengthY)^2*e_abs[2], (halfLengthZ)^2*e_abs[3] )/norm(SVector{3,T}((halfLengthX)*e_abs[1], (halfLengthY)*e_abs[2], (halfLengthZ)*e_abs[3]) )
+    end
+end
 
 # [Gino v.d. Bergen, p. 135]
-@inline supportPoint_abs_Box(shape::Box, e_abs::SVector{3,T},  collisionSmoothingRadius::T) where {T} =
-    @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) )
+@inline function supportPoint_abs_Box(shape::Box, e_abs::SVector{3,T},  collisionSmoothingRadius::T) where {T}
+    @inbounds begin
+        halfLengthX = T(0.5*shape.lengthX)
+        halfLengthY = T(0.5*shape.lengthY)
+        halfLengthZ = T(0.5*shape.lengthZ)
+        return SVector{3,T}(
+            Basics.sign_eps(e_abs[1])*(halfLengthX-collisionSmoothingRadius),
+            Basics.sign_eps(e_abs[2])*(halfLengthY-collisionSmoothingRadius),
+            Basics.sign_eps(e_abs[3])*(halfLengthZ-collisionSmoothingRadius) )
+    end
+end
 
 # [Gino v.d. Bergen, p. 136, XenoCollide, p. 168, 169]
 @inline function supportPoint_abs_Cylinder(shape::Cylinder, e_abs::SVector{3,T}) where {T}
     @inbounds begin
+        halfLength = T(0.5*shape.length)
+        halfDiameter = T(0.5*shape.diameter)
         if shape.axis == 1
             enorm = norm(SVector(e_abs[2], e_abs[3]))
             if enorm <= Modia3D.neps
-                return Basics.sign_eps(e_abs[1])*SVector(shape.length/2, 0.0, 0.0)
+                return Basics.sign_eps(e_abs[1])*SVector(halfLength, 0.0, 0.0)
             else
-                return Basics.sign_eps(e_abs[1])*SVector(shape.length/2, 0.0, 0.0) + SVector(0.0, (shape.diameter/2)*e_abs[2], (shape.diameter/2)*e_abs[3])/enorm
+                return Basics.sign_eps(e_abs[1])*SVector(halfLength, 0.0, 0.0) + SVector(0.0, (halfDiameter)*e_abs[2], (halfDiameter)*e_abs[3])/enorm
             end
         elseif shape.axis == 2
             enorm = norm(SVector(e_abs[3], e_abs[1]))
             if enorm <= Modia3D.neps
-                return Basics.sign_eps(e_abs[2])*SVector(0.0, shape.length/2, 0.0)
+                return Basics.sign_eps(e_abs[2])*SVector(0.0, halfLength, 0.0)
             else
-                return Basics.sign_eps(e_abs[2])*SVector(0.0, shape.length/2, 0.0) + SVector((shape.diameter/2)*e_abs[1], 0.0, (shape.diameter/2)*e_abs[3])/enorm
+                return Basics.sign_eps(e_abs[2])*SVector(0.0, halfLength, 0.0) + SVector((halfDiameter)*e_abs[1], 0.0, (halfDiameter)*e_abs[3])/enorm
             end
         else
             enorm = norm(SVector(e_abs[1], e_abs[2]))
             if enorm <= Modia3D.neps
-                return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, shape.length/2)
+                return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, halfLength)
             else
-                return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, shape.length/2) + SVector((shape.diameter/2)*e_abs[1], (shape.diameter/2)*e_abs[2], 0.0)/enorm
+                return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, halfLength) + SVector((halfDiameter)*e_abs[1], (halfDiameter)*e_abs[2], 0.0)/enorm
             end
         end
     end
@@ -87,12 +104,14 @@ end
 # G. Hippmann: Cylinder + Sphere as bottom and top
 @inline function supportPoint_abs_Capsule(shape::Capsule, e_abs::SVector{3,T}) where {T}
     @inbounds begin
+        halfLength = T(0.5*shape.length)
+        halfDiameter = T(0.5*shape.diameter)
         if shape.axis == 1
-            return Basics.sign_eps(e_abs[1])*SVector(0.5*shape.length, 0.0, 0.0) + 0.5*shape.diameter*e_abs
+            return Basics.sign_eps(e_abs[1])*SVector(halfLength, 0.0, 0.0) + halfDiameter*e_abs
         elseif shape.axis == 2
-            return Basics.sign_eps(e_abs[2])*SVector(0.0, 0.5*shape.length, 0.0) + 0.5*shape.diameter*e_abs
+            return Basics.sign_eps(e_abs[2])*SVector(0.0, halfLength, 0.0) + halfDiameter*e_abs
         else
-            return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, 0.5*shape.length) + 0.5*shape.diameter*e_abs
+            return Basics.sign_eps(e_abs[3])*SVector(0.0, 0.0, halfLength) + halfDiameter*e_abs
         end
     end
 end
@@ -101,76 +120,77 @@ end
 # for frustum of a cone: A. Neumayr, G. Hippmann
 @inline function supportPoint_abs_Cone(shape::Cone, e_abs::SVector{3,T}) where {T}
     @inbounds begin
-        baseRadius = shape.diameter/2
-        rightCone = shape.topDiameter == 0.0
+        baseRadius = T(0.5*shape.diameter)
+        rightCone = T(shape.topDiameter) == T(0.0)
+        shapeLength = T(shape.length)
         if rightCone
-            sin_phi = baseRadius/sqrt(baseRadius^2 + shape.length^2)  # sin of base angle
+            sin_phi = T(baseRadius/sqrt(baseRadius^2 + shapeLength^2))  # sin of base angle
         else
-            topRadius = shape.topDiameter/2
-            diffRadius = baseRadius - topRadius
-            sin_phi = diffRadius/sqrt(diffRadius^2 + shape.length^2)  # sin of base angle
+            topRadius = T(0.5*shape.topDiameter)
+            diffRadius = T(baseRadius - topRadius)
+            sin_phi = T(diffRadius/sqrt(diffRadius^2 + shapeLength^2))  # sin of base angle
         end
         if shape.axis == 1
             value = e_abs[1] / norm(SVector(e_abs[1], e_abs[2], e_abs[3]))
             if value >= sin_phi
                 if rightCone
-                    return SVector(shape.length, 0.0, 0.0)  # apex is support point
+                    return SVector(shapeLength, 0.0, 0.0)  # apex is support point
                 else  # frustum of a cone
                     enorm = norm(SVector(e_abs[2], e_abs[3]))
                     if enorm > Modia3D.neps
-                        return SVector(shape.length, 0.0, 0.0) + SVector(0.0, topRadius*e_abs[2], topRadius*e_abs[3]) / enorm  # point on top circle is support point
+                        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
                     else
-                        return SVector(shape.length, 0.0, 0.0)  # top circle center is support point
+                        return SVector(shapeLength, 0.0, 0.0)  # top circle center is support point
                     end
                 end
             else
                 enorm = norm(SVector(e_abs[2], e_abs[3]))
                 if enorm > Modia3D.neps
                     return SVector(0.0, baseRadius*e_abs[2], baseRadius*e_abs[3]) / enorm  # point on base circle is support point
                 else
-                    return SVector(0.0, 0.0, 0.0)  # base circle center is support point
+                    return SVector{3,T}(0.0, 0.0, 0.0)  # base circle center is support point
                 end
             end
         elseif shape.axis == 2
             value = e_abs[2] / norm(SVector(e_abs[1], e_abs[2], e_abs[3]))
             if value >= sin_phi
                 if rightCone
-                    return SVector(0.0, shape.length, 0.0)  # apex is support point
+                    return SVector(0.0, shapeLength, 0.0)  # apex is support point
                 else  # frustum of a cone
                     enorm = norm(SVector(e_abs[3], e_abs[1]))
                     if enorm > Modia3D.neps
-                        return SVector(0.0, shape.length, 0.0) + SVector(topRadius*e_abs[1], 0.0, topRadius*e_abs[3]) / enorm  # point on top circle is support point
+                        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
                     else
-                        return SVector(0.0, shape.length, 0.0)  # top circle center is support point
+                        return SVector(0.0, shapeLength, 0.0)  # top circle center is support point
                     end
                 end
             else
                 enorm = norm(SVector(e_abs[3], e_abs[1]))
                 if enorm > Modia3D.neps
                     return SVector(baseRadius*e_abs[1], 0.0, baseRadius*e_abs[3]) / enorm  # point on base circle is support point
                 else
-                    return SVector(0.0, 0.0, 0.0)  # base circle center is support point
+                    return SVector{3,T}(0.0, 0.0, 0.0)  # base circle center is support point
                 end
             end
         else
             value = e_abs[3] / norm(SVector(e_abs[1], e_abs[2], e_abs[3]))
             if value >= sin_phi
                 if rightCone
-                    return SVector(0.0, 0.0, shape.length)  # apex is support point
+                    return SVector(0.0, 0.0, shapeLength)  # apex is support point
                 else  # frustum of a cone
                     enorm = norm(SVector(e_abs[1], e_abs[2]))
                     if enorm > Modia3D.neps
-                        return SVector(0.0, 0.0, shape.length) + SVector(topRadius*e_abs[1], topRadius*e_abs[2], 0.0) / enorm  # point on top circle is support point
+                        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
                     else
-                        return SVector(0.0, 0.0, shape.length)  # top circle center is support point
+                        return SVector(0.0, 0.0, shapeLength)  # top circle center is support point
                     end
                 end
             else
                 enorm = norm(SVector(e_abs[1], e_abs[2]))
                 if enorm > Modia3D.neps
                     return SVector(baseRadius*e_abs[1], baseRadius*e_abs[2], 0.0) / enorm  # point on base circle is support point
                 else
-                    return SVector(0.0, 0.0, 0.0)  # base circle center is support point
+                    return SVector{3,T}(0.0, 0.0, 0.0)  # base circle center is support point
                 end
             end
         end
@@ -180,26 +200,29 @@ end
 # G. Hippmann: Outer half circles of beam
 @inline function supportPoint_abs_Beam(shape::Beam, e_abs::SVector{3,T}) where {T}
     @inbounds begin
+        halfLength = T(0.5*shape.length)
+        halfWidth  = T(0.5*shape.width)
+        halfThickness = T(0.5*shape.thickness)
         if shape.axis == 1
             enorm = norm(SVector(e_abs[1], e_abs[2]))
             if enorm <= Modia3D.neps
-                return SVector(Basics.sign_eps(e_abs[1])*shape.length/2, 0.0, Basics.sign_eps(e_abs[3])*shape.thickness/2)
+                return SVector(Basics.sign_eps(e_abs[1])*halfLength, 0.0, Basics.sign_eps(e_abs[3])*halfThickness)
             else
-                return SVector(Basics.sign_eps(e_abs[1])*shape.length/2, 0.0, Basics.sign_eps(e_abs[3])*shape.thickness/2) + SVector(shape.width/2*e_abs[1], shape.width/2*e_abs[2], 0.0)/enorm
+                return SVector(Basics.sign_eps(e_abs[1])*halfLength, 0.0, Basics.sign_eps(e_abs[3])*halfThickness) + SVector(halfWidth*e_abs[1], halfWidth*e_abs[2], 0.0)/enorm
             end
         elseif shape.axis == 2
             enorm = norm(SVector(e_abs[2], e_abs[3]))
             if enorm <= Modia3D.neps
-                return SVector(Basics.sign_eps(e_abs[1])*shape.thickness/2, Basics.sign_eps(e_abs[2])*shape.length/2, 0.0)
+                return SVector(Basics.sign_eps(e_abs[1])*halfThickness, Basics.sign_eps(e_abs[2])*halfLength, 0.0)
             else
-                return SVector(Basics.sign_eps(e_abs[1])*shape.thickness/2, Basics.sign_eps(e_abs[2])*shape.length/2, 0.0) + SVector(0.0, shape.width/2*e_abs[2], shape.width/2*e_abs[3])/enorm
+                return SVector(Basics.sign_eps(e_abs[1])*halfThickness, Basics.sign_eps(e_abs[2])*halfLength, 0.0) + SVector(0.0, halfWidth*e_abs[2], halfWidth*e_abs[3])/enorm
             end
         else
             enorm = norm(SVector(e_abs[3], e_abs[1]))
             if enorm <= Modia3D.neps
-                return SVector(0.0, Basics.sign_eps(e_abs[2])*shape.thickness/2, Basics.sign_eps(e_abs[3])*shape.length/2)
+                return SVector(0.0, Basics.sign_eps(e_abs[2])*halfThickness, Basics.sign_eps(e_abs[3])*halfLength)
             else
-                return SVector(0.0, Basics.sign_eps(e_abs[2])*shape.thickness/2, Basics.sign_eps(e_abs[3])*shape.length/2) + SVector(shape.width/2*e_abs[1], 0.0, shape.width/2*e_abs[3])/enorm
+                return SVector(0.0, Basics.sign_eps(e_abs[2])*halfThickness, Basics.sign_eps(e_abs[3])*halfLength) + SVector(halfWidth*e_abs[1], 0.0, halfWidth*e_abs[3])/enorm
             end
         end
     end
@@ -210,7 +233,7 @@ end
     e_absVec = Vector{SVector{3,Float64}}(undef, 1)
     e_absVec[1] = e_abs
     (max_value, position) = findmax(broadcast(dot, shape.objPoints, e_absVec))
-    return shape.objPoints[position]
+    return SVector{3,T}(shape.objPoints[position])
 end