basic/universe/Universe.cpp

Go to the documentation of this file.

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

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

//## Module: Universe%36FB1455010E; Pseudo Package body
//## Source file: C:\project\mpk\code\Universe\Universe.cpp

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

//## begin module%36FB1455010E.includes preserve=yes
#include <list>
#include <assert.h>
#include "streams\mystreams.h"
//## end module%36FB1455010E.includes

// Configuration
#include "kinematics\Configuration.h"
// Universe
#include "Universe\Universe.h"
// Mesh
#include "geometry\Mesh.h"
// DH_Link
#include "Kinematics\DH_Link.h"
// RobotBase
#include "robots\RobotBase.h"
// IfstreamWithComments
#include "additional\streams\IfstreamWithComments.h"
//## begin module%36FB1455010E.additionalDeclarations preserve=yes
//## end module%36FB1455010E.additionalDeclarations


// Class Universe 





Universe::Universe ()
  //## begin Universe::Universe%961125355.hasinit preserve=no
  //## end Universe::Universe%961125355.hasinit
  //## begin Universe::Universe%961125355.initialization preserve=yes
:
        CD_JointLimits() 
  //## end Universe::Universe%961125355.initialization
{
  //## begin Universe::Universe%961125355.body preserve=yes
  //## end Universe::Universe%961125355.body
}

Universe::Universe (const Universe& right)
:
        CD_JointLimits( right )
{

        int thisSize =  this->theFrameManager.GetNumberOfFrames() ;
        int rightSize = right.theFrameManager.GetNumberOfFrames() ;

        unsigned int i ;
        for( i = 0; i < right.links.size(); i++ )
        {
                LinkBase* addme = right.links[ i ];
                this->AddLink( addme ) ;
        }
        this->totalPolys = right.totalPolys;
}


Universe::~Universe()
{
  //## begin Universe::~Universe%.body preserve=yes
        this->Empty();
  //## end Universe::~Universe%.body
}



//## Other Operations (implementation)
void Universe::AddEntity (Entity* entity)
{
  //## begin Universe::AddEntity%922649211.body preserve=yes
        assert( entity != NULL ) ;

        entity->SetFrameManager( &theFrameManager ) ;   //IMPROVE: should this be done on the cloned version

        //if the entity is really a robot, then each link needs to be added individually
        if( dynamic_cast<RobotBase*>( entity ))
        {
                RobotBase* robot = dynamic_cast<RobotBase*>( entity->Clone() );
                robot->SetFrameManager( &theFrameManager ) ;
                
                //add all the links independantly
                std::list< LinkBase* > allLinks = robot->GetAllLinks() ;
                for( std::list< LinkBase* >::iterator i = allLinks.begin(); i != allLinks.end(); i++ )
                {
                        LinkBase* link = ( *i )->Clone();
                        link->SetFrameManager( this->GetFrameManager() ) ;
                        AddLink( link ) ;
                        delete link ;
                }

                //
                // Add the entity portion of the robot
                //
                entities.push_back( robot ) ;

                //
                // Add all the tool frames to the universe
                //
                std::list< int > allToolFrames = robot->GetAllToolFrames();
                std::list< int >::iterator fIt;
                for( fIt = allToolFrames.begin(); fIt != allToolFrames.end(); fIt++ )
                {
                        int addMe = *fIt;
                        theFrameManager.MarkToolFrame( addMe );
                }
        }
        else
        {
                Entity* addme = entity->Clone() ;
                addme->SetFrameManager( &theFrameManager ) ;
                entities.push_back( addme ) ;
        }
  //## end Universe::AddEntity%922649211.body
}

void Universe::AddLink (LinkBase* link)
{
  //## begin Universe::AddLink%924739817.body preserve=yes
        assert( link != NULL ) ;
        LinkBase* cloned = link->Clone()  ;
        links.push_back( cloned ) ;
        int size = links.size();
        LinkBase* retrieved = links[ size - 1 ];

        //add the objects that are attached to this link
    unsigned int i;
        for( i = 0; i < link->objects.size(); i++ )
        {
                ObjectBase* object = link->objects[ i ] ;
                AddEntity( dynamic_cast< Entity* >( object ) ) ;
        }

        //set up the base frames for the frames controlled by the link just added
        for( i = 0; i < link->controlledFrames.size(); i++ )
        {
                theFrameManager.SetBaseFrame( link->controlledFrames[ i ], link->BaseFrameNum() ) ;
        }
  //## end Universe::AddLink%924739817.body
}

