/****************************************************************************
* 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
* @param[out] dist The distance
*/
template <class ScalarType>
void PointBoxDistance(const Point3<ScalarType> &p,
const Box3<ScalarType> &b,
ScalarType& dist)
{
///if fall inside return distance to a face
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));
}
{
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 a 3D box and a point
* @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 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 ¶llel,
ScalarType &dist,
vcg::Point3<ScalarType> &mClosestPoint0,
vcg::Point3<ScalarType> &mClosestPoint1)
{
const ScalarType 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 >=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;
}
///find the two closest points
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( Segment3<ScalarType> s,
const Point3<ScalarType> & p,
Point3< ScalarType > &clos,
ScalarType &sqr_dist)
{
///create a line
vcg::Line3<ScalarType> l;
l.Set(s.P0(),s.P1()-s.P0());
l.Normalize();
///take the closest point on a Line
vcg::LinePointSquaredDistance(l,p,clos,sqr_dist);
///quick test with a BB
vcg::Box3<ScalarType> b;
b.Add(s.P0());
b.Add(s.P1());
///if the point is in the BB since it is ALSO along the line
///means that it is also along the SEGMENT
if (b.IsIn(clos))
return;
else
{
///check the extremes
ScalarType d0=(s.P0()-p).SquaredNorm();
ScalarType d1=(s.P1()-p).SquaredNorm();
if (d0<d1)
clos= (s.P0());
else
clos= (s.P1());
}
}
/*
* 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 ¶llel,
vcg::Point3<ScalarType> &closest0,
vcg::Point3<ScalarType> &closest1)
{
typedef typename vcg::Point3<ScalarType> CoordType;
vcg::Line3<ScalarType> l0,l1;
///construct two lines
l0.SetOrigin(s0.P0());
l0.SetDirection((s0.P1()-s0.P0()).Normalize());
l1.SetOrigin(s1.P0());
l1.SetDirection((s1.P1()-s1.P0()).Normalize());
///then find closest point
ScalarType line_dist;
CoordType closest_test0,closest_test1;
LineLineDistance(l0,l1,parallel,line_dist,closest_test0,closest_test1);
///special case if the two lines are parallel
if (parallel)
{
///find the minimum distance between extremes to segments
ScalarType dist_test;
CoordType clos_test;
CoordType to_test[4]={s1.P0(),s1.P1(),s0.P0(),s1.P1()};
///find combination of distances between all extremes and segments
SegmentPointSquaredDistance(s0,s1.P0(),clos_test,dist);
closest0=clos_test;
closest1=s1.P0();
///and find the minimum updating coherently the closest points
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;
}
///then ffind the closest segments points...
///means that if it is not an extreme then take
///the closest extreme
ScalarType sqr_dist0;
SegmentPointSquaredDistance(s0,closest_test0,closest0,sqr_dist0);
ScalarType sqr_dist1;
SegmentPointSquaredDistance(s1,closest_test1,closest1,sqr_dist1);
///then return the distance
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,
typename ScalarType & dist,
typename vcg::Point3<ScalarType> & closest )
{
typedef typename vcg::Point3<ScalarType> CoordType;
CoordType clos[3];
ScalarType distv[3];
CoordType clos_proj;
ScalarType distproj;
///find distance on the plane
vcg::Plane3<ScalarType> plane;
plane.Init(t.P(0),t.P(1),t.P(2));
clos_proj=plane.Projection(q);
///control if inside/outside
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;
///find minimum distance
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();
///test the intersection
ScalarType a,b;
typedef typename vcg::Point3<ScalarType> CoordType;
bool intersect=IntersectionSegmentTriangle<vcg::Triangle3<ScalarType> >(s,t,a,b);
if (intersect)
{
dist=0;
return;
}
///project endpoints and see if they fall into the triangle
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));
///take distances
ScalarType dpj0=(pj0-s.P(0)).Norm();
ScalarType dpj1=(pj1-s.P(1)).Norm();
///test if they fall inside the triangle
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;
}
///initialize with the sdistance if only once falls into
if (Inside0)
dist=dpj0;
if (Inside1)
dist=dpj1;
///then test segment-to segment distance with edges of the triangle
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 EPSILON=(vcg::DoubleArea(t0)+vcg::DoubleArea(t1))*(ScalarType)0.0000001;
dist=std::numeric_limits<ScalarType>::max();
///test each segment of t1 with t0
///keeping the minimum distance
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<EPSILON)
{
dist=0;
return;
}
if (test_dist<dist)
dist=test_dist;
}
///then viceversa
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<EPSILON)
{
dist=0;
return;
}
if (test_dist<dist)
dist=test_dist;
}
}
}///end namespace vcg
#endif