Ramer-Douglas-Peucker path simplification algorithm

I implemented a path simplification algorithm after reading the article here:

http://losingfight.com/blog/2011/05/30/how-to-implement-a-vector-brush/

It's worked for me pretty well for generating optimized level geometry for my game. But, I'm using it now to clean up a* pathfinding paths and it's got a weird edge case that fails miserably.

Here's a screenshot of it working - optimizing the path from red circle to the blue circle. The faint green line is the a* output, and the lighter whiteish line is the optimized path.

And here's a screenshot of it failing:

Here's my code. I adapted the ObjC code from the article to c++

Note: vec2fvec is a std::vector< vec2<float> >, and 'real' is just a typedef'd float.

       void rdpSimplify( const vec2fvec &in, vec2fvec &out, real threshold )
{
    if ( in.size() <= 2 )
    {
        out = in;
        return;
    }

    //
    //  Find the vertex farthest from the line defined by the start and and of the path
    //

    real maxDist = 0;
    size_t maxDistIndex = 0;      
    LineSegment line( in.front(), in.back() );

    for ( vec2fvec::const_iterator it(in.begin()),end(in.end()); it != end; ++it )
    {
        real dist = line.distance( *it );
        if ( dist > maxDist )
        {
            maxDist = dist;
            maxDistIndex = it - in.begin();
        }
    }

    //
    //  If the farhtest vertex is greater than our threshold, we need to
    //  partition and optimize left and right separately
    //

    if ( maxDist > threshold )
    {
        //
        //  Partition 'in' into left and right subvectors, and optimize them
        //

        vec2fvec left( maxDistIndex+1 ),
                 right( in.size() - maxDistIndex ),
                 leftSimplified,
                 rightSimplified;

        std::copy( in.begin(), in.begin() + maxDistIndex + 1, left.begin() );
        std::copy( in.begin() + maxDistIndex, in.end(), right.begin() );

        rdpSimplify(left, leftSimplified, threshold );
        rdpSimplify(right, rightSimplified, threshold );

        //
        //  Stitch optimized left and right into 'out'
        //

        out.resize( leftSimplified.size() + rightSimplified.size() - 1 );
        std::copy( leftSimplified.begin(), leftSimplified.end(), out.beg开发者_如何转开发in());
        std::copy( rightSimplified.begin() + 1, rightSimplified.end(), out.begin() + leftSimplified.size() );
    }
    else
    {
        out.push_back( line.a );
        out.push_back( line.b );
    }
}

I'm really at a loss as to what's going wrong. My spidey sense says it's in the std::copy calls... I must be copying garbage in some circumstances.

EDIT: I've rewritten the algorithm dropping any use of iterators and std::copy, and the like. It still fails in the exact same way.

       void rdpSimplify( const vec2fvec &in, vec2fvec &out, real threshold )
{
    if ( in.size() <= 2 )
    {
        out = in;
        return;
    }

    //
    //  Find the vertex farthest from the line defined by the start and and of the path
    //

    real maxDist = 0;
    size_t maxDistIndex = 0;      
    LineSegment line( in.front(), in.back() );

    for ( size_t i = 0, N = in.size(); i < N; i++ )
    {
        real dist = line.distance( in[i] );
        if ( dist > maxDist )
        {
            maxDist = dist;
            maxDistIndex = i;
        }
    }


    //
    //  If the farthest vertex is greater than our threshold, we need to
    //  partition and optimize left and right separately
    //

    if ( maxDist > threshold )
    {
        //
        //  Partition 'in' into left and right subvectors, and optimize them
        //


        vec2fvec left, right, leftSimplified, rightSimplified;
        for ( size_t i = 0; i < maxDistIndex + 1; i++ ) left.push_back( in[i] );
        for ( size_t i = maxDistIndex; i < in.size(); i++ ) right.push_back( in[i] );


        rdpSimplify(left, leftSimplified, threshold );
        rdpSimplify(right, rightSimplified, threshold );

        //
        //  Stitch optimized left and right into 'out'
        //

        out.clear();
        for ( size_t i = 0, N = leftSimplified.size(); i < N; i++ ) out.push_back(leftSimplified[i]);
        for ( size_t i = 1, N = rightSimplified.size(); i < N; i++ ) out.push_back( rightSimplified[i] );
    }
    else
    {
        out.push_back( line.a );
        out.push_back( line.b );
    }
}

I can't find any faults in your code.

Some things to try:

• Add some debug print statements to check what maxDist is in the failing case. It should be really low, but if it comes out high then you know there's a problem with your line segment distance code.
• Check that the path you are seeing actually matches the path that your algorithm returns. If not then perhaps there is something wrong with your path rendering? Maybe a bug when the path only has two points?
• Check that your input path is what you expect it to be by printing out all its coordinates at the start of the algorithm.

It shouldn't take too long to find the cause of the problem if you just investigate a little. After a few minutes, staring at code is a very poor way to debug.