RepereImpl.hpp

/* $Id$ */

#include "jmath/ublasExtra.hpp"

namespace jafar {
  namespace geom {
// VecDtorDriver implemenation
    template<int dimension>
    Repere<dimension>::VectorsDriver::VectorsDriver(const Repere<dimension>* ref) :
        Driver( )
    {
      m_matrix = jblas::identity_mat(dimension + 1, dimension + 1);
      m_reference = ref;
    }

    template<int dimension>
    Repere<dimension>::VectorsDriver::VectorsDriver(const Repere<dimension>* ref, const typename Repere<dimension>::HomogenousVecD* repere, const typename Repere<dimension>::HomogenousVecD& origin) : Driver( )
    {
      for(int i = 0; i < dimension; i++)
      {
        ublas::matrix_column<MatrixD> mc( m_matrix, i);
        HomogenousVecD v = repere[i];
        for(int j = 0; j < i; j++)
        {
          ublas::matrix_column<MatrixD> u( m_matrix, j);
          v -= ublas::inner_prod(repere[i], u) * u;
        }
        v(dimension) = 0.0;
        mc = v / ublas::norm_2(v);
      }
      ublas::matrix_column<MatrixD> mcOrig( m_matrix, dimension);
      mcOrig = origin;
      mcOrig /= mcOrig( dimension);
      m_reference = ref;
    }
    template<int dimension>
    const Repere<dimension>* Repere<dimension>::VectorsDriver::reference() const
    {
      return m_reference;
    }
    template<int dimension>
    typename Repere<dimension>::HomogenousVecD Repere<dimension>::VectorsDriver::origin() const
    {
      return ublas::matrix_column<const MatrixD>( m_matrix, dimension);
    }
    template<int dimension>
    typename Repere<dimension>::HomogenousVecD Repere<dimension>::VectorsDriver::vec(unsigned int index) const
    {
      return ublas::matrix_column<const MatrixD>( m_matrix, index);
    }
    template<int dimension>
    void Repere<dimension>::VectorsDriver::localToReference(typename Repere<dimension>::MatrixD& dst) const
    {
      dst.assign(m_matrix);
    }
    
// Static Repere implemenation
    template<int dimension>
    Repere<dimension>* Repere<dimension>::s_globalRef = 0;
    template<int dimension>
    inline const Repere<dimension>* Repere<dimension>::global()
    {
      return (s_globalRef == 0) ? (s_globalRef = new Repere<dimension>( )) : s_globalRef;
    }
    
// Repere implemenation
    template<int dimension>
    Repere<dimension>::Repere() : Atom<dimension>(0), m_driver(new VectorsDriver(0))
    {
      Atom<dimension>::setDriver( m_driver);
      JFR_ASSERT(not s_globalRef, "Global reference allready exist");
    }
    template<int dimension>
    Repere<dimension>::Repere(Driver* driver) : Atom<dimension>(driver), m_driver(driver)
    {
      JFR_ASSERT(reference(), "Repere can't have a null reference, unless its the global reference");
    }
    template<int dimension>
    Repere<dimension>::~Repere()
    {
    }
    template<int dimension>
    inline BoundingBox<dimension> Repere<dimension>::boundingBox() const
    {
      HomogenousVecD v = this->reference()->convertInGlobal( this->origin());
      return BoundingBox<dimension>(v, v);
    }
    template<int dimension>
    inline const Repere<dimension>* Repere<dimension>::reference() const
    {
      return m_driver->reference();
    }
    template<int dimension>
    inline typename Repere<dimension>::HomogenousVecD Repere<dimension>::origin() const
    {
      return m_driver->origin();
    }
    template<int dimension>
    inline typename Repere<dimension>::HomogenousVecD Repere<dimension>::vec(unsigned int index) const
    {
      JFR_ASSERT(index < dimension, "Index bigger than dimension");
      return m_driver->vec(index);
    }
    template<int dimension>
    inline typename Repere<dimension>::HomogenousVecD Repere<dimension>::convertIn( const HomogenousVecD& v, const Repere* dstRepere) const
    {
      if(dstRepere == this)
      {
        return v;
      } else {
        MatrixD m;
        this->localToRepere( m, dstRepere);
        return ublas::prod(m, v );
      }
    }
    template<int dimension>
    inline typename Repere<dimension>::HomogenousVecD Repere<dimension>::convertInGlobal( const HomogenousVecD& v) const
    {
      if( this == global())
      {
        return v;
      } else {
        MatrixD m;
        this->localToGlobal( m);
        return ublas::prod(m, v );
      }
    }
    template<int dimension>
    inline void Repere<dimension>::localToReference(typename Repere<dimension>::MatrixD& dst) const
    {
      JFR_ASSERT(m_driver, "No driver");
      return m_driver->localToReference(dst);
    }
    template<int dimension>
    inline void Repere<dimension>::referenceToLocal(typename Repere<dimension>::MatrixD& dst) const
    {
      MatrixD lTR;
      localToReference(lTR);
      jmath::ublasExtra::inv(lTR, dst );
    }
    template<int dimension>
    inline void Repere<dimension>::localToGlobal(typename Repere<dimension>::MatrixD& dst) const
    {
      localToReference( dst );
      if(this == global() ) return;
      const Repere<dimension>* ref = this;
      MatrixD m;
      while( (ref = ref->reference() ) != global() ) {
        ref->localToReference( m );
        dst = ublas::prod( m, dst);
      }
    }
    template<int dimension>
    inline void Repere<dimension>::globalToLocal(typename Repere<dimension>::MatrixD& dst) const
    {
      MatrixD lTG;
      localToGlobal(lTG);
      jmath::ublasExtra::inv(lTG, dst );
    }
    template<int dimension>
    inline void Repere<dimension>::localToRepere(typename Repere<dimension>::MatrixD& dst, const Repere* r) const
    {
      if( r == global() )
      {
        localToGlobal(dst);
      } else {
        // repere to global
        MatrixD gTR;
        r->globalToLocal(gTR);
        // local to global
        MatrixD lTG;
        localToGlobal(lTG);
        // Final result
        dst = ublas::prod( gTR, lTG );
      }
    }
    template<int dimension>
    inline void Repere<dimension>::repereToLocal(typename Repere<dimension>::MatrixD& dst, const Repere* r) const
    {
      if( r == global() )
      {
        globalToLocal(dst);
      } else {
        // repere to global
        MatrixD gTL;
        globalToLocal(gTL);
        // local to global
        MatrixD rTG;
        r->localToGlobal(rTG);
        // Final result
        dst = ublas::prod( gTL, rTG );
      }
    }
  }
}