int intersectLineSegment(Point2f a, Point2f b, Point2f c, double r, Point2f& p1, Point2f& p2) { int n = intersectLineCircle(a, b, c, r, p1, p2); if (n == 0) { return 0; } std::cout << "DEBUG: " << p1.x << " " << p1.y << " " << p2.x << " " << p2.y << std::endl; Point2f p1a, p2a; n = 0; if (inSegment(a, b, p1)) { p1a = p1; n++; std::cout << "p1 drin" << std::endl; } if (inSegment(a, b, p2)) { if (n == 0) { p1a = p2; } p2a = p2; n++; std::cout << "p2 drin" << std::endl; } p1 = p1a; p2 = p2a; return n; }
float EKFLocalization::getDistance(wmLine* wmline, Vector2<double> ps3D, Vector2<double> pe3D) { double lenght = sqrt(((ps3D.x-pe3D.x)*(ps3D.x-pe3D.x)) + ((ps3D.y-pe3D.y)*(ps3D.y-pe3D.y))); int divs = 10; Vector2<double> vd, vod; vd.x = (ps3D.x-pe3D.x)/(double)divs; vd.y = (ps3D.y-pe3D.y)/(double)divs; vod.x = -vd.y; vod.y = vd.x; float min = 10000.0; Vector2<double> pw1, pw2; pw1.x = _WorldModel->getPoint(wmline->p1)->p.X; pw1.y = _WorldModel->getPoint(wmline->p1)->p.Y; pw2.x = _WorldModel->getPoint(wmline->p2)->p.X; pw2.y = _WorldModel->getPoint(wmline->p2)->p.Y; float aux; aux = sqrt((pw1.x-ps3D.x)*(pw1.x-ps3D.x)+(pw1.y-ps3D.y)*(pw1.y-ps3D.y)); if(aux<min) min=aux; aux = sqrt((pw2.x-ps3D.x)*(pw2.x-ps3D.x)+(pw2.y-ps3D.y)*(pw2.y-ps3D.y)); if(aux<min) min=aux; aux = sqrt((pw1.x-pe3D.x)*(pw1.x-pe3D.x)+(pw1.y-pe3D.y)*(pw1.y-pe3D.y)); if(aux<min) min=aux; aux = sqrt((pw2.x-pe3D.x)*(pw2.x-pe3D.x)+(pw2.y-pe3D.y)*(pw2.y-pe3D.y)); if(aux<min) min=aux; float s = 0.0; for(int i=0; i<divs; i++) { float dist; Vector2<double> pd; // s=s+(lenght/(float)divs); pd.x = pe3D.x + (double)i * vd.x; pd.y = pe3D.y + (double)i * vd.y; // double A,B,C; getEqLine(pd, vod, A, B, C); Vector2<double> iwm; if(getInterseccLines(A, B, C, wmline->A, wmline->B, wmline->C, iwm)) { //cerr<<"\t("<<iwm.x<<","<<iwm.y<<") -> ("<<pd.x<<","<<pd.y<<") ["<<iwm.x<<","<<iwm.y<<"] "; if((fabs(iwm.x)>10000.0) || (fabs(iwm.y)>10000.0)) continue; //cerr<<" = "; //cerr<<"i = ("<<iwm.x<<","<<iwm.y<<")"<<endl; HPoint3D ps3DH, pe3DH; ps3DH.X = ps3D.x; ps3DH.Y = ps3D.y; pe3DH.X = pe3D.x; pe3DH.Y = pe3D.y; if(inSegment(_WorldModel->getPoint(wmline->p1)->p, _WorldModel->getPoint(wmline->p2)->p, iwm)) { dist = getDistance(iwm, pd); //cerr<<dist<<endl; } else continue; if((dist<min) && (dist>1.0)) min=dist; } } //cerr<<"("<<_WorldModel->getPoint(wmline->p1)->p.X<<","<<_WorldModel->getPoint(wmline->p1)->p.Y<<") -> "; //cerr<<"("<<_WorldModel->getPoint(wmline->p2)->p.X<<","<<_WorldModel->getPoint(wmline->p2)->p.Y<<") = "<<min<<endl; return min; }
int intersect2D_Segments( const Vector2d &p1, const Vector2d &p2, const Vector2d &p3, const Vector2d &p4, Vector2d &I0, Vector2d &I1, double maxerr) { Vector2d u = p2 - p1; Vector2d v = p4 - p3; Vector2d w = p1 - p3; double D = perp(u,v); double t0, t1; // Temp vars for parametric checks // test if they are parallel (includes either being a point) if (abs(D) < maxerr) { // S1 and S2 are parallel if (perp(u,w) != 0 || perp(v,w) != 0) { return 0; // they are NOT collinear } // they are collinear or degenerate // check if they are degenerate points double du = u.dot(u); double dv = v.dot(v); if (du==0 && dv==0) { // both segments are points if (p1 != p3) // they are distinct points return 0; I0 = p1; // they are the same point return 1; } if (du==0) { // S1 is a single point if (!inSegment(p1, p3, p4)) // but is not in S2 return 0; I0 = p1; return 1; } if (dv==0) { // S2 a single point if (!inSegment(p3, p1,p2)) // but is not in S1 return 0; I0 = p3; return 1; } // they are collinear segments - get overlap (or not) Vector2d w2 = p2 - p3; if (v.x() != 0) { t0 = w.x() / v.x(); t1 = w2.x() / v.x(); } else { t0 = w.y() / v.y(); t1 = w2.y() / v.y(); } if (t0 > t1) { // must have t0 smaller than t1 double t=t0; t0=t1; t1=t; // swap if not } if (t0 > 1 || t1 < 0) { return 0; // NO overlap } t0 = t0<0? 0 : t0; // clip to min 0 t1 = t1>1? 1 : t1; // clip to max 1 if (t0 == t1) { // intersect is a point I0 = p3 + v*t0; return 1; } // they overlap in a valid subsegment I0 = p3 + v*t0; I1 = p3 + v*t1; return 2; } // end parallel bool outside = false; // the segments are skew and may intersect in a point // get the intersect parameter for S1 t0 = perp(v,w) / D; if (t0 < 0 || t0 > 1) // no intersect in S1 outside = true; //return 0; // get the intersect parameter for S2 t1 = perp(u,w) / D; if (t1 < 0 || t1 > 1) // no intersect in S2 outside = true; // return 0; I0 = p1 + u * t0; // compute S1 intersect point if (outside) return 3; return 1; }
// Following function licensed as: // Copyright 2001, softSurfer (www.softsurfer.com) // This code may be freely used and modified for any purpose // providing that this copyright notice is included with it. // SoftSurfer makes no warranty for this code, and cannot be held // liable for any real or imagined damage resulting from its use. // Users of this code must verify correctness for their application. // // intersect2D_2Segments(): the intersection of 2 finite 2D segments // Input: two finite segments S1 and S2 // Output: *I0 = intersect point (when it exists) // *I1 = endpoint of intersect segment [I0,I1] (when it exists) // Return: 0=disjoint (no intersect) // 1=intersect in unique point I0 // 2=overlap in segment from I0 to I1 int intersect2D_Segments( const Vector2d &p1, const Vector2d &p2, const Vector2d &p3, const Vector2d &p4, Vector2d &I0, Vector2d &I1, double &t0, double &t1 ) { Vector2d u = p2 - p1; Vector2d v = p4 - p3; Vector2d w = p1 - p3; double D = perp(u,v); // test if they are parallel (includes either being a point) if (fabs(D) < SMALL_NUM) { // S1 and S2 are parallel if (perp(u,w) != 0 || perp(v,w) != 0) { return 0; // they are NOT collinear } // they are collinear or degenerate // check if they are degenerate points double du = dot(u,u); double dv = dot(v,v); if (du==0 && dv==0) { // both segments are points if (p1 != p3) // they are distinct points return 0; I0 = p1; // they are the same point return 1; } if (du==0) { // S1 is a single point if (inSegment(p1, p3, p4) == 0) // but is not in S2 return 0; I0 = p1; return 1; } if (dv==0) { // S2 a single point if (inSegment(p3, p1,p2) == 0) // but is not in S1 return 0; I0 = p3; return 1; } // they are collinear segments - get overlap (or not) //double t0, t1; // endpoints of S1 in eqn for S2 Vector2d w2 = p2 - p3; if (v.x != 0) { t0 = w.x / v.x; t1 = w2.x / v.x; } else { t0 = w.y / v.y; t1 = w2.y / v.y; } if (t0 > t1) { // must have t0 smaller than t1 double t=t0; t0=t1; t1=t; // swap if not } if (t0 > 1 || t1 < 0) { return 0; // NO overlap } t0 = t0<0? 0 : t0; // clip to min 0 t1 = t1>1? 1 : t1; // clip to max 1 if (t0 == t1) { // intersect is a point I0 = p3 + v*t0; return 1; } // they overlap in a valid subsegment I0 = p3 + v*t0; I1 = p3 + v*t1; return 2; } // the segments are skew and may intersect in a point // get the intersect parameter for S1 t0 = perp(v,w) / D; if (t0 < 0 || t0 > 1) // no intersect with S1 return 0; // get the intersect parameter for S2 t1 = perp(u,w) / D; if (t1 < 0 || t1 > 1) // no intersect with S2 return 0; I0 = p1 + u * t0; // compute S1 intersect point return 1; }