Algorithms.hh

/*===========================================================================*\
 *                                                                           *
 *                              OpenFlipper                                  *
 *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
 *           Department of Computer Graphics and Multimedia                  *
 *                          All rights reserved.                             *
 *                            www.openflipper.org                            *
 *                                                                           *
 *---------------------------------------------------------------------------*
 * This file is part of OpenFlipper.                                         *
 *---------------------------------------------------------------------------*
 *                                                                           *
 * Redistribution and use in source and binary forms, with or without        *
 * modification, are permitted provided that the following conditions        *
 * are met:                                                                  *
 *                                                                           *
 * 1. Redistributions of source code must retain the above copyright notice, *
 *    this list of conditions and the following disclaimer.                  *
 *                                                                           *
 * 2. Redistributions in binary form must reproduce the above copyright      *
 *    notice, this list of conditions and the following disclaimer in the    *
 *    documentation and/or other materials provided with the distribution.   *
 *                                                                           *
 * 3. Neither the name of the copyright holder nor the names of its          *
 *    contributors may be used to endorse or promote products derived from   *
 *    this software without specific prior written permission.               *
 *                                                                           *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
 *                                                                           *
\*===========================================================================*/



#ifndef GEO_ALGORITHMS_HH
#define GEO_ALGORITHMS_HH


//== INCLUDES =================================================================

#include <cfloat>
#include <ACG/Math/VectorT.hh>
#include <vector>
#include <iostream>

#include "../Math/Matrix3x3T.hh"


