smoothers/SM_PathSmoothingRandomNodePair.cpp

Go to the documentation of this file.

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

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

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

//===============================================================================
// Smooth
//
// Description: performs the smoothing operation based on path smoothing method
//===============================================================================
void SM_PathSmoothingRandomNodePair::Smooth()
{
        bool pathInterfering = true;
        int      t1 = 0, t2 = 0, temp = 0, size = 0;
        double old_dist = 0.0, new_dist = 0.0;
        Configuration c1;
        Configuration c2;

        size = this->m_PathToSmooth->Size();
        // no filtering is needed
        if (size == 2)
        {
                this->m_SmoothedPath.Clear();
                this->m_SmoothedPath.AppendPoint(this->m_PathToSmooth->GetPoint(0));
                this->m_SmoothedPath.AppendPoint(this->m_PathToSmooth->GetPoint(1));
                return;
        }

        this->m_Points.clear();
        for (int i = 0; i < size; i++)
        {
                this->m_Points.push_back(this->m_PathToSmooth->GetPoint(i));
        }

        // repeat n times
        for (int n = 0; n < 1000; n++)
        {
                t1 = Random(0,this->m_Points.size());
                t2 = Random(0,this->m_Points.size());

                // ensure t1 < t2
                if (t1 > t2)
                {
                        temp = t1;
                        t1 = t2;
                        t2 = temp;
                }
                assert(t1 <= t2);

                c1 = this->m_Points[t1];
                c2 = this->m_Points[t2];
                pathInterfering = this->m_CollisionDetector->IsInterferingLinear(c1,c2);

                if (pathInterfering)
                        continue;
                else
                {
                        new_dist = Distance(c1,c2);
                        old_dist = 0.0;
                        for (int j = t1; j < t2; j++)
                        {
                                old_dist += Distance(this->m_Points[j],this->m_Points[j+1]);
                        }
                        if (new_dist < old_dist)
                                UpdatePoints(t1,t2);
                }
        }

        this->m_SmoothedPath.Clear();
        for (int k = 0; k < this->m_Points.size(); k++)
        {
                this->m_SmoothedPath.AppendPoint(this->m_Points[k]);
        }
}

double SM_PathSmoothingRandomNodePair::Distance(const Configuration& conf1, const Configuration& conf2) const
{
        return m_CollisionDetector->DistanceBetween( conf1, conf2 ) ;
}

int SM_PathSmoothingRandomNodePair::Random(const int min, const int max) const
{
        return ( int(min + rand() % max) );
}

void SM_PathSmoothingRandomNodePair::UpdatePoints(const int index1, const int index2)
{
        this->m_Points.erase(this->m_Points.begin()+index1+1, this->m_Points.begin()+index2 );
}

---