basic/paths/PA_Points.cpp

Go to the documentation of this file.

//## begin module%36FDB0E70104.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%36FDB0E70104.cm

//## begin module%36FDB0E70104.cp preserve=no
//## end module%36FDB0E70104.cp

//## Module: PA_Points%36FDB0E70104; Pseudo Package body
//## Source file: C:\project\mpk\code\Paths\PA_Points.cpp

//## begin module%36FDB0E70104.additionalIncludes preserve=no
//## end module%36FDB0E70104.additionalIncludes

//## begin module%36FDB0E70104.includes preserve=yes
#include <math.h>
#include <iostream>
#include <strstream>
#include <assert.h>
#include "streams/mystreams.h"
//## end module%36FDB0E70104.includes

// Configuration
#include "kinematics\Configuration.h"
// PA_Points
#include "Paths\PA_Points.h"
//## begin module%36FDB0E70104.additionalDeclarations preserve=yes
//## end module%36FDB0E70104.additionalDeclarations


// Class PA_Points 


PA_Points::PA_Points ()
  //## begin PA_Points::PA_Points%922595690.hasinit preserve=no
  //## end PA_Points::PA_Points%922595690.hasinit
  //## begin PA_Points::PA_Points%922595690.initialization preserve=yes
  //## end PA_Points::PA_Points%922595690.initialization
{
  //## begin PA_Points::PA_Points%922595690.body preserve=yes
  //## end PA_Points::PA_Points%922595690.body
}


PA_Points::~PA_Points()
{
  //## begin PA_Points::~PA_Points%.body preserve=yes
  //## end PA_Points::~PA_Points%.body
}



//## Other Operations (implementation)
PA_Points& PA_Points::operator = (const PA_Points& right)
{
  //## begin PA_Points::operator=%923255757.body preserve=yes

        if( this == &right )
        {
                return *this ;
        }
        Clear() ;
        for( int i = 0; i < right.Size() ; i++ )
        {
                Configuration r = right[ i ] ;  //IMPROVE: this is supposed to be here

                points.push_back( r ) ;
//              points.push_back( r ) ;
//              points.push_back( r ) ;
//              AppendPoint( r ) ;
        }
        return *this ;
  //## end PA_Points::operator=%923255757.body
}

void PA_Points::AppendPoint (const Configuration& point)
{
#ifdef _DEBUG
    int pointsSize = points.size();
    if( pointsSize >= 1 )
    {
        int firstSize = points[ 0 ].Size();
        int thisSize = point.size();
        IJG_Assert( firstSize == thisSize );
        //this assert means that some things in the 
        //path aren't points in the same vector space
        //thats BAD
    }
#endif

        points.push_back( point ) ;
}

void PA_Points::Clear ()
{
        points.clear() ;
}

void PA_Points::Output () const
{
  //## begin PA_Points::Output%923255763.body preserve=yes
        using std::cout ;
        using std::endl ;
        cout << "Path Start" << endl ;
        for( int i = 0; i < points.size(); i++ )
        {
                points[ i ].Output() ;
                cout << endl ;
        }
        cout << "PathEnd" << endl ;
  //## end PA_Points::Output%923255763.body
}

double PA_Points::PhysicalLength() const
{
    double totalDistance = 0.0;
    int i;
    int size = Size() - 1;
    for( i = 0; i < size; ++i )
    {
        const Configuration& c0 = operator[]( i + 0 );
        const Configuration& c1 = operator[]( i + 1 );
        double distance = ( c1 - c0 ).Magnitude();
        totalDistance += distance;
    }
    return totalDistance;
}

unsigned int PA_Points::Size () const
{
  //## begin PA_Points::Size%926550116.body preserve=yes
        return points.size() ;
  //## end PA_Points::Size%926550116.body
}

const Configuration& PA_Points::operator [](const unsigned int index) const
{
        assert( index < points.size() ) ;
        return points[ index ] ;
}

Configuration& PA_Points::operator [] (const unsigned int index)
{
        assert( index < points.size() ) ;
        return points[ index ] ;
}

Configuration PA_Points::GetConfiguration (double percent) const
{
  //## begin PA_Points::GetConfiguration%926635235.body preserve=yes
        assert( percent >= 0.0 ) ;
        assert( percent <= 1.0 ) ;
        if( points.size() == 1 )
        {
                return points[ 0 ] ;
        }

        assert( points.size() != 0 ) ;
        assert( points.size() >= 1 ) ;
        if( points.size() == 1 )
        {
                return points[ 0 ] ;
        }

        //determine the correct index 
        int index1 = floor( ( points.size() - 1 ) * percent ) ;
        int index2 = ceil(  ( points.size() - 1 ) * percent );
        double remainder = ( points.size() - 1 ) * percent - ( floor( ( points.size() - 1 ) * percent ) ) ;

        double weight1 = ( 1 - remainder ) ;
        double weight2 = remainder ;
        return points[ index1 ] * weight1 + points[ index2 ] * weight2 ;
  //## end PA_Points::GetConfiguration%926635235.body
}

Configuration PA_Points::GetPoint(const unsigned int index) const
{
        assert (index >= 0);
        assert (index < points.size() );
        return points[ index ];
}

void PA_Points::Serialize (char* str, const unsigned int size) const
{
  //## begin PA_Points::Serialize%933360619.body preserve=yes
        //output the start marker
        using std::endl ;
        using std::strstream ;
        strstream stream( str, size, std::ios::out ) ;
        stream << "#PA_POINTS" << endl ;
        stream << *this ;       

  //## end PA_Points::Serialize%933360619.body
}

void PA_Points::AppendPointToBeginning (const Configuration& point)
{
  //## begin PA_Points::AppendPointToBeginning%965675402.body preserve=yes
        points.insert( points.begin(), point ) ;
  //## end PA_Points::AppendPointToBeginning%965675402.body
}

void PA_Points::DeleteFirstPoint()
{
        points.erase(points.begin());
}

// Additional Declarations
  //## begin PA_Points%36FDB0E70104.declarations preserve=yes
//-----------------------------------------------------------------------------
std::ostream & operator<<(std::ostream &os, const PA_Points& v)
{
        using std::endl ;
        os << v.Size() << endl ;

        for( int i = 0; i < v.Size(); i++ )
        {
                Configuration config = v[ i ]; 
                os << config << endl ;
        }

        //output the terminating marker
        os << "#end_pa_points" << endl ;
        return os ;
}
//-----------------------------------------------------------------------------
std::istream & operator>>( std::istream &is, PA_Points& v)
{
        int size = 0 ;
        is >> size ;

        for( int i = 0; i < size; i++ )
        {
                Configuration config  ;
                is >> config ;
                v.AppendPoint( config ) ;
        }

        char marker[ 300 ] = "" ;
        eatwhite( is ) ;
        is.getline( marker, 300 ) ;
        assert( stricmp( marker, "#end_pa_points" ) == 0 ) ;

        return is ;
}
//-----------------------------------------------------------------------------
  //## end PA_Points%36FDB0E70104.declarations
//## begin module%36FDB0E70104.epilog preserve=yes
//## end module%36FDB0E70104.epilog

---