Rename normalized into normalize

src/algorithm/joint-configuration.hpp

@@ -105,14 +105,13 @@ namespace se3
   inline Eigen::VectorXd randomConfiguration(const Model & model);
 
   /**
-   * @brief      Normalize a configuration
+   * @brief         Normalize a configuration
    *
-   * @param[in]  model      Model
-   * @param[in]  q          Configuration to normalize
-   * @return     The normalized configuration
+   * @param[in]     model      Model
+   * @param[in,out] q          Configuration to normalize
    */
-  inline Eigen::VectorXd normalized(const Model & model,
-                                    const Eigen::VectorXd & q);
+  inline void normalize(const Model & model,
+                        Eigen::VectorXd & q);
 } // namespace se3 
 
 /* --- Details -------------------------------------------------------------------- */
@@ -307,35 +306,29 @@ namespace se3
     return randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit);
   }
 
-  struct NormalizedStep : public fusion::JointModelVisitor<NormalizedStep>
+  struct NormalizeStep : public fusion::JointModelVisitor<NormalizeStep>
   {
-    typedef boost::fusion::vector<const Eigen::VectorXd &,
-                                  Eigen::VectorXd &
-                                  > ArgsType;
+    typedef boost::fusion::vector<Eigen::VectorXd &> ArgsType;
 
-    JOINT_MODEL_VISITOR_INIT(NormalizedStep);
+    JOINT_MODEL_VISITOR_INIT(NormalizeStep);
 
     template<typename JointModel>
     static void algo(const se3::JointModelBase<JointModel> & jmodel,
-                     const Eigen::VectorXd & q,
-                     Eigen::VectorXd & result)
+                     Eigen::VectorXd & q)
     {
-      jmodel.jointConfigSelector(result) = jmodel.normalized(q);
+      jmodel.normalize(q);
     }
   };
 
-  inline Eigen::VectorXd
-  normalized(const Model & model,
-             const Eigen::VectorXd & q)
+  inline void
+  normalize(const Model & model,
+            Eigen::VectorXd & q)
   {
-    Eigen::VectorXd norma(model.nq);
     for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
     {
-      NormalizedStep::run(model.joints[i],
-                          NormalizedStep::ArgsType (q, norma)
-                          );
+      NormalizeStep::run(model.joints[i],
+                         NormalizeStep::ArgsType (q));
     }
-    return norma;
   }
 
 

src/multibody/joint/joint-base.hpp

@@ -339,20 +339,15 @@ namespace se3
     /**
      * @brief      Normalize a configuration
      *
-     * @param[in]  q     Configuration to normalize (size full model.nq)
-     *
-     * @return     The normalized configuration
+     * @param[in,out]  q     Configuration to normalize (size full model.nq)
      */
-    ConfigVector_t normalized(const Eigen::VectorXd & q) const
-    { return derived().normalized_impl(q); }
+    void normalize(Eigen::VectorXd & q) const
+    { return derived().normalize_impl(q); }
 
     /**
-     * @brief      Default implementation of normalized
+     * @brief      Default implementation of normalize
      */
-    ConfigVector_t normalized_impl(const Eigen::VectorXd & q) const
-    {
-      return q.segment<NQ>(idx_q());
-    }
+    void normalize_impl(Eigen::VectorXd &) const { }
 
     JointIndex i_id; // ID of the joint in the multibody list.
     int i_q;    // Index of the joint configuration in the joint configuration vector.

src/multibody/joint/joint-basic-visitors.hpp

@@ -157,16 +157,14 @@ namespace se3
   inline Eigen::VectorXd neutralConfiguration(const JointModelVariant & jmodel);
   
   /**
-   * @brief      Visit a JointModelVariant through JointNormalizedVisitor to compute
+   * @brief      Visit a JointModelVariant through JointNormalizeVisitor to compute
    *             the normalized configuration.
    *
-   * @param[in]  jmodel  The JointModelVariant
-   * @param[in]  q       configuration to normalize
-   *
-   * @return     The normalized configuration
+   * @param[in]      jmodel  The JointModelVariant
+   * @param[in,out]  q       configuration to normalize
    */
-  inline Eigen::VectorXd normalized(const JointModelVariant & jmodel,
-                                    const Eigen::VectorXd & q);
+  inline void normalize(const JointModelVariant & jmodel,
+                        Eigen::VectorXd & q);
 
   /**
    * @brief      Visit a JointModelVariant through JointNvVisitor to get the dimension of 

src/multibody/joint/joint-basic-visitors.hxx

@@ -233,25 +233,25 @@ namespace se3
   }
 
   /**
-   * @brief      JointNormalizedVisitor visitor
+   * @brief      JointNormalizeVisitor visitor
    */
