distance3.h

Go to the documentation of this file.

/****************************************************************************
* VCGLib                                                            o o     *
* Visual and Computer Graphics Library                            o     o   *
*                                                                _   O  _   *
* Copyright(C) 2004                                                \/)\/    *
* Visual Computing Lab                                            /\/|      *
* ISTI - Italian National Research Council                           |      *
*                                                                    \      *
* All rights reserved.                                                      *
*                                                                           *
* This program is free software; you can redistribute it and/or modify      *   
* it under the terms of the GNU General Public License as published by      *
* the Free Software Foundation; either version 2 of the License, or         *
* (at your option) any later version.                                       *
*                                                                           *
* This program is distributed in the hope that it will be useful,           *
* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
* for more details.                                                         *
*                                                                           *
****************************************************************************/
/****************************************************************************/

#ifndef __VCG_DISTANCE3
#define __VCG_DISTANCE3

#include <limits>
#include <vcg/space/intersection3.h>

namespace vcg {


/*
* Computes the minimum distance between a 3D box and a point
* @param[in] p                          The input point
* @param[in] b                          The input bounding box
* return                        The distance
* This function returns 0 for points Inside the bbox while the next one return the distance from the surface
*/
template<class Scalar>
Scalar PointFilledBoxDistance(const Point3<Scalar> &p, const Box3<Scalar> &bbox)
{
        Scalar dist2 = 0.;
        Scalar aux;
        for (int k=0 ; k<3 ; ++k)
        {
                if ( (aux = (p[k]-bbox.min[k]))<0. )
                        dist2 += aux*aux;
                else if ( (aux = (bbox.max[k]-p[k]))<0. )
                        dist2 += aux*aux;
        }
        return sqrt(dist2);
}

/*
* Computes the minimum distance between a 3D box and a point
* @param[in] p                          The input point
* @param[in] b                          The input bounding box
* @param[out] dist                      The distance
* Note that this function with respect to the previous one compute the distance between a point
* and the 'surface' of a Box3. 
* 
*/
template <class ScalarType> 
void PointBoxDistance(const Point3<ScalarType> &p,
                                                                                const Box3<ScalarType> &b,
                                                                                ScalarType& dist)
{
        if (b.IsIn(p))
        {
                const ScalarType dx = std::min<ScalarType>(b.max.X()-p.X(), p.X()-b.min.X());
                const ScalarType dy = std::min<ScalarType>(b.max.Y()-p.Y(), p.Y()-b.min.Y());
                const ScalarType dz = std::min<ScalarType>(b.max.Z()-p.Z(), p.Z()-b.min.Z());

                dist= std::min<ScalarType>(dx, std::min<ScalarType>(dy, dz));
                return;
        }
    
        {
                ScalarType sq_dist = ScalarType(0);
                for (int i=0; i<3; ++i)
                {
                        ScalarType delta = ScalarType(0);
                        if       (p[i] < b.min[i])  delta = p[i] - b.min[i];
                        else if  (p[i] > b.max[i])  delta = p[i] - b.max[i];
                        sq_dist += delta * delta;
                }

    dist= math::Sqrt(sq_dist);
        }
}

/*
* Computes the minimum distance between a sphere and a point
* @param[in] p                          The input point
* @param[in] sphere             The input sphere
* @param[out] dist                      The distance
*/
template <class ScalarType> 
void SpherePointDistance(const Sphere3<ScalarType> &sphere, 
                                                                                                                         const Point3<ScalarType> &p,
                                                                                                                         ScalarType& dist) 
{
  dist = Distance(p, sphere.Center()) - sphere.Radius();
  if(dist < 0) dist = 0;
}

/*
* Computes the minimum distance between two spheres
* @param[in] sphere0                            The input sphere
* @param[in] sphere1                            The input sphere
* @param[out] dist                      The distance
*/
template <class ScalarType> 
void SphereSphereDistance(const Sphere3<ScalarType> &sphere0, 
                                                                                                        const Sphere3<ScalarType> &sphere1,
                                                                                                        ScalarType& dist) 
{
  dist = (sphere1.Center()-sphere0.Center()).Norm()
                    - sphere0.Radius() - sphere1.Radius();
  if(dist < 0) dist = 0;
  return dist;
}

/*
* Computes the minimum squared distance between a between a point and a plane
* @param[in] pl                 The input line
* @param[in] p          The input point
* @param[out] closest   The closest point
* @param[out] dist              The squared distance
*/
template <class ScalarType>
void  PlanePointSquaredDistance(const vcg::Plane3<ScalarType> &Pl,
                                const Point3<ScalarType> &p,
                                Point3<ScalarType> &closest,
                                ScalarType &dist)
{
    closest=Pl.Projection(p);
    dist= (closest - p).SquaredNorm();
}


/*
* Computes the minimum squared distance between a between a point and a plane
* @param[in] pl                 The input line
* @param[in] p          The input point
* @param[out] closest   The closest point
* @param[out] dist              The squared distance
*/
template <class ScalarType>
ScalarType  PlanePointSquaredDistance(const vcg::Plane3<ScalarType> &Pl,
                                    const Point3<ScalarType> &p)
{
    ScalarType dist;
    vcg::Point3<ScalarType> closest;
    PlanePointSquaredDistance(Pl,p,closest,dist);
    return (dist);
}


/*
* Computes the minimum squared distance between a between a point and a line
* @param[in] l                          The input line
* @param[in] p                          The input point
* @param[out] closest   The closest point
* @param[out] dist              The squared distance
*/
template <class ScalarType> 
void  LinePointSquaredDistance(const Line3<ScalarType> &l,const Point3<ScalarType> &p,
                                                                                                Point3<ScalarType> &closest,ScalarType &dist) 
{
        closest=l.P(l.Projection(p));
  dist= (closest - p).SquaredNorm();
}

/*
* Computes the minimum  distance between a between a point and a line
* @param[in] l                          The input line
* @param[in] p                          The input point
* @param[out] closest                           The closest point
* @param[out] dist                      The distance
*/
template <class ScalarType> 
void  LinePointDistance(const Line3<ScalarType> &l,const Point3<ScalarType> &p,
                                                                                                Point3<ScalarType> &closest,ScalarType &dist) 
{
        LinePointSquaredDistance(l,p,closest,dist);
        dist=sqrt(dist);
}

/*
* Computes the minimum  distance between two lines
* @param[in] mLine0                             The input line0
* @param[in] mLine1                             The input line1
* @param[out] parallel          true if the two lines are parallel
* @param[mClosestPoint0]  the closest point on line0
* @param[mClosestPoint1]  the closest point on line1
*/
template <class ScalarType>
void LineLineDistance(const vcg::Line3<ScalarType> &mLine0,
                                                                                        const vcg::Line3<ScalarType> &mLine1,
                                                                                        bool &parallel,
                                                                                        ScalarType &dist,
                                                                                        vcg::Point3<ScalarType> &mClosestPoint0,
                                                                                        vcg::Point3<ScalarType> &mClosestPoint1)
{
  const ScalarType loc_EPSILON=ScalarType(0.000000001);
        typedef typename vcg::Point3<ScalarType> CoordType;
  CoordType diff = mLine0.Origin() - mLine1.Origin();
  ScalarType a01 = -mLine0.Direction()* mLine1.Direction();
  ScalarType b0 = diff *(mLine0.Direction());
  ScalarType c = diff.SquaredNorm();
  ScalarType det = fabs((ScalarType)1 - a01*a01);
  ScalarType b1, s0, s1, sqrDist;

    if (det >=loc_EPSILON)
    {
        // Lines are not parallel.
        b1 = -diff*(mLine1.Direction());
        ScalarType invDet = ((ScalarType)1)/det;
        s0 = (a01*b1 - b0)*invDet;
        s1 = (a01*b0 - b1)*invDet;
        sqrDist = s0*(s0 + a01*s1 + ((ScalarType)2)*b0) +
            s1*(a01*s0 + s1 + ((ScalarType)2)*b1) + c;
                                parallel=false;
    }
    else
    {
        // Lines are parallel, select any closest pair of points.
        s0 = -b0;
        s1 = (ScalarType)0;
        sqrDist = b0*s0 + c;
                                parallel=true;
    }
    mClosestPoint0 = mLine0.Origin() + mLine0.Direction()*s0;
    mClosestPoint1 = mLine1.Origin() + mLine1.Direction()*s1;
    /*mLine0Parameter = s0;
    mLine1Parameter = s1;*/

    // Account for numerical round-off errors.
    if (sqrDist < (ScalarType)0)
    {
        sqrDist = (ScalarType)0;
    }
    dist=sqrt(sqrDist);
}


/*
* Computes the minimum distance between a segment and a point
* @param[in] segment    The input segment
* @param[in] p                          The input point
* @param[in] clos                       The closest point
* @param[in] sqr_dist The squared distance
*/
template <class ScalarType> 
void SegmentPointSquaredDistance( const Segment3<ScalarType> &s,
                                                                                                                                                                                         const Point3<ScalarType> & p,
                                                                                                                                                                                         Point3< ScalarType > &closest,
                                                                                                                                                                                         ScalarType &sqr_dist) 
{
        Point3<ScalarType> e = s.P1()-s.P0();
        ScalarType eSquaredNorm = e.SquaredNorm();
        if (eSquaredNorm < std::numeric_limits<ScalarType>::min())
        {
                closest=s.MidPoint();
                sqr_dist=SquaredDistance(closest,p);
        }
        else
        {
                ScalarType  t = ((p-s.P0())*e)/eSquaredNorm;
                if(t<0)      t = 0;
                else if(t>1) t = 1;
                closest = s.P0()+e*t;
                sqr_dist = SquaredDistance(p,closest);
                assert(!math::IsNAN(sqr_dist));
        }
}

/*
* Computes the minimum distance between a segment and a point
* @param[in] segment    The input segment
* @param[in] p                          The input point
* @param[in] clos                       The closest point
* @param[in] dist The distance
*/
template <class ScalarType> 
void SegmentPointDistance( Segment3<ScalarType> s, 
                                                                                                        const Point3<ScalarType> & p,
                                                                                                        Point3< ScalarType > &clos,
                                                                                                        ScalarType &dist) 
{
        SegmentPointSquaredDistance(s,p,clos,dist);
        dist=sqrt(dist);
}

/*
* Computes the minimum distance between two segments
* @param[in] s0                         The input segment0
* @param[in] s1                         The input segment1
* @param[out] parallel          true if the two segments are parallel
* @param[out] dist              the distance
* @param[closest0]  the closest point on segment0
* @param[closest1]  the closest point on segment1
*/
template <class ScalarType>
void SegmentSegmentDistance(const vcg::Segment3<ScalarType> &s0,
                                                                                                                const vcg::Segment3<ScalarType> &s1,
                                                                                                                ScalarType &dist,
                                                                                                                bool &parallel,
                                                                                                                vcg::Point3<ScalarType> &closest0,
                                                                                                                vcg::Point3<ScalarType> &closest1)

{
        typedef typename vcg::Point3<ScalarType> CoordType;

        vcg::Line3<ScalarType> l0,l1;

        l0.SetOrigin(s0.P0());
        l0.SetDirection((s0.P1()-s0.P0()).Normalize());

        l1.SetOrigin(s1.P0());
        l1.SetDirection((s1.P1()-s1.P0()).Normalize());

        ScalarType line_dist;
        CoordType closest_test0,closest_test1;
        LineLineDistance(l0,l1,parallel,line_dist,closest_test0,closest_test1);
        if (parallel)
        {
                ScalarType dist_test;
                CoordType clos_test;
    //CoordType to_test[4]={s1.P0(),s1.P1(),s0.P0(),s1.P1()};

                SegmentPointSquaredDistance(s0,s1.P0(),clos_test,dist);
                closest0=clos_test;
                closest1=s1.P0();
                SegmentPointSquaredDistance(s0,s1.P1(),clos_test,dist_test);
                if (dist_test<dist)
                {
                        dist=dist_test;
                        closest0=clos_test;
                        closest1=s1.P1();
                }
                SegmentPointSquaredDistance(s1,s0.P0(),clos_test,dist_test);
                if (dist_test<dist)
                {
                        dist=dist_test;
                        closest0=s0.P0();
                        closest1=clos_test;
                }
                SegmentPointSquaredDistance(s1,s0.P1(),clos_test,dist_test);
                if (dist_test<dist)
                {
                        dist=dist_test;
                        closest0=s0.P1();
                        closest1=clos_test;
                }
                dist=sqrt(dist);
                return;
        }

        ScalarType sqr_dist0;
        SegmentPointSquaredDistance(s0,closest_test0,closest0,sqr_dist0);
        ScalarType sqr_dist1;
        SegmentPointSquaredDistance(s1,closest_test1,closest1,sqr_dist1);

        dist=(closest0-closest1).Norm();
}

