smoothers/SM_PathSmoothingSuccessiveNodePair.cpp

Go to the documentation of this file.

#include <assert.h>
#include "CollisionDetectors\CD_BasicStyle.h"
#include "SM_PathSmoothingSuccessiveNodePair.h"

//=============================================================================
// Constructor
//
// Description: constructor
//=============================================================================
SM_PathSmoothingSuccessiveNodePair::SM_PathSmoothingSuccessiveNodePair()
        :
        SmootherBase()
{
}

//=============================================================================
// Destructor
//
// Description: destructor
//=============================================================================
SM_PathSmoothingSuccessiveNodePair::~SM_PathSmoothingSuccessiveNodePair()
{
}

//===============================================================================
// Smooth
//
// Description: performs the smoothing operation based on graph smoothing method
//===============================================================================
void SM_PathSmoothingSuccessiveNodePair::Smooth()
{
        bool pathInterfering = true;
        int  i = 0, j = 0, size = 0;
        Configuration iNode;
        Configuration jNode;

        this->m_SmoothedPath.Clear();
        this->m_SmoothedPath.AppendPoint(this->m_PathToSmooth->GetPoint(0));

        size = this->m_PathToSmooth->Size();
        if (size == 2)
        {
                this->m_SmoothedPath.AppendPoint(this->m_PathToSmooth->GetPoint(1));
                return;
        }

        i = 0;
        while (i < size - 2 )
        {
                iNode = this->m_PathToSmooth->GetPoint(i);

                for (j = i + 2; j < size; j++)
                {       
                        jNode = this->m_PathToSmooth->GetPoint(j);
                        pathInterfering = this->m_CollisionDetector->IsInterferingLinear(iNode,jNode);
                        
                        if (!pathInterfering && j == size - 1)
                        {
                                this->m_SmoothedPath.AppendPoint(this->m_PathToSmooth->GetPoint(j));
                                return;
                        }
                        else if (!pathInterfering)
                                continue;
                        else
                        {
                                this->m_SmoothedPath.AppendPoint(this->m_PathToSmooth->GetPoint(j-1));
                                break;
                        }
                } // end for
                
                i = j-1;
        } // end while

        assert( i == size - 2 && j == size - 1 );
        this->m_SmoothedPath.AppendPoint(this->m_PathToSmooth->GetPoint(j));
}

---