FrameManager* Universe::GetFrameManager ()
{
  //## begin Universe::GetFrameManager%924978286.body preserve=yes
        return &theFrameManager ;
  //## end Universe::GetFrameManager%924978286.body
}

const std::vector< Entity* >& Universe::GetAllEntities () const
{
  //## begin Universe::GetAllEntities%924978289.body preserve=yes
        return entities ;
  //## end Universe::GetAllEntities%924978289.body
}

const std::vector< LinkBase* > Universe::GetAllLinks () const
{
  //## begin Universe::GetAllLinks%924978292.body preserve=yes
        return links ;
  //## end Universe::GetAllLinks%924978292.body
}

void Universe::SetConfiguration (const Configuration& configuration)
{
  //## begin Universe::SetConfiguration%926014533.body preserve=yes
        int lSize = links.size() ;
        int cSize = configuration.Length() ;
        assert( configuration.Length() == links.size() ) ;
        //for all the links, update their frame values
    unsigned int i;
        for( i = 0; i < links.size(); i++ )
        {
                double c = configuration[ i ] ;
                links[ i ]->SetJointVariable( c ) ;
                links[ i ]->UpdateFrames() ;
        }
        theFrameManager.MarkAllFramesChanged() ;        //IMPROVE: don't have to mark them all
  //## end Universe::SetConfiguration%926014533.body
}

int Universe::TotalPolys () const
{
  //## begin Universe::TotalPolys%961125357.body preserve=yes
        int totalPolys = 0 ;
    unsigned int i;
        for( i = 0; i < entities.size(); i++ )
        {
                ObjectBase* entity = dynamic_cast< ObjectBase* >( entities[ i ] ) ;
                if( entity != NULL )
                {
                        totalPolys += entity->TotalPolys() ;
                }
        }
        return totalPolys ;
  //## end Universe::TotalPolys%961125357.body
}

void Universe::Empty ()
{
  //## begin Universe::Empty%927844451.body preserve=yes

        //delete every entity in the universe
    unsigned int i;
        for( i = 0; i < entities.size(); i++ )
        {
                delete( entities[ i ] ) ;
                entities[ i ] = NULL ;
        }

        for( i = 0; i < links.size(); i++ )
        {
                delete( links[ i ] ) ;
                links[ i ] = NULL ;
        }

        //remove all the entities
        entities.clear() ;

        //remove all the links
        links.clear() ;

        //clear the frameManager
        theFrameManager.Clear() ;

  //## end Universe::Empty%927844451.body
}

double Universe::JointMax (const unsigned int jointNum) const
{
  //## begin Universe::JointMax%928263339.body preserve=yes
        unsigned int size = links.size() ;
        assert( jointNum < size ) ;
        LinkBase* theLink = links[ jointNum ] ;
        return theLink->JointMax() ;
  //## end Universe::JointMax%928263339.body
}

double Universe::JointMin (const unsigned int jointNum) const
{
  //## begin Universe::JointMin%928263340.body preserve=yes
        assert( jointNum < links.size() ) ;
        LinkBase* theLink = links[ jointNum ] ;
        return theLink->JointMin() ;
        //IMPROVE: not yet written
  //## end Universe::JointMin%928263340.body
}

unsigned int Universe::DOF () const
{
  //## begin Universe::DOF%928263341.body preserve=yes
        return links.size() ;           //IMPROVE: is this correct?
  //## end Universe::DOF%928263341.body
}

bool Universe::JointWraps (const unsigned int jointNum) const
{
  //## begin Universe::JointWraps%928263342.body preserve=yes
        assert( jointNum < links.size() ) ;
        LinkBase* theLink = links[ jointNum ] ;
        return theLink->JointWraps() ;
  //## end Universe::JointWraps%928263342.body
}

