// The function that returns true if line segment 'p1q1'
// and 'p2q2' intersect.
bool ObjectMatching::doIntersect(geometry_msgs::Point p1, geometry_msgs::Point q1,
geometry_msgs::Point p2, geometry_msgs::Point q2)
{
// Find the four orientations needed for general and
// special cases
int o1 = orientation(p1, q1, p2);
int o2 = orientation(p1, q1, q2);
int o3 = orientation(p2, q2, p1);
int o4 = orientation(p2, q2, q1);
// General case
if (o1 != o2 && o3 != o4)
return true;
// Special Cases
// p1, q1 and p2 are colinear and p2 lies on segment p1q1
if (o1 == 0 && onSegment(p1, p2, q1)) return true;
// p1, q1 and p2 are colinear and q2 lies on segment p1q1
if (o2 == 0 && onSegment(p1, q2, q1)) return true;
// p2, q2 and p1 are colinear and p1 lies on segment p2q2
if (o3 == 0 && onSegment(p2, p1, q2)) return true;
// p2, q2 and q1 are colinear and q1 lies on segment p2q2
if (o4 == 0 && onSegment(p2, q1, q2)) return true;
return false; // Doesn't fall in any of the above cases
}
// Check if two lines are intersected
bool intersectLines(Vec p1, Vec p2, Vec p3, Vec p4)
{
// Relative orientation
double d1 = direction(p3, p4, p1);
double d2 = direction(p3, p4, p2);
double d3 = direction(p1, p2, p3);
double d4 = direction(p1, p2, p4);
// If (p1, p2) (p3, p4) straddle each other, the line segments must
// intersect
if (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) &&
((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) {
return true;
}
if (d1 == 0 && onSegment(p3, p4, p1)) {
return true;
}
if (d2 == 0 && onSegment(p3, p4, p2)) {
return true;
}
if (d3 == 0 && onSegment(p1, p2, p3)) {
return true;
}
if (d4 == 0 && onSegment(p1, p2, p4)) {
return true;
}
return false;
}
//This is a method from CLRS
//There was a bug in the original version of this, I replaced all of if(d1==0) with if( eq(d1,0) ) and so on.
//This bug took 4 hours to fix.
bool intersection(Point* p1, Point* p2, Point* p3, Point* p4){
double d1 = direction(p3,p4,p1);
double d2 = direction(p3,p4,p2);
double d3 = direction(p1,p2,p3);
double d4 = direction(p1,p2,p4);
if(
(((d1>0)&&(d2<0)) || ((d1<0)&&(d2>0))) &&
(((d3>0)&&(d4<0)) || ((d3<0)&&(d4>0)))
){
return true;
}else if(eq(d1,0) && onSegment(p3, p4, p1)){
return true;
}else if(eq(d2,0) && onSegment(p3, p4, p2)){
return true;
}else if(eq(d3,0) && onSegment(p1, p2, p3)){
return true;
}else if(eq(d4,0) && onSegment(p1, p2, p4)){
return true;
}else{
return false;
}
}