From 6134c11fbd956cf49667b602cd023c4cae7c744c Mon Sep 17 00:00:00 2001
From: nicopietroni <nico.pietroni@isti.cnr.it>
Date: Fri, 15 Oct 2010 15:13:22 +0000
Subject: [PATCH] first release version of the collection of minimum distance
 functions between geometric entities in 3D

---
 vcg/space/distance3.h | 504 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 504 insertions(+)
 create mode 100644 vcg/space/distance3.h

diff --git a/vcg/space/distance3.h b/vcg/space/distance3.h
new file mode 100644
index 00000000..5bb005ee
--- /dev/null
+++ b/vcg/space/distance3.h
@@ -0,0 +1,504 @@
+/****************************************************************************
+* 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 &parallel,
+											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 &parallel,
+														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
\ No newline at end of file