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
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
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
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
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)
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)
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;
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)
00125 continue;
00126 double x_intersect = w1.x() + (yp-w1.y())*dx/dy;
00127 if(w1.y() < w2.y())
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
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
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
00216 if(f==0) return false;
00217 double num = d*A.x();
00218 double offset = 0;
00219 u.x() = p1.x() + (num+offset)/f;
00220
00221 num = d*A.y();
00222 offset = 0;
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());
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;
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 }