Configuration Universe::GetConfiguration () const
{
  //## begin Universe::GetConfiguration%940357560.body preserve=yes
        Configuration returnMe ;
        int length = links.size() ;
        returnMe.SetLength( length ) ;

        for( int i = 0; i < length; i++ )
        {
                DH_Link* link = dynamic_cast< DH_Link* >( links[ i ] );
                returnMe[ i ] = link->GetJointVariable() ;
        }

        return returnMe ;
  //## end Universe::GetConfiguration%940357560.body
}

const Entity* Universe::GetEntity (const char* name) const
{
  //## begin Universe::GetEntity%961125356.body preserve=yes
    unsigned int i;
        for( i = 0; i < entities.size(); i++ )
        {
                Entity* testMe = entities[ i ] ;
                if( strcmp( testMe->GetName(), name ) == 0 )
                {
                        return testMe ;
                }
        }
        return NULL ;
  //## end Universe::GetEntity%961125356.body
}

CollisionDetectorBase* Universe::Clone () const
{
  //## begin Universe::Clone%961125360.body preserve=yes
        assert( false ) ;
        return NULL ;
  //## end Universe::Clone%961125360.body
}

// Additional Declarations
  //## begin Universe%36FB1455010E.declarations preserve=yes
//-----------------------------------------------------------------------------
std::ostream & operator<<(std::ostream &os, const Universe& v)
{

        using std::endl ;

        //output all the links in the universe
        std::vector< LinkBase*> links = v.GetAllLinks() ;
        os << "#links " << links.size() << endl ;
    unsigned int i;
        for( i = 0; i < links.size(); i++ )
        {
                links[ i ]->Serialize( os ) ;
        }
        os << "#endLinks" << endl ;

        //output all the meshes in frame 0
        std::vector< ObjectBase* > frame0Entities ;
        std::vector< Entity* > allEntities = v.GetAllEntities() ;

        for( i = 0; i < allEntities.size(); i++ )
        {
                Entity* entity = allEntities[ i ] ;
                ObjectBase* object = dynamic_cast< ObjectBase* >( entity ) ;
                if( object == NULL )
                {
                        //IMPROVE: this should not happen
                }
                else
                {
                        if( object->BaseFrame() != 0 )
                        {
                                continue ;
                        }
                        frame0Entities.push_back( object ) ;
                }
        }

        os << "#obstacles " << frame0Entities.size() << endl ;
        for( i = 0; i < frame0Entities.size(); i++ )
        {
                frame0Entities[ i ]->Serialize( os ) ;
                os << endl ;
        }

        //IMPROVE: this code is not yet written
        os << "#endUniverse" << endl ;
        return os ;
}
//-----------------------------------------------------------------------------
IfstreamWithComments & operator>>( IfstreamWithComments &is, Universe& v )
{
        v.Empty() ;

        char marker[ 300 ] = "" ;

        //input the links
        eatwhite( is ) ;
        is.getline( marker, 300, ' ' ) ;
        assert( strcmp( marker, "#links" ) == 0 ) ;

        //get the number of links stored
        int size = 0 ;
        is >> size ;

        //get each of the links, and add it to the universe
    int i;
        for( i = 0; i < size; i++ )
        {
                DH_Link readme( v.GetFrameManager() ) ;
                is >> readme ;
                v.AddLink( &readme ) ;
        }

        //get the end of links marker
        eatwhite( is ) ;
        is.getline( marker, 300 ) ;
        assert( strcmp( marker, "#endLinks" ) == 0 ) ;          //IMPROVE: it's bad to have strings, especially twice

        //read in all the meshes in link 0      //improve: this should be all the meshes and obejcts
        eatwhite( is ) ;
        is.getline( marker, 300, ' ' ) ;
        assert( strcmp( marker, "#obstacles" ) == 0 ) ;         //IMPROVE: it's bad to have strings, especially twice
        int numObstacles = -1 ;
        is >> numObstacles ;

        for( i = 0; i < numObstacles; i++ )
        {
                ObjectBase* object = ObjectBase::Deserialize( is ) ;
                object->SetBaseFrame( 0 ) ;             //IMPROVE: is this necessary?
                v.AddEntity( object ) ;
                delete object ;
        }

        //get the end of universe marker
        eatwhite( is ) ;
        is.getline( marker, 300 ) ;
        assert( strcmp( marker, "#endUniverse" ) == 0 ) ;               //IMPROVE: it's bad to have strings, especially twice

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

---