/** Only if the point lies 'in front of' the segment! Returns -1.0 otherwise.
**/
PointCoordinateType ComputeSquareDistToEdge(const CCVector2& P, const CCVector2& A, const CCVector2& B)
{
CCVector2 AP = P-A;
CCVector2 AB = B-A;
PointCoordinateType dot = AB.dot(AP); // = cos(PAB) * ||AP|| * ||AB||
if (dot < 0)
{
//return AP.norm2();
return -1.0;
}
else
{
PointCoordinateType squareLengthAB = AB.norm2();
if (dot > squareLengthAB)
{
//return (P-B).norm2();
return -1.0;
}
else
{
CCVector2 HP = AP - AB * (dot / squareLengthAB);
return HP.norm2();
}
}
}
/** \return The nearest point distance (or -1 if no point was found!)
**/
static PointCoordinateType FindNearestCandidate( unsigned& minIndex,
const VertexIterator& itA,
const VertexIterator& itB,
const std::vector<Vertex2D>& points,
const std::vector<HullPointFlags>& pointFlags,
PointCoordinateType minSquareEdgeLength,
PointCoordinateType maxSquareEdgeLength,
bool allowLongerChunks = false)
{
//look for the nearest point in the input set
PointCoordinateType minDist2 = -1;
CCVector2 AB = **itB - **itA;
PointCoordinateType squareLengthAB = AB.norm2();
unsigned pointCount = static_cast<unsigned>(points.size());
for (unsigned i = 0; i < pointCount; ++i)
{
const Vertex2D& P = points[i];
if (pointFlags[P.index] != POINT_NOT_USED)
continue;
//skip the edge vertices!
if (P.index == (*itA)->index || P.index == (*itB)->index)
continue;
//we only consider 'inner' points
CCVector2 AP = P - **itA;
if (AB.x * AP.y - AB.y * AP.x < 0)
{
continue;
}
PointCoordinateType dot = AB.dot(AP); // = cos(PAB) * ||AP|| * ||AB||
if (dot >= 0 && dot <= squareLengthAB)
{
CCVector2 HP = AP - AB * (dot / squareLengthAB);
PointCoordinateType dist2 = HP.norm2();
if (minDist2 < 0 || dist2 < minDist2)
{
//the 'nearest' point must also be a valid candidate
//(i.e. at least one of the created edges is smaller than the original one
//and we don't create too small edges!)
PointCoordinateType squareLengthAP = AP.norm2();
PointCoordinateType squareLengthBP = (P - **itB).norm2();
if ( squareLengthAP >= minSquareEdgeLength
&& squareLengthBP >= minSquareEdgeLength
&& (allowLongerChunks || (squareLengthAP < squareLengthAB || squareLengthBP < squareLengthAB))
)
{
minDist2 = dist2;
minIndex = i;
}
}
}
}
return (minDist2 < 0 ? minDist2 : minDist2/squareLengthAB);
}
//! Returns true if the AB and CD segments intersect each other
bool Intersect(const CCVector2& A, const CCVector2& B, const CCVector2& C, const CCVector2& D)
{
if (cross(A,B,C) * cross(A,B,D) >= 0) //both points are on the same side? No intersection
return false;
CCVector2 AB = B-A;
CCVector2 AC = C-A;
CCVector2 CD = D-C;
PointCoordinateType q = CD.y * AB.x - CD.x * AB.y;
if (fabs(q) > ZERO_TOLERANCE) //AB and CD are not parallel
{
//where do they interest?
PointCoordinateType p = AC.x * AB.y - AC.y * AB.x;
PointCoordinateType v = p/q;
if (v <= 0 || v >= 1) //not CD!
return false;
//test further with AB
p = CD.y * AC.x - CD.x * AC.y;
v= p/q;
return (v > 0 && v < 1); //not AB?
}
else //AB and CD are parallel
{
PointCoordinateType dAB = AB.norm();
PointCoordinateType dAC = AC.norm();
PointCoordinateType dot = AB.dot(AC) / (dAB * dAC);
if (fabs(dot) < 0.999)
{
//not colinear
return false;
}
else
{
//colinear --> do they actually intersect?
return (dAC < dAB || (D-A).norm() < dAB);
}
}
}
bool PointProjectionTools::extractConcaveHull2D(std::vector<IndexedCCVector2>& points,
std::list<IndexedCCVector2*>& hullPoints,
PointCoordinateType maxSquareEdgeLength/*=0*/)
{
//first compute the Convex hull
if (!extractConvexHull2D(points,hullPoints))
return false;
//shall we compute the concave hull?
if (hullPoints.size() >= 2 && maxSquareEdgeLength > 0)
{
size_t pointCount = points.size();
std::vector<HullPointFlags> pointFlags;
try
{
pointFlags.resize(pointCount,POINT_NOT_USED);
}
catch(...)
{
//not enough memory
return false;
}
//hack: compute the 'minimal' edge length
PointCoordinateType minSquareEdgeLength = 0;
{
CCVector2 minP,maxP;
for (size_t i=0; i<pointCount; ++i)
{
const IndexedCCVector2& P = points[i];
if (i)
{
minP.x = std::min(P.x,minP.x);
minP.y = std::min(P.y,minP.y);
maxP.x = std::max(P.x,maxP.x);
maxP.y = std::max(P.y,maxP.y);
}
else
{
minP = maxP = P;
}
}
minSquareEdgeLength = (maxP-minP).norm2() / static_cast<PointCoordinateType>(1.0e7); //10^-7 of the max bounding rectangle side
minSquareEdgeLength = std::min(minSquareEdgeLength, maxSquareEdgeLength/10);
//we remove very small edges
for (std::list<IndexedCCVector2*>::iterator itA = hullPoints.begin(); itA != hullPoints.end(); ++itA)
{
std::list<IndexedCCVector2*>::iterator itB = itA; ++itB;
if (itB == hullPoints.end())
itB = hullPoints.begin();
if ((**itB-**itA).norm2() < minSquareEdgeLength)
{
pointFlags[(*itB)->index] = POINT_IGNORED;
hullPoints.erase(itB);
}
}
if (hullPoints.size() < 2)
{
//no more edges?!
return false;
}
}
//build the initial edge list & flag the convex hull points
std::list<std::list<PointProjectionTools::IndexedCCVector2*>::iterator> edges;
{
for (std::list<PointProjectionTools::IndexedCCVector2*>::iterator itA = hullPoints.begin(); itA != hullPoints.end(); ++itA)
{
try
{
edges.push_back(itA);
}
catch(...)
{
//not enough memory
return false;
}
pointFlags[(*itA)->index] = POINT_USED;
}
}
//we look for edges that are longer than the maximum specified length
while (!edges.empty())
{
//current edge (AB)
std::list<PointProjectionTools::IndexedCCVector2*>::iterator itA = edges.front();
std::list<PointProjectionTools::IndexedCCVector2*>::iterator itB = itA; ++itB;
if (itB == hullPoints.end())
itB = hullPoints.begin();
edges.pop_front();
//long edge?
PointCoordinateType squareLengthAB = (**itB-**itA).norm2();
if (squareLengthAB > maxSquareEdgeLength)
{
//look for the nearest point in the input set
PointCoordinateType minDist2 = -1;
size_t minIndex = 0;
for (size_t i=0; i<pointCount; ++i)
{
const PointProjectionTools::IndexedCCVector2& P = points[i];
if (pointFlags[P.index] == POINT_IGNORED)
continue;
//skip the edge vertices!
if (P.index == (*itA)->index || P.index == (*itB)->index)
continue;
//we only consider 'inner' points
if (cross(**itA, **itB, P) < 0)
{
continue;
}
PointCoordinateType dist2 = ComputeSquareDistToEdge(P,**itA,**itB);
if (dist2 >= 0 && (minDist2 < 0 || dist2 < minDist2))
{
minDist2 = dist2;
minIndex = i;
}
}
//if we have found a candidate
if (minDist2 >= 0)
{
const PointProjectionTools::IndexedCCVector2& P = points[minIndex];
if (pointFlags[P.index] == POINT_NOT_USED) //we don't consider already used points!
{
CCVector2 AP = (P-**itA);
CCVector2 PB = (**itB-P);
PointCoordinateType squareLengthAP = AP.norm2();
PointCoordinateType squareLengthPB = PB.norm2();
//check that we don't create too small edges!
if (squareLengthAP < minSquareEdgeLength || squareLengthPB < minSquareEdgeLength)
{
pointFlags[P.index] = POINT_IGNORED;
edges.push_front(itA); //retest the edge!
}
//at least one of the new segments must be smaller than the initial one!
else if ( squareLengthAP < squareLengthAB || squareLengthPB < squareLengthAB )
{
//now check that the point is not nearer to the neighbor edges
//DGM: only if the edge could 'need' it!
//next edge vertex (BC)
std::list<PointProjectionTools::IndexedCCVector2*>::iterator itC = itB; ++itC;
if (itC == hullPoints.end())
itC = hullPoints.begin();
PointCoordinateType dist2ToRight = -1;
CCVector2 BC = (**itC-**itB);
PointCoordinateType squareLengthBC = BC.norm2();
if (squareLengthBC > maxSquareEdgeLength)
dist2ToRight = ComputeSquareDistToEdge(P,**itB,**itC);
if (dist2ToRight < 0 || minDist2 <= dist2ToRight)
{
//previous edge vertex (OA)
std::list<PointProjectionTools::IndexedCCVector2*>::iterator itO = itA;
if (itO == hullPoints.begin())
itO = hullPoints.end();
--itO;
PointCoordinateType dist2ToLeft = -1;
CCVector2 OA = (**itA-**itO);
PointCoordinateType squareLengthOA = OA.norm2();
if (squareLengthOA > maxSquareEdgeLength)
dist2ToLeft = ComputeSquareDistToEdge(P,**itO,**itA);
if (dist2ToLeft < 0 || minDist2 <= dist2ToLeft)
{
//last check: the new segments must not intersect with the actual hull!
bool intersect = false;
{
for (std::list<IndexedCCVector2*>::iterator itI = hullPoints.begin(); itI != hullPoints.end(); ++itI)
{
std::list<IndexedCCVector2*>::iterator itJ = itI; ++itJ;
if (itJ == hullPoints.end())
itJ = hullPoints.begin();
//we avoid testing with already connected segments!
if ( (*itI)->index == (*itA)->index
|| (*itJ)->index == (*itA)->index
|| (*itI)->index == (*itB)->index
|| (*itJ)->index == (*itB)->index )
continue;
if (Intersect(**itI,**itJ,**itA,P) || Intersect(**itI,**itJ,P,**itB))
{
intersect = true;
break;
}
}
}
if (!intersect)
{
hullPoints.insert(itB == hullPoints.begin() ? hullPoints.end() : itB, &points[minIndex]);
//we'll inspect the two new segments later
try
{
if (squareLengthAP > maxSquareEdgeLength)
{
edges.push_back(itA);
}
if (squareLengthPB > maxSquareEdgeLength)
{
std::list<PointProjectionTools::IndexedCCVector2*>::iterator itP = itA; ++itP;
edges.push_back(itP);
}
}
catch(...)
{
//not enough memory
return false;
}
//we won't use P anymore!
pointFlags[P.index] = POINT_USED;
}
}
}
}
}
} //end of candidate examination
} //end of current edge examination
} //no more edges
}
return true;
}
bool PointProjectionTools::segmentIntersect(const CCVector2& A, const CCVector2& B, const CCVector2& C, const CCVector2& D)
{
CCVector2 AB = B-A;
CCVector2 AC = C-A;
CCVector2 AD = D-A;
PointCoordinateType cross_AB_AC = AB.cross(AC);
PointCoordinateType cross_AB_AD = AB.cross(AD);
//both C and D are on the same side of AB?
if (cross_AB_AC * cross_AB_AD > 0)
{
//no intersection
return false;
}
CCVector2 CD = D-C;
CCVector2 CB = B-C;
PointCoordinateType cross_CD_CA = -CD.cross(AC);
PointCoordinateType cross_CD_CB = CD.cross(CB);
//both A and B are on the same side of CD?
if (cross_CD_CA * cross_CD_CB > 0)
{
//no intersection
return false;
}
PointCoordinateType cross_AB_CD = AB.cross(CD);
if (fabs(cross_AB_CD) != 0) //AB and CD are not parallel
{
//where do they intersect?
//PointCoordinateType v = cross_AB_AC/cross_AB_CD;
//assert(v >= 0 && v <= 1);
return true;
}
else //AB and CD are parallel (therefore they are colinear - see above tests)
{
PointCoordinateType dAB = AB.norm();
PointCoordinateType dot_AB_AC = AB.dot(AC);
if (dot_AB_AC >= 0 && dot_AB_AC < dAB * AC.norm())
{
//C is between A and B
return true;
}
PointCoordinateType dot_AB_AD = AB.dot(AD);
if (dot_AB_AD >= 0 && dot_AB_AD < dAB * AD.norm())
{
//D is between A and B
return true;
}
//otherwise there's an intersection only if B and C are on both sides!
return (dot_AB_AC * dot_AB_AD < 0);
}
}
