intersection.cpp

00001 #include "intersection.h"
00002 
00003 #include <geometry/coordinates.h>
00004 
00005 #include <QTextStream>
00006 
00007 static QTextStream out(stderr);
00008 
00009 namespace geometry
00010 {
00011 
00012   using util::normsq;
00013   using util::normalized;
00014 
00015   bool planeLineIntersection(Point3d &u, double &s,
00016                              const Point3d& p, const Point3d& n1, const Point3d& u1, const Point3d& u2)
00017   {
00018     // Normalize n, check if 0
00019     Point3d n = n1;
00020     double nlen = n.norm();
00021     if(nlen != 0)
00022       n /= nlen;
00023     else
00024       return false;
00025 
00026     Point3d u1u2 = u2 - u1;
00027     double t = n * u1u2;
00028     // Check if parallel
00029     if(t == 0)
00030       return false;
00031 
00032     s = n * (p - u1)/t;
00033     u = u1 + s * u1u2;
00034     return true;
00035   }
00036 
00037   template <size_t N>
00038   static bool pointInTriangle_nd(const util::Vector<N,double>& p,
00039                                  const util::Vector<N,double>& p1, const util::Vector<N,double>& p2, const util::Vector<N,double>& p3)
00040   {
00041     // First, place everything in the reference system of p1
00042     util::Vector<N,double> v13 = p3 - p1;
00043     util::Vector<N,double> v12 = p2 - p1;
00044     util::Vector<N,double> v1 = p - p1;
00045     // Compute barycentric coordinates of p
00046     double v1v12 = v1*v12;
00047     double v1v13 = v1*v13;
00048     double v12v13 = v12*v13;
00049     double l12 = v12*v12;
00050     double l13 = v13*v13;
00051 
00052     double denom = v12v13*v12v13 - l12*l13;
00053     if(fabs(denom) < VVE_EPSILON) // Flat triangle ...
00054       return false;
00055     double invDenom = 1/denom;
00056     double u = (v1v13*v12v13-v1v12*l13)*invDenom;
00057     double v = (v1v12*v12v13-v1v13*l12)*invDenom;
00058     return (u >= 0) and (v >= 0) and (u+v <= 1);
00059   }
00060 
00061   bool pointInTriangle(const Point2d& p,
00062                        const Point2d& p1, const Point2d& p2, const Point2d& p3)
00063   {
00064     return pointInTriangle_nd(p, p1, p2, p3);
00065   }
00066 
00067   bool pointInTriangle( Point3d& u, double &s,
00068                        const Point3d& p, const Point3d& p1, const Point3d& p2, const Point3d& p3)
00069   {
00070     Point3d n = normalized((p2-p1)^(p3-p1));
00071     planeLineIntersection(u, s, p1, n, p, p+n);
00072     s *= s;
00073     return pointInTriangle_nd(u, p1, p2, p3);
00074   }
00075 
00076   bool lineTriangleIntersection(Point3d& intersection, double& s,
00077                                 const Point3d& p1, const Point3d& p2,
00078                                 const Point3d& tr1, const Point3d& tr2, const Point3d& tr3)
00079   {
00080     s = FP_NAN;
00081     Point3d normal = normalized((tr2-tr1) ^ (tr3-tr1));
00082     if(planeLineIntersection(intersection, s, tr1, normal, p1, p2))
00083     {
00084       Point3d u = tr2 - tr1;
00085       Point3d v = tr3 - tr1;
00086       Point3d n = u^v;
00087       if(n.norm() < VVE_EPSILON) // Triangle is degenerate
00088       {
00089         return false;
00090       }
00091       Point3d w = intersection - tr1;
00092       double uu, uv, vv, wu, wv, D;
00093       uu = u*u;
00094       uv = u*v;
00095       vv = v*v;
00096       wu = w*u;
00097       wv = w*v;
00098       D = uv * uv - uu * vv;
00099 
00100       double s1, t1;
00101       s1 = (uv*wv-vv*wu)/D;
00102       if(s1 < 0 || s1 > 1)
00103         return false;
00104       t1 = (uv*wu - uu*wv)/D;
00105       if(t1 < 0 || (s1+t1) > 1)
00106         return false;
00107       return true;
00108     }
00109     return false;
00110   }
00111 
00112   bool pointInPolygon(const Point2d& p, const std::vector<Point2d>& polygon)
00113   {
00114     int wn = 0; // Winding number
00115     size_t n = polygon.size();
00116     double xp = p.x();
00117     double yp = p.y();
00118     for(size_t i = 0 ; i < n ; ++i)
00119     {
00120       const Point2d& w1 = polygon[i];
00121       const Point2d& w2 = polygon[(i+1)%n];
00122       double dx = w2.x() - w1.x();
00123       double dy = w2.y() - w1.y();
00124       if(dy == 0) // Ignore horizontal sides
00125         continue;
00126       double x_intersect = w1.x() + (yp-w1.y())*dx/dy;
00127       if(w1.y() < w2.y()) // Updward edge
00128       {
00129         if(w1.y() <= yp and  w2.y() > yp)
00130         {
00131           if(x_intersect > xp)
00132             ++wn;
00133         }
00134       }
00135       else if(w1.y() > yp and  w2.y() <= yp)
00136       {
00137         if(x_intersect > xp)
00138           --wn;
00139       }
00140     }
00141     return wn > 0;
00142   }
00143 
00144 #define SAME_SIGNS(n1,n2) (std::signbit(n1) == std::signbit(n2))
00145 
00146   bool segmentSegmentIntersection(Point2d& u,
00147                             const Point2d& p1, const Point2d& p2,
00148                             const Point2d& q1, const Point2d& q2)
00149   {
00150     /*
00151     double ratio = (p1.x() - p2.x())*(q1.y()-q2.y()) - (p1.y() - p2.y())*(q1.x()-q2.x());
00152     u.x() = ((p1.x()*p2.y()-p1.y()*p2.x())*(q1.x()-q2.x())-(p1.x()-p2.x())*(q1.x()*q2.y()-q1.y()*q2.x()))/ratio;
00153     u.y() = ((p1.x()*p2.y()-p1.y()*p2.x())*(q1.y()-q2.y())-(p1.y()-p2.y())*(q1.x()*q2.y()-q1.y()*q2.x()))/ratio;
00154     // Test if u is on the segment [p1,p2]
00155     if((u.x()-p1.x())*(u.x()-p2.x()) <= VVE_EPSILON and
00156        (u.y()-p1.y())*(u.y()-p2.y()) <= VVE_EPSILON and
00157        (u.x()-q1.x())*(u.x()-q2.x()) <= VVE_EPSILON and
00158        (u.y()-q1.y())*(u.y()-q2.y()) <= VVE_EPSILON)
00159       return true;
00160     return false;
00161     */
00162 
00163     //double x1lo, x1hi, y1lo, y1hi;
00164 
00165     Point2d A = p2-p1;
00166     Point2d B = q1-q2;
00167 
00168     Point2d low, high;
00169 
00170     if(A.x() < 0)
00171     { low.x() = p2.x(); high.x() = p1.x(); }
00172     else
00173     { low.x() = p1.x(); high.x() = p2.x(); }
00174     if(B.x() > 0)
00175     {
00176       if(high.x() < q2.x() or q1.x() < low.x())
00177         return false;
00178     }
00179     else if(high.x() < q1.x() or q2.x() < low.x())
00180       return false;
00181 
00182     if(A.y() < 0)
00183     { low.y() = p2.y(); high.y() = p1.y(); }
00184     else
00185     { low.y() = p1.y(); high.y() = p2.y(); }
00186 
00187     if(B.y() > 0)
00188     {
00189       if(high.y() < q2.y() or q1.y() < low.y())
00190         return false;
00191     }
00192     else if(high.y() < q1.y() or q2.y() < low.y())
00193       return false;
00194 
00195     Point2d C = p1-q1;
00196     double d = C^B;
00197     double f = B^A;
00198     if(f>0)
00199     {
00200       if(d<0 or d>f)
00201         return false;
00202     }
00203     else if(d>0 or d<f)
00204       return false;
00205 
00206     double e = A^C;
00207     if(f>0)
00208     {
00209       if(e<0 or e>f)
00210         return false;
00211     }
00212     else if(e>0 or e<f)
00213       return false;
00214 
00215     // Compute intersection coordinates
00216     if(f==0) return false;
00217     double num = d*A.x();
00218     double offset = 0;// SAME_SIGNS(num,f)? f/2 : -f/2;
00219     u.x() = p1.x() + (num+offset)/f;
00220 
00221     num = d*A.y();
00222     offset = 0; //SAME_SIGNS(num,f)? f/2 : -f/2;
00223     u.y() = p1.y() + (num+offset)/f;
00224 
00225     return true;
00226   }
00227 
00228   bool lineSegmentIntersection(Point2d& u,
00229                                const Point2d& p1, const Point2d& p2,
00230                                const Point2d& p, const Point2d& n)
00231   {
00232     Point2d dir(-n.y(), n.x()); // Vector along the line
00233     Point2d seg_dir = p2-p1;
00234     double det = dir^seg_dir;
00235     if(fabs(det) < VVE_REL_EPSILON*util::norm(seg_dir))
00236       return false; // Parallel lines
00237     Point2d p1p = p - p1;
00238     double lambda = (dir ^ p1p) / det;
00239     if(lambda >= -VVE_EPSILON and lambda <= 1+VVE_EPSILON)
00240     {
00241       u = p1 + lambda*seg_dir;
00242       return true;
00243     }
00244     return false;
00245   }
00246 
00247   bool lineLineIntersection(Point2d& u,
00248                             const Point2d& p1, const Point2d& u1,
00249                             const Point2d& p2, const Point2d& u2)
00250   {
00251     if(fabs(u1^u2)/(util::normsq(u1)*util::normsq(u2)) < VVE_EPSILON*VVE_EPSILON)
00252     {
00253       return false;
00254     }
00255     double t1 = (u2^(p1-p2)) / (u1^u2);
00256     u.x() = p1.x() + t1*u1.x();
00257     u.y() = p1.y() + t1*u1.y();
00258     return true;
00259   }
00260 
00261 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines
Generated on Fri May 31 15:37:50 2013 for VVE by  doxygen 1.6.3