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;
}
