[BeamInterpolation] Update method rotateFrame to take a normalized ve…

…ctor (#71)

* [BeamInterpolation] Update method rotateFrame to reduce the number of call of vec3 normalize

* Add method RotateFrameForAlignNormalizedX which is not normalizing input vector

src/BeamAdapter/component/BeamInterpolation.h

@@ -220,7 +220,11 @@ class BeamInterpolation : public virtual sofa::core::objectmodel::BaseObject
                                           const Transform &global_H_local1,const Real &L,
                                           const Real& baryCoordMin, const Real& baryCoordMax);
 
-    void RotateFrameForAlignX(const Quat &input,  Vec3 &x, Quat &output);
+    /// Method to rotate a Frame define by a Quat @param input around an axis @param x, x will be normalized in method. Output is return inside @param output
+    void RotateFrameForAlignX(const Quat &input, Vec3 &x, Quat &output);
+
+    /// Method to rotate a Frame define by a Quat @param input around an axis @param x , x has to be normalized. Output is return inside @param output
+    void RotateFrameForAlignNormalizedX(const Quat& input, const Vec3& x, Quat& output);
 
     unsigned int getStateSize() const ;
 

src/BeamAdapter/component/BeamInterpolation.inl

@@ -51,9 +51,15 @@ using sofa::helper::ReadAccessor ;
 
 /////////////////////////// TOOL /////////////////////////////////////////////////////////////////
 template <class DataTypes>
-void BeamInterpolation<DataTypes>::RotateFrameForAlignX(const Quat &input, Vec3 &x, Quat &output)
+void BeamInterpolation<DataTypes>::RotateFrameForAlignX(const Quat& input, Vec3& x, Quat& output)
 {
     x.normalize();
+    return RotateFrameForAlignNormalizedX(input, x, output);
+}
+
+template <class DataTypes>
+void BeamInterpolation<DataTypes>::RotateFrameForAlignNormalizedX(const Quat &input, const Vec3 &x, Quat &output)
+{
     Vec3 x0=input.inverseRotate(x);
 
     Real cTheta=x0[0];
@@ -1244,21 +1250,22 @@ void BeamInterpolation<DataTypes>::InterpolateTransformUsingSpline(Transform& gl
 
         /// the tangent is computed by derivating the spline
         Vec3 n_x = P0 * (-3 * invBx2) + P1 * (3 - 12 * baryCoord + 9 * bx2) + P2 * (6 * baryCoord - 9 * bx2) + P3 * (3 * bx2);
+        n_x.normalize();
 
         /// try to interpolate the "orientation" (especially the torsion) the best possible way...
         /// (but other ways should exit...)
         Quat R0, R1, Rslerp;
 
         ///      1. The frame at each node of the beam are rotated to align x along n_x
-        RotateFrameForAlignX(global_H_local0.getOrientation(), n_x, R0);
-        RotateFrameForAlignX(global_H_local1.getOrientation(), n_x, R1);
+        RotateFrameForAlignNormalizedX(global_H_local0.getOrientation(), n_x, R0);
+        RotateFrameForAlignNormalizedX(global_H_local1.getOrientation(), n_x, R1);
 
         ///     2. We use the "slerp" interpolation to find a solution "in between" these 2 solution R0, R1
         Rslerp.slerp(R0,R1, (float)baryCoord,true);
         Rslerp.normalize();
 
         ///     3. The result is not necessarily alligned with n_x, so we re-aligned Rslerp to obtain a quatResult.
-        RotateFrameForAlignX(Rslerp, n_x,quatResult);
+        RotateFrameForAlignNormalizedX(Rslerp, n_x,quatResult);
     }
 
     global_H_localResult.set(posResult, quatResult);

src/BeamAdapter/component/WireBeamInterpolation.inl

@@ -125,13 +125,14 @@ void WireBeamInterpolation<DataTypes>::getSplineRestTransform(unsigned int edgeI
     if (this->isControlled() && this->m_restShape!=nullptr)
     {
         const Vec2 &curvAbs = this->d_curvAbsList.getValue()[edgeInList];
+        auto restShape = this->m_restShape.get();
 
         Real x_middle = (curvAbs.x() + curvAbs.y()) / 2;
         Transform global_H_local_middle, global_H_local_0, global_H_local_1;
 
-        this->m_restShape.get()->getRestTransformOnX(global_H_local_middle, x_middle);
-        this->m_restShape.get()->getRestTransformOnX(global_H_local_0, curvAbs.x());
-        this->m_restShape.get()->getRestTransformOnX(global_H_local_1, curvAbs.y());
+        restShape->getRestTransformOnX(global_H_local_middle, x_middle);
+        restShape->getRestTransformOnX(global_H_local_0, curvAbs.x());
+        restShape->getRestTransformOnX(global_H_local_1, curvAbs.y());
 
         local_H_local0_rest = global_H_local_middle.inversed() * global_H_local_0;
         local_H_local1_rest = global_H_local_middle.inversed() * global_H_local_1;