Animation export: Fix initial orientation

- Use consistently Float64 for animation data to avoid wrong initial orientation caused by round-off errors (e.g. in test models BouncingCapsules.jl, BouncingCones.jl and NewtonsCradle.jl).
- Simplify total quaternion calculation for animation.

Author: GerhardHippmann

src/AnimationExport/exportAnimation.jl

@@ -188,7 +188,7 @@ end
 
 function getObjectInfo(obj, geometry, material, initPos, initRot; r_obj=Modia3D.ZeroVector3D, R_obj=Modia3D.NullRotation, scale=ones(3))
     name = String(Modia3D.fullName(obj))
-    return (; name=name, uuid=name2uuid(name), type=:Mesh, geometry=get(geometry, :uuid, nothing), material=get(material, :uuid, ""), position=initPos+initRot'*r_obj, rotation=Modia3D.rot123fromR(R_obj*initRot), scale=scale )
+    return (; name=name, uuid=name2uuid(name), type=:Mesh, geometry=get(geometry, :uuid, nothing), material=get(material, :uuid, ""), position=initPos+initRot'*r_obj, rotation=Modia3D.rot123fromR(R_obj*initRot), scale=scale)
 end
 
 printVisuMaterialToJSON(obj, visuMaterial) = nothing
@@ -264,7 +264,7 @@ function addCameras!(object, uuid4, position, orientation)
 
     name = "Perspective Camera"
     uuid = string(UUIDs.uuid5(UUIDs.UUID(uuid4), name))
-    camera = (; name=name, uuid=uuid, type="PerspectiveCamera", position=position, rotation=Modia3D.rot123fromR(SMatrix{3,3,Float64}(orientation)))
+    camera = (; name=name, uuid=uuid, type="PerspectiveCamera", position=position, rotation=Modia3D.rot123fromR(orientation))
     push!(object.children, camera)
 
     name="Orthographic Camera XY"
@@ -286,7 +286,7 @@ end
 function createAnimationPositionTrack(tracks, animation, obj, iobj, r_obj)
     keys = []
     for istep in 1:length(animation)
-        r_all = animation[istep].objectData[iobj].position + (Modia3D.from_q(SVector{4,Float64}(animation[istep].objectData[iobj].quaternion)))'*r_obj
+        r_all = animation[istep].objectData[iobj].position + (Modia3D.from_q(animation[istep].objectData[iobj].quaternion))'*r_obj
         push!(keys, (; time=animation[istep].time, value=r_all) )
     end
     name = string(Modia3D.fullName(obj), ".position")
@@ -296,8 +296,8 @@ end
 function createAnimationQuaternionTrack(tracks, animation, obj, iobj, R_obj)
     keys = []
     for istep in 1:length(animation)
-        R_all = Modia3D.from_R(R_obj*Modia3D.from_q(SVector{4,Float64}(animation[istep].objectData[iobj].quaternion)) )
-        push!(keys, (; time=animation[istep].time, value=R_all) )
+        q_all = Modia3D.absoluteRotation(animation[istep].objectData[iobj].quaternion, Modia3D.from_R(R_obj))
+        push!(keys, (; time=animation[istep].time, value=q_all) )
     end
     name = string(Modia3D.fullName(obj), ".quaternion")
     push!(tracks, (; name=name, uuid=name2uuid(name), type="quaternion", keys ) )
@@ -339,7 +339,7 @@ function exportAnimation(scene)
             tracks = []
             for obj in allVisuElements
                 iobj = iobj + 1
-                (r_obj, R_obj) = printObjectToJSON(object, elements, obj, initPos=animation[1].objectData[iobj].position, initRot=Modia3D.from_q(SVector{4,Float64}(animation[1].objectData[iobj].quaternion)) )
+                (r_obj, R_obj) = printObjectToJSON(object, elements, obj, initPos=animation[1].objectData[iobj].position, initRot=Modia3D.from_q(animation[1].objectData[iobj].quaternion))
                 if !isnothing(R_obj)
                     createAnimationPositionTrack(tracks, animation, obj, iobj, r_obj)
                     createAnimationQuaternionTrack(tracks, animation, obj, iobj, R_obj)

src/Composition/joints/FreeMotion.jl

@@ -17,14 +17,14 @@ In case of singular state (angle2 +-90 deg, x and z axes parallel) `rot3_guess`
 """
 function rot123fromR(R::Frames.RotationMatrix; rot3_guess=0.0)
 
-    sbe = R[3,1]
+    sbe = clamp(R[3,1], -1.0, 1.0)
     cbe2 = 1.0 - sbe*sbe
     if (cbe2 > 1e-12)
         al = atan(-R[3,2], R[3,3])
         be = atan(sbe/sqrt(cbe2))
         ga = atan(-R[2,1], R[1,1])
     else
-        # be is 90 deg -> singular, only al+ga is defined -> set ga to zero
+        # be is 90 deg -> singular, only al+ga is defined -> set ga to rot3_guess
         al = atan(R[2,3], R[2,2]) - sign(sbe)*rot3_guess  # ???
         be = sign(sbe)*0.5*pi
         ga = rot3_guess
@@ -41,7 +41,7 @@ In case of singular state (angle2 +-90 deg, x and y axes parallel) `rot3_guess`
 """
 function rot132fromR(R::Frames.RotationMatrix; rot3_guess=0.0)
 
-    sga = - R[2,1]
+    sga = clamp(-R[2,1], -1.0, 1.0)
     cga2 = 1.0 - sga*sga
     if (cga2 > 1e-12)
         al = atan(R[2,3], R[2,2])

src/Composition/scene.jl

@@ -252,8 +252,8 @@ function cameraPosition(distance, longitude, latitude, cameraUpDir, sceneUpDir)
     else
         # camera upwards := +y direction
     end
-    r_Camera = R_view' * r_Camera
-    R_Camera = R_Camera * R_view
+    r_Camera = SVector{3,Float64}(R_view' * r_Camera)
+    R_Camera = SMatrix{3,3,Float64}(R_Camera * R_view)
     return (r_Camera, R_Camera)
 end
 
@@ -396,12 +396,12 @@ struct SceneOptions <: Modia3D.AbstractSceneOptions
 end
 
 struct animationData
-    position::SVector{3,Float32}    # abs. Object3D position
-    quaternion::SVector{4,Float32}  # abs. Object3D quaternion
+    position::SVector{3,Float64}    # abs. Object3D position
+    quaternion::SVector{4,Float64}  # abs. Object3D quaternion
 end
 
 struct animationStep
-    time::Float32                      # simulation time
+    time::Float64                      # simulation time
     objectData::Vector{animationData}  # animation data of Object3Ds
 end