 /* @brief Computes the distance between a triangle and a point.
 *
 * @param t         reference to the triangle
 * @param q         point location
 * @param dist      distance from p to t
 * @param closest   perpendicular projection of p onto t
 */
template<class ScalarType>
void TrianglePointDistance(const vcg::Triangle3<ScalarType> &t,
                           const typename vcg::Point3<ScalarType> & q,
                           ScalarType & dist,
                           typename vcg::Point3<ScalarType> & closest )
{
        typedef typename vcg::Point3<ScalarType> CoordType;

        CoordType clos[3];
        ScalarType distv[3];
        CoordType clos_proj;
        ScalarType distproj;

        vcg::Plane3<ScalarType> plane;
        plane.Init(t.P(0),t.P(1),t.P(2));
        clos_proj=plane.Projection(q);

        CoordType n=(t.P(1)-t.P(0))^(t.P(2)-t.P(0));
        CoordType n0=(t.P(0)-clos_proj)^(t.P(1)-clos_proj);
        CoordType n1=(t.P(1)-clos_proj)^(t.P(2)-clos_proj);
        CoordType n2=(t.P(2)-clos_proj)^(t.P(0)-clos_proj);
        distproj=(clos_proj-q).Norm();
        if (((n*n0)>=0)&&((n*n1)>=0)&&((n*n2)>=0))
        {
                closest=clos_proj;
                dist=distproj;
                return;
        }
        

        //distance from the edges
        vcg::Segment3<ScalarType> e0=vcg::Segment3<ScalarType>(t.P(0),t.P(1));
        vcg::Segment3<ScalarType> e1=vcg::Segment3<ScalarType>(t.P(1),t.P(2));
        vcg::Segment3<ScalarType> e2=vcg::Segment3<ScalarType>(t.P(2),t.P(0));
        SegmentPointDistance(e0,q,clos[0],distv[0]);
        SegmentPointDistance(e1,q,clos[1],distv[1]);
        SegmentPointDistance(e2,q,clos[2],distv[2]);
        /*clos[0]=ClosestPoint<ScalarType>( e0, q);
        clos[1]=ClosestPoint<ScalarType>( e1, q);
        clos[2]=ClosestPoint<ScalarType>( e2, q);
        */
        //distv[0]=(clos[0]-q).Norm();
        //distv[1]=(clos[1]-q).Norm();
        //distv[2]=(clos[2]-q).Norm();
        int min=0;

        for (int i=1;i<3;i++)
        {
                if (distv[i]<distv[min])
                        min=i;
        }

        closest=clos[min];
        dist=distv[min];
}


/*
* return the distance between a triangle and a segment
* @param[in] t                          The input triangle
* @param[in] s                          The input segment
* @param[out] dist              the distance
*/
template<class ScalarType>
void TriangleSegmentDistance(const vcg::Triangle3<ScalarType> &t,
                                                                                                                 const vcg::Segment3<ScalarType> &s,
                                                                                                                 ScalarType & dist)
{
        dist=std::numeric_limits<ScalarType>::max();
        ScalarType a,b;
        typedef typename vcg::Point3<ScalarType> CoordType;

        bool intersect=IntersectionSegmentTriangle<vcg::Triangle3<ScalarType> >(s,t,a,b);
        if (intersect)
        {
                dist=0;
                return;
        }
        vcg::Plane3<ScalarType> pl3;
        pl3.Init(t.P(0),t.P(1),t.P(2));
        CoordType pj0=pl3.Projection(s.P(0));
        CoordType pj1=pl3.Projection(s.P(1));
        ScalarType dpj0=(pj0-s.P(0)).Norm();
        ScalarType dpj1=(pj1-s.P(1)).Norm();

        CoordType bary0,bary1;
        bool Inside0=vcg::InterpolationParameters(t,pj0,bary0);
        bool Inside1=vcg::InterpolationParameters(t,pj1,bary1);
        if (Inside0&&Inside1)
        {
                dist=std::min(dpj0,dpj1);
                return;
        }
        if (Inside0)
                dist=dpj0;
        if (Inside1)
                dist=dpj1;

        for (int i=0;i<3;i++)
 {
         vcg::Segment3<ScalarType> edge=vcg::Segment3<ScalarType>(t.P0(i),t.P0((i+1)%3));
         ScalarType test_dist;
         CoordType clos1,clos2;
         bool parallel;
         vcg::SegmentSegmentDistance<ScalarType>(s,edge,test_dist,parallel,clos1,clos2);
         if (test_dist<dist)
                 dist=test_dist;
 }
}

/*
* return the minimum distance between two triangles
* @param[in] t0                         The input triangle0
* @param[in] t1                         The input triangle1
* @param[out] dist              the distance
*/
template<class ScalarType>
void TriangleTriangleDistance(const  vcg::Triangle3<ScalarType> &t0,
                                                                                                                        const  vcg::Triangle3<ScalarType> &t1,
                                                                                                                        ScalarType &dist)
{
  const ScalarType loc_EPSILON=(vcg::DoubleArea(t0)+vcg::DoubleArea(t1))*(ScalarType)0.0000001;
 dist=std::numeric_limits<ScalarType>::max();

 for (int i=0;i<3;i++)
 {
         vcg::Segment3<ScalarType> edge=vcg::Segment3<ScalarType>(t0.P0(i),t0.P0((i+1)%3));
         ScalarType test_dist;
         vcg::TriangleSegmentDistance<ScalarType>(t1,edge,test_dist);
   if (test_dist<loc_EPSILON)
         {
                 dist=0;
                 return;
         }
         if (test_dist<dist)
                 dist=test_dist;
 }
 for (int i=0;i<3;i++)
 {
         vcg::Segment3<ScalarType> edge=vcg::Segment3<ScalarType>(t1.P0(i),t1.P0((i+1)%3));
         ScalarType test_dist;
         vcg::TriangleSegmentDistance<ScalarType>(t0,edge,test_dist);
   if (test_dist<loc_EPSILON)
         {
                 dist=0;
                 return;
         }
         if (test_dist<dist)
                 dist=test_dist;
 }
}

}

#endif