/**
Sets aa and bb points of the segment.
*/
int Cline::set_line(const Cpoint &paa, const Cpoint &pbb)
{
aa.set_point(paa.get_xx(),paa.get_yy());
bb.set_point(pbb.get_xx(),pbb.get_yy());
length=aa.d2point(&bb);
set_vv();
return 0;
}
Cline::Cline(const Cpoint &paa, const Cpoint &pbb, const float &hght, const bool & inOut)
{
aa.set_point(paa.get_xx(),paa.get_yy());
bb.set_point(pbb.get_xx(),pbb.get_yy());
length=aa.d2point(&bb);
set_vv();
height=hght;
inOutDoor=inOut;
description[0]='\0';
}
/**
Segment Constructor with aa and bb coordinates
*/
Cline::Cline(const Cpoint &paa, const Cpoint &pbb)
{
aa.set_point(paa.get_xx(),paa.get_yy());
bb.set_point(pbb.get_xx(),pbb.get_yy());
length=aa.d2point(&bb);
set_vv();
height=0;
inOutDoor=0;
description[0]='\0';
}
/**
Returns the distance from this segment to point qq. Evaluate first if distance is from one of the limiting points (aa or bb). If not, evaluate distance from the segment. Sets to *iPoint the closest point of the segment to qq.
*/
int Cline::d2point(const Cpoint & qq, Cpoint & iPoint, float & dist) const
{
//float t1, iPx, iPy;
float dotProd;
//cout << endl << endl << "************* d2point *************" << endl;
//cout << "qq=" << qq << endl;
//ww is the two coordinate vector going from bb to qq
Cpoint ww((qq.get_xx()-bb.get_xx()),(qq.get_yy()-bb.get_yy()));
//check if BB is the closest point
dotProd = vv.scalar(ww);
if (dotProd >= 0)
{
iPoint=bb;//bb is the closest point to qq
dist = bb.d2point(qq);
return iBB;
}
//ww is the two coordinate vector going from aa to qq
ww.set_point((qq.get_xx()-aa.get_xx()),(qq.get_yy()-aa.get_yy()));
//check if AA is the closest point
dotProd = vv.scalar(ww);
if (dotProd <= 0)
{
iPoint = aa; //aa is the closest point to qq
dist = aa.d2point(qq);
return iAA;
}
//point between AA and BB
//compute projection of ww onto vv
dotProd = dotProd/(length*length);
iPoint.set_point(aa.get_xx()+dotProd*vv.get_xx(), aa.get_yy()+dotProd*vv.get_yy());
/*
cout << "iPoint=" << iPoint << endl;
Cline auxLine1(aa, iPoint);
Cline auxLine2(iPoint, bb);
cout << "SEGMENT!" << endl;
printLine();
cout << "\tdir1=" << getDirection() << endl;
cout << "auxLine1(aa, iPoint)=" << endl;
auxLine1.printLine();
cout << "\tdir1=" << auxLine1.getDirection() << endl;
cout << "auxLine2(iPoint, bb)=" << endl;
auxLine2.printLine();
cout << "\tdir2=" << auxLine2.getDirection() << endl;
*/
dist = iPoint.d2point(qq);
return iAB;
}
unsigned short int Cline::intersectionsWithCircle(Cpoint c, double r, vector<Cpoint> &intersections)
{
intersections.clear();
// move circle to (0,0)
//Cpoint p1 = aa - c; cout << "DEBUG: p1 is " << p1 << endl;
//Cpoint p2 = bb - c; cout << "DEBUG: p2 is " << p2 << endl;
Cpoint p1(aa.get_xx() - c.get_xx(), aa.get_yy() - c.get_yy()); //cout << "DEBUG: p1 is " << p1 << endl;
Cpoint p2(bb.get_xx() - c.get_xx(), bb.get_yy() - c.get_yy()); //cout << "DEBUG: p1 is " << p1 << endl;
// discriminant
double dx = p2.get_xx() - p1.get_xx();
double dy = p2.get_yy() - p1.get_yy();
double dr_sq = dx*dx + dy*dy;
double D = p1.get_xx() * p2.get_yy() - p2.get_xx() * p1.get_yy();
double disc = r*r * dr_sq - D*D;
if(disc < 0)
return 0;
else if(disc == 0)
{
double x = D * dy / dr_sq;
double y = -D * dx / dr_sq;
// check if point belongs to segment
Cpoint i = Cpoint(x + c.get_xx(), y + c.get_yy());
if( i.d2point2(&aa) <= aa.d2point2(&bb) && i.d2point2(&bb) <= aa.d2point2(&bb))
intersections.push_back( i );
return intersections.size();
}
else
{
int sign_dy = (dy >= 0) ? 1 : -1;
double x1 = ( D * dy + sign_dy * dx * sqrt(disc) ) / dr_sq;
double x2 = ( D * dy - sign_dy * dx * sqrt(disc) ) / dr_sq;
double abs_dy = ( dy >= 0 ) ? dy : -dy;
double y1 = ( -D * dx + abs_dy * sqrt(disc) ) / dr_sq;
double y2 = ( -D * dx - abs_dy * sqrt(disc) ) / dr_sq;
Cpoint i1 = Cpoint(x1 + c.get_xx(), y1 + c.get_yy());
Cpoint i2 = Cpoint(x2 + c.get_xx(), y2 + c.get_yy());
if( i1.d2point2(&aa) <= aa.d2point2(&bb) && i1.d2point2(&bb) <= aa.d2point2(&bb))
intersections.push_back( i1 );
if( i2.d2point2(&aa) <= aa.d2point2(&bb) && i2.d2point2(&bb) <= aa.d2point2(&bb))
intersections.push_back( i2 );
return intersections.size();
}
}
