From 460709fc2124d6ed5d2c9ee53b34f9c188aa0407 Mon Sep 17 00:00:00 2001
From: Vladimir Poliakov <vladimir.poliakov@kuleuven.be>
Date: Mon, 16 Sep 2019 08:31:41 +0200
Subject: [PATCH] Added Rigid to Euler orientation export

---
 modules/SofaGeneralEngine/RigidToQuatEngine.h   | 1 +
 modules/SofaGeneralEngine/RigidToQuatEngine.inl | 6 +++++-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/modules/SofaGeneralEngine/RigidToQuatEngine.h b/modules/SofaGeneralEngine/RigidToQuatEngine.h
index b5da7b05159..1c1010e097c 100644
--- a/modules/SofaGeneralEngine/RigidToQuatEngine.h
+++ b/modules/SofaGeneralEngine/RigidToQuatEngine.h
@@ -82,6 +82,7 @@ class RigidToQuatEngine : public sofa::core::DataEngine
     //
     Data<helper::vector<Vec3 > > f_positions; ///< Positions (Vector of 3)
     Data<helper::vector<Quat> > f_orientations; ///< Orientations (Quaternion)
+    Data<helper::vector<Vec3> > f_orientationsEuler; ///< Orientation (Euler angle)
     Data<helper::vector<RigidVec3> > f_rigids; ///< Rigid (Position + Orientation)
 };
 
diff --git a/modules/SofaGeneralEngine/RigidToQuatEngine.inl b/modules/SofaGeneralEngine/RigidToQuatEngine.inl
index b4bed2184c4..9418a631f7c 100644
--- a/modules/SofaGeneralEngine/RigidToQuatEngine.inl
+++ b/modules/SofaGeneralEngine/RigidToQuatEngine.inl
@@ -37,6 +37,7 @@ template <class DataTypes>
 RigidToQuatEngine<DataTypes>::RigidToQuatEngine()
     : f_positions( initData (&f_positions, "positions", "Positions (Vector of 3)") )
     , f_orientations( initData (&f_orientations, "orientations", "Orientations (Quaternion)") )
+    , f_orientationsEuler( initData (&f_orientationsEuler, "orientationsEuler", "Orientations (Euler angle)") )
     , f_rigids( initData (&f_rigids, "rigids", "Rigid (Position + Orientation)") )
 {
     //
@@ -58,6 +59,7 @@ void RigidToQuatEngine<DataTypes>::init()
 
     addOutput(&f_positions);
     addOutput(&f_orientations);
+    addOutput(&f_orientationsEuler);
 
     setDirtyValue();
 }
@@ -72,18 +74,20 @@ template <class DataTypes>
 void RigidToQuatEngine<DataTypes>::doUpdate()
 {
     helper::ReadAccessor< Data< helper::vector<RigidVec3> > > rigids = f_rigids;
-
     helper::WriteOnlyAccessor< Data< helper::vector<Vec3> > > positions = f_positions;
     helper::WriteOnlyAccessor< Data< helper::vector<Quat> > > orientations = f_orientations;
+    helper::WriteOnlyAccessor< Data< helper::vector<Vec3> > > orientationsEuler = f_orientationsEuler;
 
     unsigned int sizeRigids = rigids.size();
     positions.resize(sizeRigids);
     orientations.resize(sizeRigids);
+    orientationsEuler.resize(sizeRigids);
     for (unsigned int i=0 ; i< sizeRigids ; i++)
     {
         RigidVec3 r = rigids[i];
         positions[i] = r.getCenter();
         orientations[i] = r.getOrientation();
+        orientationsEuler[i] = orientations[i].toEulerVector();
     }
 }