void Sonar2DUtils::DouglasPeucker(const Sonar2DUtils::tBottomLine& bottomLineFt, float epsilon, Sonar2DUtils::tBottomLine& returnResults)
{
const unsigned int minPoints = 3;
if(bottomLineFt.size() < minPoints)
{
returnResults = bottomLineFt;
}
else
{
Sonar2DUtils::tBottomLine lineSegment;
lineSegment.push_back( *bottomLineFt.begin() );
lineSegment.push_back( bottomLineFt.back() );
float dMax = 0.0f;
Sonar2DUtils::tBottomLine::const_iterator iterMax;
Sonar2DUtils::tBottomLine::const_iterator iterEnd = bottomLineFt.begin() + (bottomLineFt.size() - 2);
// Find the point with maximum distance, excluding the first and last point of the segment.
for(Sonar2DUtils::tBottomLine::const_iterator iter=bottomLineFt.begin()+1; iter!=iterEnd; ++iter )
{
float d = ShortestDistanceToSegment( *iter, lineSegment );
if( d > dMax )
{
dMax = d;
iterMax = iter;
}
}
// If max distance is greater than epsilon, recursively simplify
if( dMax > epsilon)
{
// Split up new lines
Sonar2DUtils::tBottomLine line1( bottomLineFt.begin(), (iterMax+1) );
Sonar2DUtils::tBottomLine line2( iterMax, bottomLineFt.end() );
Sonar2DUtils::tBottomLine results1;
DouglasPeucker(line1, epsilon, results1 );
Sonar2DUtils::tBottomLine results2;
DouglasPeucker(line2, epsilon, results2 );
if(results1.size() < minPoints)
{
returnResults.push_back( *results1.begin() );
}
else
{
returnResults = Sonar2DUtils::tBottomLine( results1.begin(), (results1.begin() + results1.size() - 2) );
}
returnResults.insert( returnResults.end(), results2.begin(), results2.end() );
}
else
{
returnResults.push_back( *bottomLineFt.begin() );
returnResults.push_back( bottomLineFt.back() );
}
}
}
void CCampathDrawer::RamerDouglasPeucker(TempPoint * start, TempPoint * end, double epsilon)
{
double dmax = 0;
TempPoint * index = start;
for(TempPoint * i = start->nextPt; i && i != end; i = i->nextPt)
{
double d = ShortestDistanceToSegment(i, start, end);
if(d > dmax)
{
index = i;
dmax = d;
}
}
// If max distance is greater than epsilon, recursively simplify
if ( dmax > epsilon )
{
RamerDouglasPeucker(start, index, epsilon);
RamerDouglasPeucker(index, end, epsilon);
} else {
start->nextPt = end;
}
}
