AngleImpl.hpp

/* $ Id: $ */

#include <kernel/jafarMacro.hpp>

namespace jafar {
  namespace geom {
    namespace impl {
      inline double safeacos( double cosi )
      {
        if( cosi - 1.0 > 0.0)
        {
          JFR_ASSERT( (cosi - 1.0) < 1e-10, "Cosinus is supperior from 1.0" );
          return 0.0; // It can happen due to imprecision, the cosinus is slightly bigger than 1.0
        }
        return acos(cosi);
      }
    }
    template<int dimension>
    inline double angle(const Point<dimension>&, const Point<dimension>& )
    {
      return 0.0;
    }
    template<int dimension>
    inline double angle(const Point<dimension>&, const Line<dimension>& )
    {
      return 0.0;
    }
    template<int dimension>
    inline double angle(const Point<dimension>&, const HyperPlane<dimension>& )
    {
      return 0.0;
    }
    template<int dimension>
    inline double angle(const Point<dimension>&, const Repere<dimension>& )
    {
      return 0.0;
    }
    // line
    template<int dimension>
    inline double angle(const Line<dimension>& line, const Point<dimension>& point)
    {
      return angle(point, line);
    }
    template<int dimension>
    inline double angle(const Line<dimension>& line1, const typename Atom<dimension>::HomogenousVecD& v)
    {
        return impl::safeacos(ublas::inner_prod( line1.direction(), v) / ( ublas::norm_2(v)));
    }
    template<int dimension>
    inline double angle(const Line<dimension>& line1, const Line<dimension>& line2)
    {
      return impl::safeacos(ublas::inner_prod( line1.direction(), line2.reference()->convertIn( line2.direction(), line1.reference() ) ) );
    }
    template<int dimension>
    inline double angle(const Line<dimension>& line, const HyperPlane<dimension>& plan)
    {
      return impl::safeacos( ublas::inner_prod(line.direction(), plan.reference()->convertIn( plan.normal(), line.reference() ) ) );
    }
    template<int dimension>
    inline double angle(const Line<dimension>& l, const Repere<dimension>& r)
    {
      typename Atom<dimension>::HomogenousVecD v = jblas::zero_vec(dimension+1);
      v(0) = 1.0;
      return impl::safeacos( ublas::inner_prod( l.direction(), r.convertIn( v, l.reference() ) ) );
    }
    // HyperPlane
    template<int dimension>
    inline double angle(const HyperPlane<dimension>& plan, const Point<dimension>& point)
    {
      return angle(point, plan);
    }
    template<int dimension>
    inline double angle(const HyperPlane<dimension>& plan, const Line<dimension>& line)
    {
      return angle(line, plan);
    }
    template<int dimension>
    inline double angle(const HyperPlane<dimension>& plan1, const HyperPlane<dimension>& plan2)
    {
      return impl::safeacos( ublas::inner_prod(plan1.normal(), plan2.reference()->convertIn( plan2.normal(), plan1.reference() ) ) );
    }
    template<int dimension>
    inline double angle(const HyperPlane<dimension>& plan, const Repere<dimension>& r)
    {
      typename Atom<dimension>::HomogenousVecD v = jblas::zero_vec(dimension+1);
      v(0) = 1.0;
      return impl::safeacos( ublas::inner_prod( plan.normal(), r.convertIn( v, plan.reference() ) ) ) ;
    }
    // Segment
    template<int dimension, typename _T2_>
    inline double angle(const Segment<dimension>& segment, const _T2_& t2)
    {
      return angle(segment.support(), t2);
    }
    template<int dimension, typename _T1_>
    inline double angle(const _T1_& t1, const Segment<dimension>& segment)
    {
      return angle(segment.support(), t1);
    }
    template<int dimension>
    inline double angle(const Segment<dimension>& segment1, const Segment<dimension>& segment2)
    {
      return angle(segment1.support(), segment2.support() );
    }
    // PolyLine
    template<int dimension, typename _T2_>
    inline double anglePolyLine(const PolyLine<dimension>& pl, const _T2_& t2)
    {
      std::vector< Segment<dimension> > segments = pl.segments();
      double minAngle = 10.0;
      for( typename std::vector< Segment<dimension> >::iterator it = segments.begin(); it != segments.end(); ++it)
      {
        double angle_ = angle( *it, t2 );
        if(angle_ < minAngle) minAngle = angle_;
      }
      return minAngle;
    }
    template<int dimension, typename _T2_>
    inline double angle(const PolyLine<dimension>& pl, const _T2_& t2)
    {
      return anglePolyLine(pl, t2);
    }
    template<int dimension, typename _T1_>
    inline double angle(const _T1_& t1, const PolyLine<dimension>& pl)
    {
      return anglePolyLine(pl, t1);
    }
    template<int dimension>
    inline double angle(const PolyLine<dimension>& pl1, const PolyLine<dimension>& pl2)
    {
      return anglePolyLine(pl1, pl2);
    }
    template<int dimension>
    inline double angle(const PolyLine<dimension>& pl, const Segment<dimension>& s)
    {
      return anglePolyLine(pl,s);
    }
    template<int dimension>
    inline double angle(const Segment<dimension>& s, const PolyLine<dimension>& pl)
    {
      return anglePolyLine(pl,s);
    }
    // Repere
    template<int dimension>
    inline double angle(const Repere<dimension>& r, const Point<dimension>& p)
    {
      return angle(p,r);
    }
    template<int dimension>
    inline double angle(const Repere<dimension>& r, const Line<dimension>& l)
    {
      return angle(l,r);
    }
    template<int dimension>
    inline double angle(const Repere<dimension>& r, const HyperPlane<dimension>& hp)
    {
      return angle(hp,r);
    }
    template<int dimension>
    inline double angle(const Repere<dimension>& r1, const Repere<dimension>& r2)
    {
      typename Atom<dimension>::HomogenousVecD v = jblas::zero_vec(dimension+1);
      v(0) = 1.0;
      return impl::safeacos(ublas::inner_prod( r1.vec( 0 ), r2.convertIn( v, &r1 ) ));
    }
    template<int dimension>
    inline double angle(const Repere<dimension>& r, const PolyLine<dimension>& pl)
    {
      return angle(pl,r);
    }
  }
}