557 lines
17 KiB
C++
557 lines
17 KiB
C++
/****************************************************************************
|
|
* VCGLib o o *
|
|
* Visual and Computer Graphics Library o o *
|
|
* _ O _ *
|
|
* Copyright(C) 2004-2016 \/)\/ *
|
|
* 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 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));
|
|
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 ¶llel,
|
|
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;
|
|
}
|
|
///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( 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() * (1.0 - t) + s.P1() * 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 ¶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,
|
|
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.P0());
|
|
CoordType pj1=pl3.Projection(s.P1());
|
|
///take distances
|
|
ScalarType dpj0=(pj0-s.P0()).Norm();
|
|
ScalarType dpj1=(pj1-s.P1()).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 loc_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<loc_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<loc_EPSILON)
|
|
{
|
|
dist=0;
|
|
return;
|
|
}
|
|
if (test_dist<dist)
|
|
dist=test_dist;
|
|
}
|
|
}
|
|
|
|
}///end namespace vcg
|
|
|
|
#endif
|