vcglib: intersection3.h Source File

00001 /****************************************************************************
00002  * VCGLib                                                            o o     *
00003  * Visual and Computer Graphics Library                            o     o   *
00004  *                                                                _   O  _   *
00005  * Copyright(C) 2004                                                \/)\/    *
00006  * Visual Computing Lab                                            /\/|      *
00007  * ISTI - Italian National Research Council                           |      *
00008  *                                                                    \      *
00009  * All rights reserved.                                                      *
00010  *                                                                           *
00011  * This program is free software; you can redistribute it and/or modify      *   
00012  * it under the terms of the GNU General Public License as published by      *
00013  * the Free Software Foundation; either version 2 of the License, or         *
00014  * (at your option) any later version.                                       *
00015  *                                                                           *
00016  * This program is distributed in the hope that it will be useful,           *
00017  * but WITHOUT ANY WARRANTY; without even the implied warranty of            *
00018  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
00019  * GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
00020  * for more details.                                                         *
00021  *                                                                           *
00022  ****************************************************************************/
00023 /****************************************************************************
00024   History
00025 
00026 $Log: not supported by cvs2svn $
00027 Revision 1.33  2007/06/07 15:16:39  fiorin
00028 Added IntersectionSphereTriangle
00029 
00030 Revision 1.32  2007/05/29 14:33:29  fiorin
00031 Added IntersectionSegmentSphere
00032 
00033 Revision 1.31  2007/04/16 09:08:15  cignoni
00034 commented out non compiling intersectionSpherePlane
00035 
00036 Revision 1.30  2007/04/10 22:26:47  pietroni
00037 IntersectionPlanePlane first parameter is a const
00038 
00039 Revision 1.29  2007/04/04 23:19:40  pietroni
00040 - Changed name of intersection function between plane and triangle from Intersection to IntersectionPlaneTriangle.
00041 - Added Intersection_Plane_Sphere function.
00042 
00043 Revision 1.28  2007/02/21 02:40:52  m_di_benedetto
00044 Added const qualifier to bbox parameter in Intersection_Triangle_Box().
00045 
00046 Revision 1.27  2006/10/25 16:04:32  pietroni
00047 added intersection control between bounding boxes for intersection between segment and triangle function
00048 
00049 Revision 1.26  2006/09/14 08:39:07  ganovelli
00050 Intersection_sphere_sphere added
00051 
00052 Revision 1.25  2006/06/06 14:35:31  zifnab1974
00053 Changes for compilation on linux AMD64. Some remarks: Linux filenames are case-sensitive. _fileno and _filelength do not exist on linux
00054 
00055 Revision 1.24  2006/06/01 08:38:02  pietroni
00056 Added functions:
00057 
00058 - Intersection_Segment_Triangle
00059 - IntersectionPlaneBox
00060 - Intersection_Triangle_Box
00061 
00062 Revision 1.23  2006/03/29 07:53:36  cignoni
00063 Missing ';' (thx Maarten)
00064 
00065 Revision 1.22  2006/03/20 14:42:49  pietroni
00066 IntersectionSegmentPlane and IntersectionSegmentBox functions Added
00067 
00068 Revision 1.21  2006/01/20 16:35:51  pietroni
00069 added IntersectionSegmentBox function
00070 
00071 Revision 1.20  2005/10/03 16:07:50  ponchio
00072 Changed order of functions intersection_line_box and
00073 intersectuion_ray_box
00074 
00075 Revision 1.19  2005/09/30 13:11:39  pietroni
00076 corrected 1 compiling error on Ray_Box_Intersection function
00077 
00078 Revision 1.18  2005/09/29 15:30:10  pietroni
00079 Added function RayBoxIntersection, renamed intersection line box from "Intersection" to "Intersection_Line_Box"
00080 
00081 Revision 1.17  2005/09/29 11:48:00  m_di_benedetto
00082 Added functor RayTriangleIntersectionFunctor.
00083 
00084 Revision 1.16  2005/09/28 19:40:55  m_di_benedetto
00085 Added intersection for ray-triangle (with Ray3 type).
00086 
00087 Revision 1.15  2005/06/29 15:28:31  callieri
00088 changed intersection names to more specific to avoid ambiguity
00089 
00090 Revision 1.14  2005/03/15 11:22:39  ganovelli
00091 added intersection between tow planes (porting from old vcg lib)
00092 
00093 Revision 1.13  2005/01/26 10:03:08  spinelli
00094 aggiunta intersect  ray-box
00095 
00096 Revision 1.12  2004/10/13 12:45:51  cignoni
00097 Better Doxygen documentation
00098 
00099 Revision 1.11  2004/09/09 14:41:32  ponchio
00100 forgotten typename SEGMENTTYPE::...
00101 
00102 Revision 1.10  2004/08/09 09:48:43  pietroni
00103 correcter .dir to .Direction and .ori in .Origin()
00104 
00105 Revision 1.9  2004/08/04 20:55:02  pietroni
00106 added rey triangle intersections funtions
00107 
00108 Revision 1.8  2004/07/11 22:08:04  cignoni
00109 Added a cast to remove a warning
00110 
00111 Revision 1.7  2004/05/14 03:14:29  ponchio
00112 Fixed some minor bugs
00113 
00114 Revision 1.6  2004/05/13 23:43:54  ponchio
00115 minor bug
00116 
00117 Revision 1.5  2004/05/05 08:21:55  cignoni
00118 syntax errors in inersection plane line.
00119 
00120 Revision 1.4  2004/05/04 02:37:58  ganovelli
00121 Triangle3<T> replaced by TRIANGLE
00122 Segment<T> replaced by EDGETYPE
00123 
00124 Revision 1.3  2004/04/29 10:48:44  ganovelli
00125 error in plane segment corrected
00126 
00127 Revision 1.2  2004/04/26 12:34:50  ganovelli
00128 plane line
00129 plane segment
00130 triangle triangle added
00131 
00132 Revision 1.1  2004/04/21 14:22:27  cignoni
00133 Initial Commit
00134 
00135 
00136 ****************************************************************************/
00137 
00138 
00139 
00140 #ifndef __VCGLIB_INTERSECTION_3
00141 #define __VCGLIB_INTERSECTION_3
00142 
00143 #include <vcg/math/base.h>
00144 #include <vcg/space/point3.h>
00145 #include <vcg/space/line3.h>
00146 #include <vcg/space/ray3.h>
00147 #include <vcg/space/plane3.h>
00148 #include <vcg/space/segment3.h>
00149 #include <vcg/space/sphere3.h>
00150 #include <vcg/space/triangle3.h>
00151 #include <vcg/space/intersection/triangle_triangle3.h>
00152 
00153 
00154 
00155 
00156 namespace vcg {
00163 
00164   template<class T>
00165     inline bool IntersectionLineSphere( const Sphere3<T> & sp, const Line3<T> & li, Point3<T> & p0,Point3<T> & p1 ){
00166 
00167     // Per prima cosa si sposta il sistema di riferimento 
00168     // fino a portare il centro della sfera nell'origine
00169     Point3<T> neworig=li.Origin()-sp.Center();
00170     // poi si risolve il sistema di secondo grado (con maple...)
00171     T t1 = li.Direction().X()*li.Direction().X();
00172     T t2 = li.Direction().Y()*li.Direction().Y();
00173     T t3 = li.Direction().Z()*li.Direction().Z();
00174     T t6 = neworig.Y()*li.Direction().Y();
00175     T t7 = neworig.X()*li.Direction().X();
00176     T t8 = neworig.Z()*li.Direction().Z();
00177     T t15 = sp.Radius()*sp.Radius();
00178     T t17 = neworig.Z()*neworig.Z();
00179     T t19 = neworig.Y()*neworig.Y();
00180     T t21 = neworig.X()*neworig.X();
00181     T t28 = T(2.0*t7*t6+2.0*t6*t8+2.0*t7*t8+t1*t15-t1*t17-t1*t19-t2*t21+t2*t15-t2*t17-t3*t21+t3*t15-t3*t19);
00182     if(t28<0) return false;
00183     T t29 = sqrt(t28);      
00184     T val0 = 1/(t1+t2+t3)*(-t6-t7-t8+t29); 
00185     T val1 = 1/(t1+t2+t3)*(-t6-t7-t8-t29);
00186 
00187     p0=li.P(val0);
00188     p1=li.P(val1);
00189     return true;
00190   }
00191 
00192         /*
00193         * Function computing the intersection between a sphere and a segment.
00194         * @param[in]    sphere                          the sphere
00195         * @param[in]    segment                         the segment
00196         * @param[out]   intersection  the intersection point, meaningful only if the segment intersects the sphere
00197         * \return                       (0, 1 or 2)             the number of intersections between the segment and the sphere. 
00198         *                                                                                                               t1 is a valid intersection only if the returned value is at least 1; 
00199         *                                                                                                               similarly t2 is valid iff the returned value is 2.
00200         */
00201         template < class SCALAR_TYPE >
00202         inline int IntersectionSegmentSphere(const Sphere3<SCALAR_TYPE>& sphere, const Segment3<SCALAR_TYPE>& segment, Point3<SCALAR_TYPE> & t0, Point3<SCALAR_TYPE> & t1)
00203         {
00204                 typedef SCALAR_TYPE                                                                                                     ScalarType;
00205                 typedef typename vcg::Point3< ScalarType >      Point3t;
00206 
00207                 Point3t s = segment.P0() - sphere.Center();
00208                 Point3t r = segment.P1() - segment.P0();
00209 
00210                 ScalarType rho2 = sphere.Radius()*sphere.Radius();
00211 
00212                 ScalarType sr = s*r;
00213                 ScalarType r_squared_norm = r.SquaredNorm(); 
00214                 ScalarType s_squared_norm = s.SquaredNorm(); 
00215                 ScalarType sigma = sr*sr - r_squared_norm*(s_squared_norm-rho2);
00216 
00217                 if (sigma<ScalarType(0.0)) // the line containing the edge doesn't intersect the sphere
00218                         return 0;
00219 
00220                 ScalarType sqrt_sigma = ScalarType(sqrt( ScalarType(sigma) ));
00221                 ScalarType lambda1 = (-sr - sqrt_sigma)/r_squared_norm;
00222                 ScalarType lambda2 = (-sr + sqrt_sigma)/r_squared_norm;
00223 
00224                 int solution_count = 0;
00225                 if (ScalarType(0.0)<=lambda1 && lambda1<=ScalarType(1.0))
00226                 {
00227                         ScalarType t_enter = vcg::math::Max< ScalarType >(lambda1, ScalarType(0.0));
00228                         t0 = segment.P0() + r*t_enter;
00229                         solution_count++;
00230                 }
00231 
00232                 if (ScalarType(0.0)<=lambda2 && lambda2<=ScalarType(1.0))
00233                 {
00234                         Point3t *pt = (solution_count>0) ? &t1 : &t0;
00235                         ScalarType t_exit  = vcg::math::Min< ScalarType >(lambda2, ScalarType(1.0));
00236                         *pt = segment.P0() + r*t_exit;
00237                         solution_count++;
00238                 }
00239                 return solution_count;
00240         }; // end of IntersectionSegmentSphere
00241 
00242         
00252         template < class SCALAR_TYPE, class TRIANGLETYPE >
00253         bool IntersectionSphereTriangle(const vcg::Sphere3	< SCALAR_TYPE >              & sphere  ,
00254                                                                                                                                         TRIANGLETYPE                                                                                                            triangle, 
00255                                                                                                                                         vcg::Point3					< SCALAR_TYPE >         & witness ,
00256                                                                                                                                         std::pair< SCALAR_TYPE, SCALAR_TYPE > * res=NULL)
00257         {
00258                 typedef SCALAR_TYPE                                                                                                             ScalarType;
00259                 typedef typename vcg::Point3< ScalarType >              Point3t;
00260                 typedef TRIANGLETYPE Triangle3t;
00261 
00262                 bool penetration_detected = false;
00263 
00264                 ScalarType radius = sphere.Radius();
00265                 Point3t center = sphere.Center();
00266                 Point3t p0 = triangle.P(0)-center;
00267                 Point3t p1 = triangle.P(1)-center;
00268                 Point3t p2 = triangle.P(2)-center;
00269 
00270                 Point3t p10 = p1-p0;
00271                 Point3t p21 = p2-p1;
00272                 Point3t p20 = p2-p0;
00273 
00274                 ScalarType delta0_p01 =  p10.dot(p1);
00275                 ScalarType delta1_p01 = -p10.dot(p0);
00276                 ScalarType delta0_p02 =  p20.dot(p2);
00277                 ScalarType delta2_p02 = -p20.dot(p0);
00278                 ScalarType delta1_p12 =  p21.dot(p2);
00279                 ScalarType delta2_p12 = -p21.dot(p1);
00280 
00281                 // the closest point can be one of the vertices of the triangle
00282                 if                      (delta1_p01<=ScalarType(0.0) && delta2_p02<=ScalarType(0.0)) { witness = p0; }
00283                 else if (delta0_p01<=ScalarType(0.0) && delta2_p12<=ScalarType(0.0)) { witness = p1; }
00284                 else if (delta0_p02<=ScalarType(0.0) && delta1_p12<=ScalarType(0.0)) { witness = p2; }
00285                 else
00286                 {
00287                         ScalarType temp = p10.dot(p2);
00288                         ScalarType delta0_p012 = delta0_p01*delta1_p12 + delta2_p12*temp;
00289                         ScalarType delta1_p012 = delta1_p01*delta0_p02 - delta2_p02*temp;
00290                         ScalarType delta2_p012 = delta2_p02*delta0_p01 - delta1_p01*(p20.dot(p1));
00291 
00292                         // otherwise, can be a point lying on same edge of the triangle
00293                         if (delta0_p012<=ScalarType(0.0)) 
00294                         {
00295                                 ScalarType denominator = delta1_p12+delta2_p12;
00296                                 ScalarType mu1 = delta1_p12/denominator;
00297                                 ScalarType mu2 = delta2_p12/denominator;
00298                                 witness = (p1*mu1 + p2*mu2);
00299                         }
00300                         else if (delta1_p012<=ScalarType(0.0))
00301                         {
00302                                 ScalarType denominator = delta0_p02+delta2_p02;
00303                                 ScalarType mu0 = delta0_p02/denominator;
00304                                 ScalarType mu2 = delta2_p02/denominator;
00305                                 witness = (p0*mu0 + p2*mu2);
00306                         }
00307                         else if (delta2_p012<=ScalarType(0.0))
00308                         {
00309                                 ScalarType denominator = delta0_p01+delta1_p01;
00310                                 ScalarType mu0 = delta0_p01/denominator;
00311                                 ScalarType mu1 = delta1_p01/denominator;
00312                                 witness = (p0*mu0 + p1*mu1);
00313                         }
00314                         else
00315                         {
00316                                 // or else can be an point internal to the triangle
00317                                 ScalarType denominator =  delta0_p012 + delta1_p012 + delta2_p012;
00318                                 ScalarType lambda0 = delta0_p012/denominator;
00319                                 ScalarType lambda1 = delta1_p012/denominator;
00320                                 ScalarType lambda2 = delta2_p012/denominator;
00321                                 witness = p0*lambda0 + p1*lambda1 + p2*lambda2;
00322                         }
00323                 }
00324 
00325                 if (res!=NULL)
00326                 {
00327                         ScalarType witness_norm = witness.Norm();
00328                         res->first  = vcg::math::Max< ScalarType >( witness_norm-radius, ScalarType(0.0) );
00329                         res->second = vcg::math::Max< ScalarType >( radius-witness_norm, ScalarType(0.0) );
00330                 }
00331                 penetration_detected = (witness.SquaredNorm() <= (radius*radius));
00332                 witness += center;
00333                 return penetration_detected;
00334         }; //end of IntersectionSphereTriangle
00335 
00336 
00338   template<class T>
00339     inline bool IntersectionLinePlane( const Plane3<T> & pl, const Line3<T> & li, Point3<T> & po){
00340     const T epsilon = T(1e-8);
00341 
00342     T k = pl.Direction().dot(li.Direction());                                           // Compute 'k' factor
00343     if( (k > -epsilon) && (k < epsilon))
00344       return false;
00345     T r = (pl.Offset() - pl.Direction().dot(li.Origin()))/k;    // Compute ray distance
00346     po = li.Origin() + li.Direction()*r;
00347     return true;
00348   }
00349         
00351   template<class T>
00352     inline bool IntersectionPlaneSegment( const Plane3<T> & pl, const Segment3<T> & s, Point3<T> & p0){
00353                 T p1_proj = s.P1()*pl.Direction()-pl.Offset();
00354                 T p0_proj = s.P0()*pl.Direction()-pl.Offset();
00355                 if ( (p1_proj>0)-(p0_proj<0)) return false;
00356                 p0 =  s.P0() + (s.P1()-s.P0()) * fabs(p0_proj/(p1_proj-p0_proj));
00357                 return true;
00358   }
00359 
00361   template<class ScalarType>
00362     inline bool IntersectionPlaneSegmentEpsilon(const Plane3<ScalarType> & pl,
00363                                                 const Segment3<ScalarType> & sg,
00364                                                 Point3<ScalarType> & po,
00365                                                 const ScalarType epsilon = ScalarType(1e-8)){
00366 
00367     typedef ScalarType T;
00368     T k = pl.Direction().dot((sg.P1()-sg.P0()));
00369     if( (k > -epsilon) && (k < epsilon))
00370       return false;
00371     T r = (pl.Offset() - pl.Direction().dot(sg.P0()))/k;        // Compute ray distance
00372     if( (r<0) || (r > 1.0))
00373       return false;
00374     po = sg.P0()*(1-r)+sg.P1() * r;
00375     return true;
00376   }
00377 
00379   // not optimal: uses plane-segment intersection (and the fact the two or none edges can be intersected)
00380   template<typename TRIANGLETYPE> 
00381     inline bool IntersectionPlaneTriangle( const Plane3<typename TRIANGLETYPE::ScalarType> & pl, 
00382                               const  TRIANGLETYPE & tr, 
00383             Segment3<typename TRIANGLETYPE::ScalarType> & sg)
00384     {
00385       typedef typename TRIANGLETYPE::ScalarType T;
00386       if(IntersectionPlaneSegment(pl,Segment3<T>(tr.P(0),tr.P(1)),sg.P0())){
00387         if(IntersectionPlaneSegment(pl,Segment3<T>(tr.P(0),tr.P(2)),sg.P1()))
00388           return true;
00389         else
00390         {
00391           IntersectionPlaneSegment(pl,Segment3<T>(tr.P(1),tr.P(2)),sg.P1());
00392           return true;
00393         }
00394       }
00395       else
00396       {
00397         if(IntersectionPlaneSegment(pl,Segment3<T>(tr.P(1),tr.P(2)),sg.P0()))
00398         {
00399           IntersectionPlaneSegment(pl,Segment3<T>(tr.P(0),tr.P(2)),sg.P1());
00400           return true;
00401         }
00402       }
00403       return false;
00404     }
00405 
00407   template<typename TRIANGLETYPE> 
00408     inline bool IntersectionTriangleTriangle(const TRIANGLETYPE & t0,const TRIANGLETYPE & t1){
00409     return NoDivTriTriIsect(t0.P0(0),t0.P0(1),t0.P0(2),
00410                             t1.P0(0),t1.P0(1),t1.P0(2));
00411   }
00412 
00413   template<class T>
00414     inline bool IntersectionTriangleTriangle(Point3<T> V0,Point3<T> V1,Point3<T> V2,
00415                              Point3<T> U0,Point3<T> U1,Point3<T> U2){
00416     return NoDivTriTriIsect(V0,V1,V2,U0,U1,U2);
00417   }
00418 #if 0
00419   template<class T>
00420     inline bool Intersection(Point3<T> V0,Point3<T> V1,Point3<T> V2,
00421                              Point3<T> U0,Point3<T> U1,Point3<T> U2,int *coplanar,
00422                              Point3<T> &isectpt1,Point3<T> &isectpt2){
00423 
00424     return tri_tri_intersect_with_isectline(V0,V1,V2,U0,U1,U2,
00425                                             coplanar,isectpt1,isectpt2);
00426   }
00427 
00428   template<typename TRIANGLETYPE,typename SEGMENTTYPE >
00429     inline bool Intersection(const TRIANGLETYPE & t0,const TRIANGLETYPE & t1,bool &coplanar,
00430                              SEGMENTTYPE  & sg){
00431     Point3<typename SEGMENTTYPE::PointType> ip0,ip1; 
00432     return  tri_tri_intersect_with_isectline(t0.P0(0),t0.P0(1),t0.P0(2),
00433                                              t1.P0(0),t1.P0(1),t1.P0(2),
00434                                              coplanar,sg.P0(),sg.P1()
00435                                              );              
00436   }
00437 
00438 #endif  
00439          
00440         /*
00441         * Function computing the intersection between a line and a triangle.
00442         * from: 
00443         * Tomas Moller and Ben Trumbore,  
00444         * ``Fast, Minimum Storage Ray-Triangle Intersection'',  
00445         * journal of graphics tools, vol. 2, no. 1, pp. 21-28, 1997
00446         * @param[in]    line                             
00447         * @param[in]    triangle vertices                        
00448         * @param[out]=(t,u,v)   the intersection point, meaningful only if the line intersects the triangle
00449         *            t is the line parameter and
00450         *            (u,v) are the baricentric coords of the intersection point
00451         *
00452         *                 Line.Orig + t * Line.Dir = (1-u-v) * Vert0 + u * Vert1 +v * Vert2 
00453         *
00454         */
00455 
00456 template<class T>
00457 bool IntersectionLineTriangle( const Line3<T> & line, const Point3<T> & vert0, 
00458                                   const Point3<T> & vert1, const Point3<T> & vert2,
00459                                   T & t ,T & u, T & v)
00460 {
00461         #define EPSIL 0.000001
00462 
00463         vcg::Point3<T> edge1, edge2, tvec, pvec, qvec;
00464         T det,inv_det;
00465 
00466    /* find vectors for two edges sharing vert0 */
00467    edge1 = vert1 - vert0;
00468    edge2 = vert2 - vert0;
00469 
00470    /* begin calculating determinant - also used to calculate U parameter */
00471    pvec =  line.Direction() ^ edge2;
00472 
00473    /* if determinant is near zero, line lies in plane of triangle */
00474    det =  edge1 *  pvec;
00475 
00476    /* calculate distance from vert0 to line origin */
00477    tvec = line.Origin() - vert0;
00478    inv_det = 1.0 / det;
00479    
00480    qvec = tvec ^ edge1;
00481       
00482    if (det > EPSIL)
00483    {
00484       u =  tvec * pvec ;
00485       if ( u < 0.0 ||  u > det)
00486          return 0;
00487             
00488       /* calculate V parameter and test bounds */
00489        v =  line.Direction() * qvec;
00490       if ( v < 0.0 ||  u +  v > det)
00491          return 0;
00492       
00493    }
00494    else if(det < -EPSIL)
00495    {
00496       /* calculate U parameter and test bounds */
00497        u =  tvec * pvec ;
00498       if ( u > 0.0 ||  u < det)
00499          return 0;
00500       
00501       /* calculate V parameter and test bounds */
00502        v =  line.Direction() * qvec  ;
00503       if ( v > 0.0 ||  u +  v < det)
00504          return 0;
00505    }
00506    else return 0;  /* line is parallell to the plane of the triangle */
00507 
00508     t = edge2 *  qvec  * inv_det;
00509    ( u) *= inv_det;
00510    ( v) *= inv_det;
00511 
00512    return 1;
00513 }
00514 
00515 template<class T>
00516 bool IntersectionRayTriangle( const Ray3<T> & ray, const Point3<T> & vert0, 
00517                                   const Point3<T> & vert1, const Point3<T> & vert2,
00518                                   T & t ,T & u, T & v)
00519 {
00520         Line3<T> line(ray.Origin(), ray.Direction());
00521         if (IntersectionLineTriangle(line, vert0, vert1, vert2, t, u, v))
00522         {
00523                 if (t < 0) return 0;
00524                 else return 1; 
00525         }else return 0;
00526 }
00527 
00528 // line-box
00529 template<class T>
00530 bool IntersectionLineBox( const Box3<T> & box, const Line3<T> & r, Point3<T> & coord )
00531 {
00532         const int NUMDIM = 3;
00533         const int RIGHT  = 0;
00534         const int LEFT   = 1;
00535         const int MIDDLE = 2;
00536 
00537         int inside = 1;
00538         char quadrant[NUMDIM];
00539     int i;
00540     int whichPlane;
00541     Point3<T> maxT,candidatePlane;
00542     
00543         // Find candidate planes; this loop can be avoided if
00544         // rays cast all from the eye(assume perpsective view)
00545     for (i=0; i<NUMDIM; i++)
00546     {
00547         if(r.Origin()[i] < box.min[i])
00548                 {
00549                         quadrant[i] = LEFT;
00550                         candidatePlane[i] = box.min[i];
00551                         inside = 0;
00552                 }
00553                 else if (r.Origin()[i] > box.max[i])
00554                 {
00555                         quadrant[i] = RIGHT;
00556                         candidatePlane[i] = box.max[i];
00557                         inside = 0;
00558                 }
00559                 else
00560                 {
00561                         quadrant[i] = MIDDLE;
00562                 }
00563     }
00564 
00565                 // Ray origin inside bounding box
00566         if(inside){
00567             coord = r.Origin();
00568             return true;
00569         }
00570 
00571         // Calculate T distances to candidate planes 
00572     for (i = 0; i < NUMDIM; i++)
00573     {
00574                 if (quadrant[i] != MIDDLE && r.Direction()[i] !=0.)
00575                         maxT[i] = (candidatePlane[i]-r.Origin()[i]) / r.Direction()[i];
00576                 else
00577                         maxT[i] = -1.;
00578     }
00579 
00580         // Get largest of the maxT's for final choice of intersection
00581     whichPlane = 0;
00582     for (i = 1; i < NUMDIM; i++)
00583             if (maxT[whichPlane] < maxT[i])
00584                         whichPlane = i;
00585 
00586         // Check final candidate actually inside box 
00587     if (maxT[whichPlane] < 0.) return false;
00588     for (i = 0; i < NUMDIM; i++)
00589                 if (whichPlane != i)
00590                 {
00591                         coord[i] = r.Origin()[i] + maxT[whichPlane] *r.Direction()[i];
00592                         if (coord[i] < box.min[i] || coord[i] > box.max[i])
00593                                 return false;
00594                 }
00595                 else
00596                 {
00597                         coord[i] = candidatePlane[i];
00598                 }
00599     return true;                        // ray hits box
00600 }       
00601 
00602 // ray-box
00603 template<class T>
00604 bool IntersectionRayBox( const Box3<T> & box, const Ray3<T> & r, Point3<T> & coord )
00605 {
00606         Line3<T> l;
00607         l.SetOrigin(r.Origin());
00608         l.SetDirection(r.Direction());
00609   return(IntersectionLineBox<T>(box,l,coord));
00610 }       
00611 
00612 // segment-box return fist intersection found  from p0 to p1
00613 template<class ScalarType>
00614 bool IntersectionSegmentBox( const Box3<ScalarType> & box,
00615                                                           const Segment3<ScalarType> & s, 
00616                                                           Point3<ScalarType> & coord )
00617 {
00618         //as first perform box-box intersection
00619         Box3<ScalarType> test;
00620         test.Add(s.P0());
00621         test.Add(s.P1());
00622         if (!test.Collide(box))
00623                 return false;
00624         else
00625         {
00626                 Line3<ScalarType> l;
00627                 Point3<ScalarType> dir=s.P1()-s.P0();
00628                 dir.Normalize();
00629                 l.SetOrigin(s.P0());
00630                 l.SetDirection(dir);
00631     if(IntersectionLineBox<ScalarType>(box,l,coord))
00632                         return (test.IsIn(coord));
00633                 return false;
00634         }
00635 }
00636 
00637 // segment-box intersection , return number of intersections and intersection points
00638 template<class ScalarType>
00639 int IntersectionSegmentBox( const Box3<ScalarType> & box,
00640                                                          const Segment3<ScalarType> & s,
00641                                                          Point3<ScalarType> & coord0,
00642                                                          Point3<ScalarType> & coord1 )
00643 {
00644         int num=0;
00645         Segment3<ScalarType> test= s;
00646   if (IntersectionSegmentBox(box,test,coord0 ))
00647         {
00648                 num++;
00649                 Point3<ScalarType> swap=test.P0();
00650                 test.P0()=test.P1();
00651                 test.P1()=swap;
00652     if (IntersectionSegmentBox(box,test,coord1 ))
00653                         num++;
00654         }
00655         return num;
00656 }       
00657 
00668 template<class ScalarType>
00669 bool IntersectionSegmentTriangle( const vcg::Segment3<ScalarType> & seg,
00670                                                                    const Point3<ScalarType> & vert0, 
00671                                                                         const Point3<ScalarType> & vert1, const
00672                                                                         Point3<ScalarType> & vert2,
00673                                                                         ScalarType & a ,ScalarType & b, ScalarType & dist)
00674 {
00675         //control intersection of bounding boxes
00676         vcg::Box3<ScalarType> bb0,bb1;
00677         bb0.Add(seg.P0());
00678         bb0.Add(seg.P1());
00679         bb1.Add(vert0);
00680         bb1.Add(vert1);
00681         bb1.Add(vert2);
00682         Point3<ScalarType> inter;
00683         if (!bb0.Collide(bb1))
00684                 return false;
00685   if (!vcg::IntersectionSegmentBox(bb1,seg,inter))
00686                 return false;
00687 
00688         //first set both directions of ray
00689   vcg::Line3<ScalarType> line;
00690         vcg::Point3<ScalarType> dir;
00691         dir=(seg.P1()-seg.P0());
00692         dir.Normalize();
00693   line.Set(seg.P0(),dir);
00694   if(IntersectionLineTriangle<ScalarType>(line,vert0,vert1,vert2,dist,a,b))
00695     return (dist>=0 && dist<=1.0);
00696     return false;
00697 }
00702 template<class TriangleType>
00703 bool IntersectionSegmentTriangle( const vcg::Segment3<typename TriangleType::ScalarType> & seg,
00704                   const TriangleType &t,
00705                   typename TriangleType::ScalarType & a ,typename TriangleType::ScalarType & b, typename TriangleType::ScalarType & dist)
00706 {
00707   return IntersectionSegmentTriangle(seg,t.P(0),t.P(1),t.P(2),a,b,dist);
00708 }
00709 
00710 template<class ScalarType>
00711 bool IntersectionPlaneBox(const vcg::Plane3<ScalarType> &pl,
00712                                                         vcg::Box3<ScalarType> &bbox)
00713 {
00714         typedef typename vcg::Segment3<ScalarType> SegmentType;
00715         typedef typename vcg::Point3<ScalarType> CoordType;
00716         SegmentType diag[4];
00717         
00718         CoordType intersection;
00719         //find the 4 diagonals
00720         diag[0]=SegmentType(bbox.P(0),bbox.P(7));
00721         diag[1]=SegmentType(bbox.P(1),bbox.P(6));
00722         diag[2]=SegmentType(bbox.P(2),bbox.P(5));
00723         diag[3]=SegmentType(bbox.P(3),bbox.P(4));
00724         ScalarType a,b,dist;
00725         for (int i=0;i<3;i++)
00726                 //call intersection of segment and plane
00727     if (vcg::IntersectionPlaneSegment(pl,diag[i],intersection))
00728                         return true;
00729         return false;
00730 }
00731 
00734 //the plane
00735 template <class ScalarType>
00736 bool IntersectionPlaneSphere(const Plane3<ScalarType> &pl,
00737                                                            const Sphere3<ScalarType> &sphere,
00738                                                            Point3<ScalarType> &center,
00739                                                            ScalarType &radius)
00740 {
00741         /* ///set the origin on the center of the sphere
00742         vcg::Plane3<ScalarType> pl1;
00743         vcg::Point3<ScalarType> p_plane=pl.Direction()*pl.Offset();
00744         vcg::Point3<ScalarType> p_plane=p_plane-sphere.Center();
00745 
00747         pl1.Set(p_plane,pl.Direction());
00748 
00750         d=pl1.Offset();
00751         vcg::Point3<ScalarType> n=pl1.Direction();
00753         if (d<0)
00754         {
00755                 n=-n;
00756                 d=-d;
00757         }
00759         if (d>r)
00760                 return false;
00761         else
00762         {
00764                 center=n*d;
00765                 center+=sphere.Center();
00766                 radius=math::Sqrt((r*r)-(d*d));
00767         }
00768          */
00769          assert(0);
00770          return true;
00771 }
00772 
00773 
00774 template<class ScalarType>
00775 bool IntersectionTriangleBox(const vcg::Box3<ScalarType>   &bbox,
00776                                                            const vcg::Point3<ScalarType> &p0,
00777                                                            const vcg::Point3<ScalarType> &p1,
00778                                                            const vcg::Point3<ScalarType> &p2)
00779 {
00780         typedef typename vcg::Point3<ScalarType> CoordType;
00781         CoordType intersection;
00782 
00784         vcg::Box3<ScalarType> test;
00785         test.SetNull();
00786         test.Add(p0);
00787         test.Add(p1);
00788         test.Add(p2);
00789         if (!test.Collide(bbox))
00790                 return false;
00792         if ((bbox.IsIn(p0))||(bbox.IsIn(p1))||(bbox.IsIn(p2)))
00793                 return true;
00794 
00796         //vcg::Plane3<ScalarType> plTri=vcg::Plane3<ScalarType>();
00797         //plTri.Init(p0,p1,p2);
00798   //if (!IntersectionPlaneBox<ScalarType>(plTri,bbox))
00799         //      return false;
00800 
00802   if (IntersectionSegmentBox<ScalarType>(bbox,vcg::Segment3<ScalarType>(p0,p1),intersection)||
00803     IntersectionSegmentBox<ScalarType>(bbox,vcg::Segment3<ScalarType>(p1,p2),intersection)||
00804     IntersectionSegmentBox<ScalarType>(bbox,vcg::Segment3<ScalarType>(p2,p0),intersection))
00805                 return true;
00807 
00808         Segment3<ScalarType> diag[4];
00809 
00810         diag[0]=Segment3<ScalarType>(bbox.P(0),bbox.P(7));
00811         diag[1]=Segment3<ScalarType>(bbox.P(1),bbox.P(6));
00812         diag[2]=Segment3<ScalarType>(bbox.P(2),bbox.P(5));
00813         diag[3]=Segment3<ScalarType>(bbox.P(3),bbox.P(4));
00814         ScalarType a,b,dist;
00815         for (int i=0;i<3;i++)
00816     if (IntersectionSegmentTriangle<ScalarType>(diag[i],p0,p1,p2,a,b,dist))
00817                         return true;
00818 
00819         return false;
00820 }
00821 
00822 template <class SphereType>
00823 bool IntersectionSphereSphere( const SphereType & s0,const SphereType & s1){
00824         return (s0.Center()-s1.Center()).SquaredNorm() < (s0.Radius()+s1.Radius())*(s0.Radius()+s1.Radius());
00825 }
00826 
00827 template<class T>
00828 bool IntersectionPlanePlane (const Plane3<T> & plane0, const Plane3<T> & plane1,
00829                              Line3<T> & line)
00830 {
00831     // If Cross(N0,N1) is zero, then either planes are parallel and separated
00832     // or the same plane.  In both cases, 'false' is returned.  Otherwise,
00833     // the intersection line is
00834     //
00835     //   L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
00836     //
00837     // for some coefficients c0 and c1 and for t any real number (the line
00838     // parameter).  Taking dot products with the normals,
00839     //
00840     //   d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
00841     //   d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
00842     //
00843     // which are two equations in two unknowns.  The solution is
00844     //
00845     //   c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
00846     //   c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
00847     //
00848     // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.
00849 
00850     T n00 = plane0.Direction()*plane0.Direction();
00851     T n01 = plane0.Direction()*plane1.Direction();
00852     T n11 = plane1.Direction()*plane1.Direction();
00853     T det = n00*n11-n01*n01;
00854 
00855     const T tolerance = (T)(1e-06f);
00856                 if ( math::Abs(det) < tolerance )
00857         return false;
00858 
00859     T invDet = 1.0f/det;
00860     T c0 = (n11*plane0.Offset() - n01*plane1.Offset())*invDet;
00861     T c1 = (n00*plane1.Offset() - n01*plane0.Offset())*invDet;
00862 
00863     line.SetDirection(plane0.Direction()^plane1.Direction());
00864     line.SetOrigin(plane0.Direction()*c0+ plane1.Direction()*c1);
00865 
00866     return true;
00867 }
00868 
00869 
00870 // Ray-Triangle Functor
00871 template <bool BACKFACETEST = true>
00872 class RayTriangleIntersectionFunctor {
00873 public:
00874         template <class TRIANGLETYPE, class SCALARTYPE>
00875         inline bool operator () (const TRIANGLETYPE & f, const Ray3<SCALARTYPE> & ray, SCALARTYPE & t) {
00876                 typedef SCALARTYPE ScalarType;
00877                 ScalarType u;
00878                 ScalarType v;
00879 
00880                 bool bret = IntersectionRayTriangle(ray, Point3<SCALARTYPE>::Construct(f.P(0)), Point3<SCALARTYPE>::Construct(f.P(1)), Point3<SCALARTYPE>::Construct(f.P(2)), t, u, v);
00881                 if (BACKFACETEST) {
00882                         if (!bret) {
00883                                 bret = IntersectionRayTriangle(ray, Point3<SCALARTYPE>::Construct(f.P(0)), Point3<SCALARTYPE>::Construct(f.P(2)), Point3<SCALARTYPE>::Construct(f.P(1)), t, u, v);
00884                         }
00885                 }
00886                 return (bret);
00887         }
00888 };
00889         
00890 
00894 } // end namespace
00895 #endif