smoothers/SM_SuccessiveAndRandomSmoothing.cpp

Go to the documentation of this file.

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

//=============================================================================
// Constructor
//
// Description: constructor
//=============================================================================
SM_SuccessiveAndRandomSmoothing::SM_SuccessiveAndRandomSmoothing()
        :
        SM_PathSmoothingSuccessiveNodePair(), SM_PathSmoothingRandomNodePair()
{
}

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

//===============================================================================
// Smooth
//
// Description: performs the smoothing operation based on cascading the
//              graph smoothing method followed by the path smoothing method
//===============================================================================
void SM_SuccessiveAndRandomSmoothing::Smooth()
{
        SM_PathSmoothingSuccessiveNodePair::Smooth();
        this->m_Interim = this->m_SmoothedPath;
        PathSmoothing();
}

void SM_SuccessiveAndRandomSmoothing::PathSmoothing()
{
        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_Interim.Size();
        // no filtering is needed
        if (size == 2)
        {
                this->m_SmoothedPath.Clear();
                this->m_SmoothedPath.AppendPoint(this->m_Interim.GetPoint(0));
                this->m_SmoothedPath.AppendPoint(this->m_Interim.GetPoint(1));
                return;
        }

        this->m_Points.clear();
        for (int i = 0; i < size; i++)
        {
                this->m_Points.push_back(this->m_Interim.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]);
        }
}

---