-  class JointNormalizedVisitor: public boost::static_visitor<Eigen::VectorXd>
+  class JointNormalizeVisitor: public boost::static_visitor<>
   {
   public:
-    const Eigen::VectorXd & q;
+    Eigen::VectorXd & q;
 
-    JointNormalizedVisitor(const Eigen::VectorXd & q) : q(q) {}
+    JointNormalizeVisitor(Eigen::VectorXd & q) : q(q) {}
 
     template<typename D>
-    Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
-    { return jmodel.normalized(q); }
+    void operator()(const JointModelBase<D> & jmodel) const
+    { jmodel.normalize(q); }
     
-    static Eigen::VectorXd run(const JointModelVariant & jmodel, const Eigen::VectorXd & q)
-    { return boost::apply_visitor( JointNormalizedVisitor(q), jmodel ); }
+    static void run(const JointModelVariant & jmodel, Eigen::VectorXd & q)
+    { boost::apply_visitor( JointNormalizeVisitor(q), jmodel ); }
   };
-  inline Eigen::VectorXd normalized(const JointModelVariant & jmodel, const Eigen::VectorXd & q)
+  inline void normalize(const JointModelVariant & jmodel, Eigen::VectorXd & q)
   {
-    return JointNormalizedVisitor::run(jmodel, q);
+    JointNormalizeVisitor::run(jmodel, q);
   }
 
 

src/multibody/joint/joint-dense.hpp

@@ -197,11 +197,9 @@ namespace se3
       return result; 
     }
 
-    ConfigVector_t normalized_impl(const Eigen::VectorXd &) const
+    void normalize_impl(Eigen::VectorXd &) const
     {
-      ConfigVector_t result;
       assert(false && "JointModelDense is read-only, should not perform any calc");
-      return result;
     }
 
     JointModelDense() {}

src/multibody/joint/joint-free-flyer.hpp

@@ -374,11 +374,9 @@ namespace se3
       return q;
     } 
 
-    ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
+    void normalize_impl(Eigen::VectorXd& q) const
     {
-      ConfigVector_t result (q.segment<NQ>(idx_q()));
-      result.tail<4>().normalize();
-      return result;
+      q.segment<4>(idx_q()+3).normalize();
     }
 
     JointModelDense<NQ, NV> toDense_impl() const

src/multibody/joint/joint-spherical.hpp

@@ -399,9 +399,9 @@ namespace se3
       return q;
     } 
 
-    ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
+    void normalize_impl(Eigen::VectorXd& q) const
     {
-      return q.segment<NQ>(idx_q()).normalized();
+      q.segment<NQ>(idx_q()).normalize();
     }
 
     JointModelDense<NQ, NV> toDense_impl() const

src/multibody/joint/joint.hpp

@@ -146,9 +146,9 @@ namespace se3
       return ::se3::distance(*this, q0, q1);
     }
 
-    ConfigVector_t normalized_impl(const Eigen::VectorXd & q) const
-    { 
-      return ::se3::normalized(*this, q);
+    void normalize_impl(Eigen::VectorXd & q) const
+    {
+      return ::se3::normalize(*this, q);
     }
 
     ConfigVector_t neutralConfiguration_impl() const

unittest/joint-configurations.cpp

@@ -518,7 +518,7 @@ BOOST_AUTO_TEST_CASE ( integrate_difference_test )
 
 }
 
-BOOST_AUTO_TEST_CASE ( normalized_test )
+BOOST_AUTO_TEST_CASE ( normalize_test )
 {
   using namespace se3;
 
@@ -526,12 +526,14 @@ BOOST_AUTO_TEST_CASE ( normalized_test )
   Model model = createModelWithAllJoints();
   se3::Data data(model);
 
-  Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
+  Eigen::VectorXd q (Eigen::VectorXd::Ones(model.nq));
+  se3::normalize(model, q);
 
-  Eigen::VectorXd qn = se3::normalized(model, q);
-
-  BOOST_CHECK(fabs(qn.segment<4>(3).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of freeflyer
-  BOOST_CHECK(fabs(qn.segment<4>(7).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of spherical joint
+  BOOST_CHECK(q.head<3>().isApprox(Eigen::VectorXd::Ones(3)));
+  BOOST_CHECK(fabs(q.segment<4>(3).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of freeflyer
+  BOOST_CHECK(fabs(q.segment<4>(7).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of spherical joint
+  const int n = model.nq - 7 - 4;
+  BOOST_CHECK(q.tail(n).isApprox(Eigen::VectorXd::Ones(n)));
 }
 
 BOOST_AUTO_TEST_SUITE_END ()