namespace ACG {
namespace Geometry {


//== 3D STUFF =================================================================

 

template<typename Scalar>
bool
circumCenter( const VectorT<Scalar,3>&  _v0,
              const VectorT<Scalar,3>&  _v1,
              const VectorT<Scalar,3>&  _v2,
              const VectorT<Scalar,3>&  _v3,
              VectorT<Scalar,3>&        _result );


template<typename Scalar>
Scalar
circumRadiusSquared( const VectorT<Scalar,3>&  _v0,
                     const VectorT<Scalar,3>&  _v1,
                     const VectorT<Scalar,3>&  _v2,
                     const VectorT<Scalar,3>&  _v3 )
{
  VectorT<Scalar,3> cc;
  return circumCenter(_v0, _v1, _v2, _v3, cc) ? (cc-_v0).sqrnorm() : FLT_MAX;
}


template<typename Scalar>
Scalar
circumRadius( const VectorT<Scalar,3>&  _v0,
              const VectorT<Scalar,3>&  _v1,
              const VectorT<Scalar,3>&  _v2,
              const VectorT<Scalar,3>&  _v3 )
{
  return sqrt(circumRadiusSquared(_v0, _v1, _v2, _v3));
}

template<typename Scalar>
bool edgeConvexPolygonIntersection(std::vector<VectorT<Scalar,3> > _polygon_points,
                                   VectorT<Scalar,3> _v0,
                                   VectorT<Scalar,3> _v1,
                                   VectorT<Scalar,3> &_result);


template<typename Scalar>
bool
rotationOfTwoVectors( const VectorT<Scalar,3>&  _v0,
                      const VectorT<Scalar,3>&  _v1,
                      VectorT<Scalar,3>&  _axis,
                      Scalar& _angle,
                      bool _degree = true);


template <typename Scalar>
VectorT<Scalar,3>
perpendicular( const VectorT<Scalar,3>&  _v );


template<typename Vec>
bool
triangleIntersection( const Vec&  _o,
                      const Vec&  _dir,
                      const Vec&  _v0,
                      const Vec&  _v1,
                      const Vec&  _v2,
                      typename Vec::value_type& _t,
                      typename Vec::value_type& _u,
                      typename Vec::value_type& _v );
      

template<typename Vec>
bool
axisAlignedBBIntersection( const Vec&  _o,
                                       const Vec&  _dir,
                                       const Vec& _bbmin,
                                       const Vec& _bbmax,
                                       typename Vec::value_type& _t0,
                                       typename Vec::value_type& _t1 );


//== 2D STUFF =================================================================

template<typename Scalar>
Scalar
pointLineOrientation( const VectorT<Scalar,2>&  _p,
                      const VectorT<Scalar,2>&  _v0,
                      const VectorT<Scalar,2>&  _v1 )
{
  VectorT<Scalar,2> d1(_p-_v0), d2(_v1-_v0);
  return (d1[0]*d2[1]-d1[1]*d2[0]);
}


template<typename Scalar>
bool
isCCW( const VectorT<Scalar,2>&  _v0,
       const VectorT<Scalar,2>&  _v1,
       const VectorT<Scalar,2>&  _v2 )
{
  return ( pointLineOrientation(_v0, _v1, _v2) < Scalar(0.0) );
}


template<typename Scalar>
bool
isCW( const VectorT<Scalar,2>&  _v0,
      const VectorT<Scalar,2>&  _v1,
      const VectorT<Scalar,2>&  _v2 )
{
  return ( pointLineOrientation(_v0, _v1, _v2) > Scalar(0.0) );
}


template<typename Scalar>
bool
lineIntersection( const VectorT<Scalar,2>&  _v0,
                  const VectorT<Scalar,2>&  _v1,
                  const VectorT<Scalar,2>&  _v2,
                  const VectorT<Scalar,2>&  _v3,
                  Scalar& _t1,
                  Scalar& _t2 );


//===========================================================================
//===========================================================================     


template<class Vec>
typename Vec::value_type
distPointLineSquared( const Vec& _p,
                      const Vec& _v0,
                      const Vec& _v1,
                      Vec*       _min_v=0);



template<class Vec>
typename Vec::value_type
distPointLine( const Vec& _p,
               const Vec& _v0,
               const Vec& _v1,
               Vec*       _min_v=0 )
{ return sqrt(distPointLineSquared(_p, _v0, _v1, _min_v)); }


template <class Vec>
typename Vec::value_type
distPointTriangleSquared( const Vec& _p,
                          const Vec& _v0,
                          const Vec& _v1,
                          const Vec& _v2,
                          Vec& _nearestPoint );

template <class Vec>
typename Vec::value_type
distPointTriangleSquaredStable( const Vec& _p,
                                const Vec& _v0,
                                const Vec& _v1,
                                const Vec& _v2,
                                Vec& _nearestPoint );

template <class Vec>
typename Vec::value_type
distPointTriangle( const Vec& _p,
                   const Vec& _v0,
                   const Vec& _v1,
                   const Vec& _v2,
                   Vec& _nearestPoint )
{ return sqrt(distPointTriangleSquared(_p, _v0, _v1, _v2, _nearestPoint)); }

template <class Vec>
typename Vec::value_type
distPointTriangleStable( const Vec& _p,
                         const Vec& _v0,
                         const Vec& _v1,
                         const Vec& _v2,
                         Vec& _nearestPoint )
{ return sqrt(distPointTriangleSquaredStable(_p, _v0, _v1, _v2, _nearestPoint)); }

template < typename VectorT , typename ValueT >
inline
ValueT 
distPointPlane(const VectorT& _porigin, 
               const VectorT& _pnormal, 
               const VectorT&  _point);                          

          
//===========================================================================
//===========================================================================                             
                          
template<typename Scalar>
Scalar
distLineLineSquared( const VectorT<Scalar,3>& _v00,
                     const VectorT<Scalar,3>& _v01,
                     const VectorT<Scalar,3>& _v10,
                     const VectorT<Scalar,3>& _v11,
                     VectorT<Scalar,3>*       _min_v0=0,
                     VectorT<Scalar,3>*       _min_v1=0,
                     bool                          _fastApprox=false );


template<typename Scalar>
Scalar
distLineLine( const VectorT<Scalar,3>& _v00,
              const VectorT<Scalar,3>& _v01,
              const VectorT<Scalar,3>& _v10,
              const VectorT<Scalar,3>& _v11,
              VectorT<Scalar,3>*       _min_v0=0,
              VectorT<Scalar,3>*       _min_v1=0 )
{
  return sqrt(distLineLineSquared(_v00, _v01, _v10, _v11,
                                  _min_v0, _min_v1));
}

//===========================================================================
//===========================================================================      


template < typename VectorT >
VectorT projectToEdge(const VectorT& _start , 
                      const VectorT& _end , 
                      const VectorT& _point );
                      
                      
template < typename VectorT >
inline
VectorT
projectToPlane(const VectorT& _porigin, 
               const VectorT& _pnormal, 
               const VectorT&  _point);                      
              
//===========================================================================
//===========================================================================   

template<typename Scalar>
bool
baryCoord( const VectorT<Scalar,2>&  _p,
           const VectorT<Scalar,2>&  _u,
           const VectorT<Scalar,2>&  _v,
           const VectorT<Scalar,2>&  _w,
           VectorT<Scalar,3>&        _result );
           

template<typename Scalar>
bool
isInTriangle( const VectorT<Scalar,2>&  _p,
              const VectorT<Scalar,2>&  _u,
              const VectorT<Scalar,2>&  _v,
              const VectorT<Scalar,2>&  _w )
{
  VectorT<Scalar,3> bary;
  if (baryCoord(_p, _u, _v, _w, bary)) 
    return ((bary[0]>0.0) && (bary[1]>0.0) && (bary[2]>0.0));
  else {
    std::cerr << "point in triangle error\n";
    return false;
  }
}

template<typename Scalar>
bool
circumCenter( const VectorT<Scalar,2>&  _v0,
              const VectorT<Scalar,2>&  _v1,
              const VectorT<Scalar,2>&  _v2,
              VectorT<Scalar,2>&        _result );

//===========================================================================
//===========================================================================  
 
template<typename Scalar>
bool
baryCoord( const VectorT<Scalar,3>&  _p,
           const VectorT<Scalar,3>&  _u,
           const VectorT<Scalar,3>&  _v,
           const VectorT<Scalar,3>&  _w,
           VectorT<Scalar,3>&        _result );


template<typename Scalar>
bool
minSphere(const VectorT<Scalar,3>&  _v0,
          const VectorT<Scalar,3>&  _v1,
          const VectorT<Scalar,3>&  _v2,
          VectorT<Scalar,3>&        _center,
          Scalar&                   _radius);


template<typename Scalar>
Scalar
minRadiusSquared( const VectorT<Scalar,3>&  _v0,
                  const VectorT<Scalar,3>&  _v1,
                  const VectorT<Scalar,3>&  _v2 );

  
template<typename Scalar>
Scalar
minRadius( const VectorT<Scalar,3>&  _v0,
           const VectorT<Scalar,3>&  _v1,
           const VectorT<Scalar,3>&  _v2 )
{
  return sqrt(minRadiusSquared(_v0, _v1, _v2));
}


template<typename Scalar>
bool
circumCenter( const VectorT<Scalar,3>&  _v0,
              const VectorT<Scalar,3>&  _v1,
              const VectorT<Scalar,3>&  _v2,
              VectorT<Scalar,3>&        _result );


template<typename Scalar>
Scalar
circumRadiusSquared( const VectorT<Scalar,3>&  _v0,
                     const VectorT<Scalar,3>&  _v1,
                     const VectorT<Scalar,3>&  _v2 );


template<typename Scalar>
Scalar
circumRadius( const VectorT<Scalar,3>&  _v0,
              const VectorT<Scalar,3>&  _v1,
              const VectorT<Scalar,3>&  _v2 )
{
  return sqrt(circumRadiusSquared(_v0, _v1, _v2));
}

template<class VectorT>
int isObtuse(const VectorT& _p0,
             const VectorT& _p1,
             const VectorT& _p2 );

//===========================================================================
//===========================================================================   


template <class Vec>
typename Vec::value_type
triangleAreaSquared( const Vec& _v0,
                     const Vec& _v1,
                     const Vec& _v2 );


template <class Vec>
typename Vec::value_type
triangleArea( const Vec& _v0,
              const Vec& _v1,
              const Vec& _v2 )
{
  return sqrt(triangleAreaSquared(_v0,_v1,_v2));
}
  

template <typename Scalar, int N>
Scalar
aspectRatio( const VectorT<Scalar, N>& _v0,
             const VectorT<Scalar, N>& _v1,
             const VectorT<Scalar, N>& _v2 );

template <typename Scalar, int N>
Scalar
roundness( const VectorT<Scalar, N>& _v0,
           const VectorT<Scalar, N>& _v1,
           const VectorT<Scalar, N>& _v2 );

template<typename Vector>
Vector closestPointLineSegment(Vector x, Vector p1, Vector p2) {
    const auto delta = ((p2-p1)|(x-p1)) / (p2-p1).sqrnorm();
    //std::cout << "\x1b[32mdelta = " << delta << "\x1b[0m" << std::endl;
    if (delta <= 0) {
        //std::cout << "\x1b[43mdelta <= 0\x1b[0m" << std::endl;
        return p1;
    } else if (delta >= 1) {
        //std::cout << "\x1b[43mdelta >= 1\x1b[0m" << std::endl;
        return p2;
    } else if (delta != delta) { // p1 = p2 or x = p1
        //std::cout << "\x1b[43mdelta != delta\x1b[0m" << std::endl;
        return (x-p1).sqrnorm() < (x-p2).sqrnorm() ? p1 : p2;
    } else {
        //std::cout << "\x1b[43mdelta \\in [0, 1]\x1b[0m" << std::endl;
        return (1 - delta) * p1 + delta * p2;
    }
};

template<typename Vector>
Vector closestPointTri(Vector p, Vector a, Vector b, Vector c) {
    constexpr double thresh = 1e-8;

    const auto n = ((b - a) % (c - a)); // normalization unnecessary

    if ((b-a).sqrnorm() < thresh || (c-a).sqrnorm() < thresh || n.sqrnorm() < thresh) {
        //std::cout << "\x1b[42mDegenerate case.\x1b[0m" << std::endl;
        // Degenerate triangle. Find distance to longest segment.
        std::array<ACG::Vec3d, 2> max_segment = {a, b};
        double max_len = (b-a).sqrnorm();
        if ((c-a).sqrnorm() > max_len)
            max_segment = {a, c};
        if ((c-b).sqrnorm() > max_len)
            max_segment = {b, c};

        // closestPointLineSegment is stable, even if the segment is super short
        return closestPointLineSegment(p, max_segment[0], max_segment[1]);
    }

    const auto abd = Matrix3x3d::fromColumns(a-c, b-c, n).inverse() * (p - c);
    const bool alpha = (abd[0] >= 0.0),
            beta = (abd[1] >= 0.0),
            gamma = (1.0-abd[0]-abd[1] >= 0.0);

    if (alpha && beta && gamma) {
        //std::cout << "\x1b[42mInside case.\x1b[0m" << std::endl;
        // Inside triangle.
        return abd[0] * a + abd[1] * b + (1.0 - abd[0] - abd[1]) * c;
    } else if (!alpha) {
        //std::cout << "\x1b[42m!alpha case.\x1b[0m" << std::endl;
        // Closest to line segment (b, c).
        return closestPointLineSegment(p, b, c);
    } else if (!beta) {
        //std::cout << "\x1b[42m!beta case.\x1b[0m" << std::endl;
        // Closest to line segment (a, c).
        return closestPointLineSegment(p, a, c);
    } else if (!gamma) {
        //std::cout << "\x1b[42m!gamma case.\x1b[0m" << std::endl;
        // Closest to line segment (a, b).
        return closestPointLineSegment(p, a, b);
    } else {
        throw std::logic_error("This cannot happen.");
    }
}

//=============================================================================
} // namespace Geometry
} // namespace ACG
//=============================================================================
#if defined(INCLUDE_TEMPLATES) && !defined(GEO_ALGORITHMS_C)
#define GEO_ALGORITHMS_TEMPLATES
#include "Algorithms.cc"
#endif
//=============================================================================
#endif // GEO_ALGORITHMS_HH defined
//=============================================================================