functions.hpp

#ifndef _FUNCTIONS_HPP_
#define _FUNCTIONS_HPP_

#include <vector>
#include "jmath/jblas.hpp"

#include "facetsmap/types.hpp"

namespace jafar {
  namespace facetsmap {

inline jblas::mat66 vecprodtransvec(const jblas::vec& v)
{
  jblas::mat66 m = ublas::zero_matrix<double> (6, 6);
  for(int i =0; i < 6; i++)
  {
    for(int j =0; j < 6; j++)
    {
      m(i,j) = v(i) * v(j);
    }
  }
  return m;
}

inline void rImage2rStereo(jblas::vec3& v)
{
  double x=v(0);
  v(0) = v(2);
  v(2) = -v(1);
  v(1) = -x;
}

inline void rStereo2rImage(jblas::vec3& v)
{
  double x=v(0);
  v(0) = -v(1);
  v(1) = -v(2);
  v(2) = x;
}

template<class PointType>
std::vector<double> findParamBestPlan(std::vector<PointType> points);

void eraseVFacets(jafar::facetsmap::vFacets& v);

template<class PointType, int p1, int p2, int p3>
std::vector<double> findParamPlan(std::vector<PointType>);

    template <class vectType>
    jblas::vec3 orthogonalProjection(jblas::vec3 normal, double d, vectType& vect);

    template <int dim, class RT, class C1 = RT, class C2 = C1>
    class vect_op {
      public:
        inline static RT dot_product (C1 premier, C2 second);
        inline static RT sub(C1,C2);
        inline static RT vectorial_product(C1,C2);
    };
    
    template <class RT, class C1, class C2>
    class vect_op<1,RT, C1, C2> {
      public:
        inline static RT dot_product (C1 premier, C2 second);
        inline static RT sub(C1,C2);
    };
    inline jblas::vec3 prodMat44byVec3(jblas::mat44 m, jblas::vec3 v, double l)
    {
      jblas::vec3 ret;
      ret(0) = m(0,0) * v(0) + m(0,1) * v(1) + m(0,2) * v(2) + m(0,3) *l;
      ret(1) = m(1,0) * v(0) + m(1,1) * v(1) + m(1,2) * v(2) + m(1,3) *l;
      ret(2) = m(2,0) * v(0) + m(2,1) * v(1) + m(2,2) * v(2) + m(2,3) *l;
      return ret;
    };
  };
};

#include "functions.tpp"

#endif