HyperPlaneImpl.hpp

/* $Id$ */

#include "geom/Line.hpp"
#include "geom/Point.hpp"

namespace jafar {
  namespace geom {
// EquationDriver Implementation
    template<int dimension>
    HyperPlane<dimension>::EquationDriver::EquationDriver(const VecD& eqn, double d, const RepereD* reference ) : Driver(), m_reference(reference)
    {
      double norm = ublas::norm_2(eqn);
      ublas::subrange(m_eqn, 0, dimension ) = eqn;
      m_eqn(dimension) = d;
      m_eqn /= norm;
    }
    template<int dimension>
    HyperPlane<dimension>::EquationDriver::EquationDriver(const HomogenousVecD& eqn, const RepereD* reference ) : Driver(), m_reference(reference)
    {
      double norm = ublas::norm_2( ublas::subrange(eqn, 0, dimension) );
      m_eqn = eqn / norm;
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::EquationDriver::origin() const
    {
      HomogenousVecD v = - m_eqn(dimension) * normal();
      v(dimension) = 1.0;
      return v;
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousSymMatrixD  HyperPlane<dimension>::EquationDriver::originCov() const
    {
      JFR_ERROR(GeomException, GeomException::Unimplemented, "Unimplemented." );
      return HomogenousSymMatrixD();
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::EquationDriver::normal() const
    {
      HomogenousVecD v = m_eqn;
      v(dimension) = 0.0;
      return v;
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousSymMatrixD  HyperPlane<dimension>::EquationDriver::normalCov() const
    {
      JFR_ERROR(GeomException, GeomException::Unimplemented, "Unimplemented." );
      return HomogenousSymMatrixD();
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::EquationDriver::equation() const
    {
      return m_eqn;
    }
    template<int dimension>
    void HyperPlane<dimension>::EquationDriver::changeReference( const RepereD* reference )
    {
      typename Atom<dimension>::HomogenousMatrixD m;
      
      reference->localToRepere(m, this->m_reference);
      
      typename Atom<dimension>::HomogenousMatrixD mt;
      
      mt = ublas::trans( m );
      
      m_eqn = ublas::prod(mt, m_eqn);
      
      double norm = ublas::norm_2( ublas::subrange(m_eqn, 0, dimension) );
      m_eqn /= norm;
      this->m_reference = reference;
    }
    template<int dimension>
    const Repere<dimension>* HyperPlane<dimension>::EquationDriver::reference( ) const
    {
      return m_reference;
    }
    template<int dimension>
    typename HyperPlane<dimension>::Driver* HyperPlane<dimension>::EquationDriver::clone( ) const
    {
      return new EquationDriver( m_eqn, this->reference() );
    }
// VectorsDriver Implementation
    template<int dimension>
    HyperPlane<dimension>::VectorsDriver::VectorsDriver(const HomogenousVecD& origin, const HomogenousVecD& normal, const RepereD* reference ) : Driver(), m_origin(origin), m_normal(normal / ublas::norm_2(normal)), m_reference(reference), m_hasCov(false)
    {
    }
    template<int dimension>
    HyperPlane<dimension>::VectorsDriver::VectorsDriver(const VecD& origin, const VecD& normal, const RepereD* reference ) : Driver(), m_reference(reference), m_hasCov(false)
    {
      m_origin(dimension) = 1.0;
      ublas::subrange(m_origin, 0, dimension ) = origin;
      m_normal(dimension) = 0.0;
      ublas::subrange(m_normal, 0, dimension ) = normal / ublas::norm_2(normal);
    }
    template<int dimension>
    void HyperPlane<dimension>::VectorsDriver::setCov(HomogenousSymMatrixD _originCov, HomogenousSymMatrixD _normalCov)
    {
      m_originCov = _originCov;
      m_normalCov = _normalCov;
      m_hasCov = true;
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::VectorsDriver::origin() const
    {
      return m_origin;
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousSymMatrixD  HyperPlane<dimension>::VectorsDriver::originCov() const
    {
      return m_originCov;
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::VectorsDriver::normal() const
    {
      return m_normal;
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousSymMatrixD  HyperPlane<dimension>::VectorsDriver::normalCov() const
    {
      return m_normalCov;
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::VectorsDriver::equation() const
    {
      HomogenousVecD eqn;
      ublas::subrange(eqn, 0, dimension ) = ublas::subrange(m_normal, 0, dimension );
      eqn(dimension) = - ublas::inner_prod(ublas::subrange(eqn, 0, dimension ), ublas::subrange(m_origin, 0, dimension));
      return eqn;
    }
    template<int dimension>
    void HyperPlane<dimension>::VectorsDriver::changeReference( const RepereD* reference )
    {
      typename Atom<dimension>::HomogenousMatrixD m;
      this->m_reference->localToRepere(m, reference);
      m_normal = ublas::prod(m, m_normal);
      m_normal /= ublas::norm_2(m_normal);
      m_origin = ublas::prod(m, m_origin);
      m_origin /= m_origin(dimension);
      this->m_reference = reference;
    }
    template<int dimension>
    const Repere<dimension>* HyperPlane<dimension>::VectorsDriver::reference( ) const
    {
      return m_reference;
    }
    template<int dimension>
    typename HyperPlane<dimension>::Driver* HyperPlane<dimension>::VectorsDriver::clone( ) const
    {
      return new VectorsDriver( m_origin, m_normal, this->reference() );
    }
// HyperPlane Implementation
    template<int dimension>
    HyperPlane<dimension>::HyperPlane(Driver* d) : Atom<dimension>(d), m_driver(d)
    {}
    template<int dimension>
    HyperPlane<dimension>::HyperPlane(const HyperPlane& hp) : Atom<dimension>(0), m_driver( hp.m_driver->clone() )
    {
      Atom<dimension>::setDriver( m_driver );
      this->setId( hp.id() );
    }
    template<int dimension>
    HyperPlane<dimension>::~HyperPlane()
    {
    }
    template<int dimension>
    inline typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::origin() const
    {
      return m_driver->origin();
    }
    template<int dimension>
    inline typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::normal() const
    {
      return m_driver->normal();
    }
    template<int dimension>
    inline typename HyperPlane<dimension>::HomogenousSymMatrixD HyperPlane<dimension>::normalCov() const
    {
      return m_driver->normalCov();
    }
    template<int dimension>
    inline typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::equation() const
    {
      return m_driver->equation();
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::projectionVector(const HomogenousVecD& p) const
    {
      return - ublas::inner_prod(p - origin(), normal()) * normal();
    }
    template<int dimension>
    void HyperPlane<dimension>::projectionVectorCov(const HomogenousVecD& p, const HomogenousSymMatrixD& pCov, HomogenousVecD& vec, HomogenousSymMatrixD& cov) const
    {
      HomogenousMatrixD Jp, Jo, Jn;
      HomogenousVecD n = normal();
      HomogenousVecD o = origin();
      // Compute the Jacobian on p
      Jp = ublas::outer_prod( n, n );
      // Compute the Jacobian on o
      Jo = -Jp;
      // Compute the Jacobian on n
      Jn = ublas::outer_prod( n, vec - n );
      double ip = ublas::inner_prod( n, vec - o );
      ublas::subrange(Jn, 0, 3, 0, 3 ) += jblas::identity_mat( 3, 3) * ip;
      // Compute projection and covariance
      vec = -ip * n;
      cov = ublas::prod( Jp, HomogenousMatrixD( ublas::prod( cov, ublas::trans(Jp) ) ) )
          + ublas::prod( Jo, HomogenousMatrixD( ublas::prod( m_driver->originCov(), ublas::trans(Jo) ) ) )
          + ublas::prod( Jn, HomogenousMatrixD( ublas::prod( m_driver->normalCov(), ublas::trans(Jn) ) ) );
    }
    template<int dimension>
    void HyperPlane<dimension>::projectionVectorCov(const Point<dimension>& p, HomogenousVecD& vec, HomogenousSymMatrixD& cov) const
    {
      projectionVectorCov(p.homogenousCoordinates(), p.homogenousCoordinatesCov(), vec, cov );
    }
    template<int dimension>
    typename HyperPlane<dimension>::HomogenousVecD HyperPlane<dimension>::project(const HomogenousVecD& p) const
    {
      return p + projectionVector(p);
    }
    template<int dimension>
    void HyperPlane<dimension>::setDriver(Driver* driver)
    {
      m_driver = driver;
      Atom<dimension>::setDriver(driver);
    }
    template<int dimension>
    inline BoundingBox<dimension> HyperPlane<dimension>::boundingBox() const
    {
      return BoundingBox<dimension>();
    }
  }
}