[Format]

loco-3d · Sep 24, 2019 · 88ffc79 · 88ffc79
1 parent 0e28b34
commit 88ffc79
Show file tree

Hide file tree

Showing 34 changed files with 4,808 additions and 5,276 deletions.
diff --git a/include/curves/MathDefs.h b/include/curves/MathDefs.h
@@ -1,16 +1,16 @@
 /**
-* \file Math.h
-* \brief Linear algebra and other maths definitions. Based on Eigen 3 or more
-* \author Steve T.
-* \version 0.1
-* \date 06/17/2013
-*
-* This file contains math definitions used
-* used throughout the library.
-* Preprocessors definition are used to use eitheir float
-* or double values, and 3 dimensional vectors for
-* the Point structure.
-*/
+ * \file Math.h
+ * \brief Linear algebra and other maths definitions. Based on Eigen 3 or more
+ * \author Steve T.
+ * \version 0.1
+ * \date 06/17/2013
+ *
+ * This file contains math definitions used
+ * used throughout the library.
+ * Preprocessors definition are used to use eitheir float
+ * or double values, and 3 dimensional vectors for
+ * the Point structure.
+ */
 
 #ifndef _SPLINEMATH
 #define _SPLINEMATH
@@ -20,23 +20,20 @@
 
 #include <vector>
 #include <utility>
-namespace curves{
-  //REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
-  template<typename _Matrix_Type_>
-  void PseudoInverse(_Matrix_Type_& pinvmat)
-  {
-    Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
-    _Matrix_Type_ m_sigma = svd.singularValues();
-    double pinvtoler= 1.e-6; // choose your tolerance widely!
-    _Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows());
-    for (long i=0; i<m_sigma.rows(); ++i)
-    {
-      if (m_sigma(i) > pinvtoler)
-      {
-        m_sigma_inv(i,i)=1.0/m_sigma(i);
-      }
+namespace curves {
+// REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
+template <typename _Matrix_Type_>
+void PseudoInverse(_Matrix_Type_& pinvmat) {
+  Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  _Matrix_Type_ m_sigma = svd.singularValues();
+  double pinvtoler = 1.e-6;  // choose your tolerance widely!
+  _Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows());
+  for (long i = 0; i < m_sigma.rows(); ++i) {
+    if (m_sigma(i) > pinvtoler) {
+      m_sigma_inv(i, i) = 1.0 / m_sigma(i);
     }
-    pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
   }
-} // namespace curves
-#endif //_SPLINEMATH
+  pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose());
+}
+}  // namespace curves
+#endif  //_SPLINEMATH
diff --git a/include/curves/bernstein.h b/include/curves/bernstein.h
@@ -1,11 +1,10 @@
 /**
-* \file bezier_curve.h
-* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
-* \author Steve T.
-* \version 0.1
-* \date 06/17/2013
-*/
-
+ * \file bezier_curve.h
+ * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
+ * \author Steve T.
+ * \version 0.1
+ * \date 06/17/2013
+ */
 
 #ifndef _CLASS_BERNSTEIN
 #define _CLASS_BERNSTEIN
@@ -18,72 +17,62 @@
 #include <vector>
 #include <stdexcept>
 
-namespace curves
-{
-  /// \brief Computes a binomial coefficient  .
-  /// \param n : an unsigned integer.
-  /// \param k : an unsigned integer.
-  /// \return \f$\binom{n}{k}f$
-  ///
-  inline unsigned int bin(const unsigned  int n, const unsigned  int k)
-  {
-    if(k >  n) throw std::runtime_error("binomial coefficient higher than degree");
-    if(k == 0) return 1;
-    if(k > n/2) return bin(n,n-k);
-    return n * bin(n-1,k-1) / k;
-  }
+namespace curves {
+/// \brief Computes a binomial coefficient  .
+/// \param n : an unsigned integer.
+/// \param k : an unsigned integer.
+/// \return \f$\binom{n}{k}f$
+///
+inline unsigned int bin(const unsigned int n, const unsigned int k) {
+  if (k > n) throw std::runtime_error("binomial coefficient higher than degree");
+  if (k == 0) return 1;
+  if (k > n / 2) return bin(n, n - k);
+  return n * bin(n - 1, k - 1) / k;
+}
 
-  /// \class Bernstein.
-  /// \brief Computes a Bernstein polynome.
-  ///
-  template <typename Numeric = double>
-  struct Bern {
-    Bern(){}
-    Bern(const unsigned int m, const unsigned int i)
-      :m_minus_i(m - i),
-       i_(i),
-       bin_m_i_(bin(m,i)) 
-    {}
+/// \class Bernstein.
+/// \brief Computes a Bernstein polynome.
+///
+template <typename Numeric = double>
+struct Bern {
+  Bern() {}
+  Bern(const unsigned int m, const unsigned int i) : m_minus_i(m - i), i_(i), bin_m_i_(bin(m, i)) {}
 
-    ~Bern(){}
+  ~Bern() {}
 
-    Numeric operator()(const Numeric u) const
-    {
-      assert(u >= 0. && u <= 1.);
-      return bin_m_i_*(pow(u, i_)) *pow((1-u),m_minus_i);
-    }
+  Numeric operator()(const Numeric u) const {
+    assert(u >= 0. && u <= 1.);
+    return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
+  }
 
-    /* Attributes */
-    Numeric m_minus_i;
-    Numeric i_;
-    Numeric bin_m_i_;
-    /* Attributes */
+  /* Attributes */
+  Numeric m_minus_i;
+  Numeric i_;
+  Numeric bin_m_i_;
+  /* Attributes */
 
-    // Serialization of the class
-    friend class boost::serialization::access;
-    template<class Archive>
-    void serialize(Archive& ar, const unsigned int version){
-      if (version) {
+  // Serialization of the class
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version) {
+    if (version) {
       // Do something depending on version ?
-      }
-      ar & boost::serialization::make_nvp("m_minus_i", m_minus_i);
-      ar & boost::serialization::make_nvp("i", i_);
-      ar & boost::serialization::make_nvp("bin_m_i", bin_m_i_);
     }
-  }; // End struct Bern
-
-  /// \brief Computes all Bernstein polynomes for a certain degree.
-  ///
-  template <typename Numeric>
-  std::vector<Bern<Numeric> > makeBernstein(const unsigned int n)
-  {
-    std::vector<Bern<Numeric> > res;
-    for(unsigned int i = 0; i<= n; ++i)
-    {
-      res.push_back(Bern<Numeric>(n, i));
-    }
-    return res;
+    ar& boost::serialization::make_nvp("m_minus_i", m_minus_i);
+    ar& boost::serialization::make_nvp("i", i_);
+    ar& boost::serialization::make_nvp("bin_m_i", bin_m_i_);
   }
-} // namespace curves
-#endif //_CLASS_BERNSTEIN
+};  // End struct Bern
 
+/// \brief Computes all Bernstein polynomes for a certain degree.
+///
+template <typename Numeric>
+std::vector<Bern<Numeric> > makeBernstein(const unsigned int n) {
+  std::vector<Bern<Numeric> > res;
+  for (unsigned int i = 0; i <= n; ++i) {
+    res.push_back(Bern<Numeric>(n, i));
+  }
+  return res;
+}
+}  // namespace curves
+#endif  //_CLASS_BERNSTEIN