collisiondetectors/CD_BasicStyle.cpp

Go to the documentation of this file.

#include <assert.h>
#include "CollisionDetectors\CD_BasicStyle.h"
#include <math.h>
#include <math/math2.h>
#include "robots/robotbase.h"
#include "Universe\Universe.h"

CD_BasicStyle::CD_BasicStyle (const Universe& universe)
  :
        CollisionDetectorBase( universe ),
        CD_Linear( universe )
{
        Universe *pUniverse = (Universe *)&universe;    
        theFrameManager = *(pUniverse->GetFrameManager()) ; 

        //add all the entities to the entity list
        std::vector< Entity* > allEntities = universe.GetAllEntities() ;
        int size = allEntities.size() ;
    int i;
        for( i = 0; i < size; i++ )
        {
                Entity* cloneMe = allEntities[ i ] ;
                Entity* clone = cloneMe->Clone() ;
                clone->SetFrameManager( &theFrameManager ) ;
                entities.push_back( clone );
        }

        //add all the links to the link list
        std::vector< LinkBase* > allLinks = universe.GetAllLinks() ;
        for( i = 0; i < allLinks.size(); i++ )
        {
                LinkBase* cloneMe = allLinks[ i ] ;
                LinkBase* addme = cloneMe->Clone() ;
                addme->SetFrameManager( &theFrameManager ) ;
                links.push_back( addme );
        }

        PermActivateAll() ;
        PermDeactivateFramesWithThemselves() ;
}

CD_BasicStyle::CD_BasicStyle (const CD_BasicStyle& right)
:
        CollisionDetectorBase( right ),
        CD_JointLimits( right ),
        CD_Linear( right )
{
        //IMPROVE: need to do some serious deep copying here

        //copy the entities
        entities.clear() ;
    int i;
        for( i = 0; i < right.entities.size(); i++ )
        {
                entities.push_back( right.entities[ i ]->Clone() ) ;
        }

        links.clear() ;
        for( i = 0; i < right.links.size(); i++ )
        {
                LinkBase* cloneMe = right.links[ i ] ;
                LinkBase* addme = cloneMe->Clone() ;
                links.push_back( addme ) ;
        }

        //IMPROVE: will this work?
        permCollisionMatrix = right.permCollisionMatrix ;
        collisionMatrix = right.collisionMatrix ;


        theFrameManager = right.theFrameManager ;
}


CD_BasicStyle::~CD_BasicStyle()
{
    //delete all the cloned entities
    for( int i = 0; i < entities.size(); i++ )
    {
                Entity* deleteMe = entities[ i ] ;
                delete( deleteMe ) ;
                entities[ i ] = NULL ;
        }

        //delete all the cloned links
        for( int j = 0; j < links.size(); j++ )
        {
                LinkBase* deleteme = links[ j ] ;
                delete( deleteme ) ;
                links[ j ] = NULL ;
        }
}



bool CD_BasicStyle::SetConfiguration (const Configuration& config)
{
        bool result = CD_Bool::SetConfiguration( config ) ;
        //update all the frames         //IMPROVE: only do this for those frames that change
        int lsize = links.size();
        int csize = config.Length();
    IJG_Assert( lsize == csize );
        for( int i = 0; i < lsize; ++i )
        {
                double jointVal = config[ i ];
                links[ i ]->SetJointVariable( jointVal );
                links[ i ]->UpdateFrames();
        }

        //mark all the frames as having changed
        theFrameManager.MarkAllFramesChanged() ;

        CD_Bool::configInitialized = true ;
        return result ;
}

double CD_BasicStyle::JointMin (const unsigned int jointNum) const
{
  //## begin CD_BasicStyle::JointMin%928263330.body preserve=yes
        //IMPROVE: I'd rather have this in a more general class - but..
        assert( jointNum < links.size() ) ;
        LinkBase* theLink = links[ jointNum ] ;
        return theLink->JointMin() ;
  //## end CD_BasicStyle::JointMin%928263330.body
}

double CD_BasicStyle::JointMax (const unsigned int jointNum) const
{
  //## begin CD_BasicStyle::JointMax%928263329.body preserve=yes
        //IMPROVE: I'd rather have this in a more general class - but..
        assert( jointNum < links.size() ) ;
        LinkBase* theLink = links[ jointNum ] ;
        return theLink->JointMax() ;
  //## end CD_BasicStyle::JointMax%928263329.body
}

//returns the type of the joint
DH_Parameter CD_BasicStyle::JointType( const unsigned int jointNum ) const
{
        assert( jointNum < links.size() ) ;
        DH_Link* theLink = static_cast< DH_Link* >( links[ jointNum ] ) ;
        return theLink->GetJointType() ;
}


// returns the frame number associated with given joint
unsigned int CD_BasicStyle::JointFrameNum( const unsigned int jointNum ) const
{
        assert( jointNum < links.size() ) ;
        DH_Link* theLink = static_cast< DH_Link* >( links[ jointNum ] ) ;
        return theLink->FrameNum();
}

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

bool CD_BasicStyle::ShouldPerformCollisionCheck (unsigned int frameA, unsigned int frameB) const
{
  //## begin CD_BasicStyle::ShouldPerformCollisionCheck%928344291.body preserve=yes

        //check the collisionMatrix
        bool c = false ;
        return true ;   //improve: this is a hack
/*      std::map< unsigned int, std::map< unsigned int, bool > >::const_iterator first = collisionMatrix.find( frameA );
        if( first != collisionMatrix.end() )
        {
                std::map< unsigned int, bool >::const_iterator second = ( *first ).second.find( frameB ) ;
                if( second != ( *first ).second.end() )
                {
                        c = true ;
                }
        }*/
        assert( false ) ;

        //check the permCollisionMatrix
        /*first = permCollisionMatrix.find( frameA );
        if( first != permCollisionMatrix.end() )
        {
                std::map< unsigned int, bool >::const_iterator second = ( *first ).second.find( frameB ) ;
                if( second != ( *first ).second.end() )
                {
                        //cannot be turned on if it's off at this point
                }
                else
                {
                        c = false ;     //can only be turned off 
                }
        }
        else
        {
                c = false ;
        }*/
        assert( false ) ;

        return c ;
//      if( links[ linkB ]->DoesLinkControlFrame( links[ linkA ]->BaseFrameNum() ) == true )
//      {
//              return false ;
//      }
//      if( links[ linkA ]->DoesLinkControlFrame( links[ linkB ]->BaseFrameNum() ) == true )
//      {
//              return false ;
//      }
//      return true ;
  //## end CD_BasicStyle::ShouldPerformCollisionCheck%928344291.body
}

unsigned int CD_BasicStyle::DOF () const
{
  //## begin CD_BasicStyle::DOF%930942582.body preserve=yes
        return links.size() ;   //IMPROVE: some links may not translate into degrees of freedom ?
  //## end CD_BasicStyle::DOF%930942582.body
}

void CD_BasicStyle::DeactivateFrames (const unsigned int frame1, const unsigned int frame2)
{
  //## begin CD_BasicStyle::DeactivateFrames%931291663.body preserve=yes
        if( frame2 > frame1 )
        {
                DeactivateFrames( frame2, frame1 ) ;
                return ;
        }

        if( !FramePairPermEnabled( frame1, frame2 ) )
        {
                return ;
        }

        PairInt findMe1( frame1, frame2 ) ;
        std::map< PairInt, bool >::iterator found1 = collisionMatrix.find( findMe1 ) ;
        if( found1 == collisionMatrix.end() )
        {
                return ;
        }
        collisionMatrix.erase( found1 ) ;

        PairInt findMe2( frame2, frame1 ) ;
        std::map< PairInt, bool >::iterator found2 = collisionMatrix.find( findMe2 ) ;
        if( found2 == collisionMatrix.end() )
        {
                return ;
        }
        collisionMatrix.erase( found2 ) ;

        //IMPROVE: check to see if this worked
  //## end CD_BasicStyle::DeactivateFrames%931291663.body
}

void CD_BasicStyle::ActivateFrames (const unsigned int frame1, const unsigned int frame2)
{
  //## begin CD_BasicStyle::ActivateFrames%931291664.body preserve=yes
        if( frame2 > frame1 )
        {
                ActivateFrames( frame2, frame1 ) ;
        }

        if( !this->FramePairPermEnabled( frame1, frame2 ) )
        {
                return ;
        }

        std::map< PairInt, bool >::value_type insertMe1( PairInt( frame1, frame2 ), true ) ;
        collisionMatrix.insert( insertMe1 ) ;
        std::map< PairInt, bool >::value_type insertMe2( PairInt( frame2, frame1 ), true ) ;
        collisionMatrix.insert( insertMe2 ) ;

        //IMPROVE: check to see if this worked
  //## end CD_BasicStyle::ActivateFrames%931291664.body
}

void CD_BasicStyle::PermDeactivateFrames (const unsigned int frame1, const unsigned int frame2)
{
  //## begin CD_BasicStyle::PermDeactivateFrames%931892328.body preserve=yes
        if( frame2 > frame1 )
        {
                PermDeactivateFrames( frame2, frame1 ) ;
        }

        PairInt findMe1( frame1, frame2 ) ;
        std::map< PairInt, bool >::iterator found1 = permCollisionMatrix.find( findMe1 ) ;
        if( found1 == permCollisionMatrix.end() )
        {
                DeactivateFrames( frame1, frame2 ) ;
                return ;
        }
        permCollisionMatrix.erase( found1 ) ;

        PairInt findMe2( frame2, frame1 ) ;
        std::map< PairInt, bool >::iterator found2 = permCollisionMatrix.find( findMe2 ) ;
        if( found2 == permCollisionMatrix.end() )
        {
                DeactivateFrames( frame2, frame1 ) ;
                return ;
        }
        permCollisionMatrix.erase( found2 ) ;

        

        assert( FramePairPermEnabled( frame1, frame2 ) == false ) ;
        DeactivateFrames( frame1, frame2 ) ;
        //frame is deactivated 

  //## end CD_BasicStyle::PermDeactivateFrames%931892328.body
}

void CD_BasicStyle::PermActivateFrames (const unsigned int frame1, const unsigned int frame2)
{
  //## begin CD_BasicStyle::PermActivateFrames%931892329.body preserve=yes
        if( frame2 > frame1 )
        {
                PermActivateFrames( frame2, frame1 ) ;
        }

        std::map< PairInt, bool >::value_type insertMe1( PairInt( frame1, frame2 ), true ) ;
        permCollisionMatrix.insert( insertMe1 ) ;
        std::map< PairInt, bool >::value_type insertMe2( PairInt( frame2, frame1 ), true ) ;
        permCollisionMatrix.insert( insertMe2 ) ;

        assert( FramePairPermEnabled( frame1, frame2 ) == true ) ;
        assert( FramePairPermEnabled( frame2, frame1 ) == true ) ;
        ActivateFrames( frame1, frame2 ) ;
  //## end CD_BasicStyle::PermActivateFrames%931892329.body
}

bool CD_BasicStyle::FramePairPermEnabled (const unsigned int frame1, const unsigned int frame2) const
{
  //## begin CD_BasicStyle::FramePairPermEnabled%931892330.body preserve=yes
        PairInt findMe( frame1, frame2 ) ;
        std::map< PairInt, bool >::const_iterator found = permCollisionMatrix.find( findMe ) ;
        if( found == permCollisionMatrix.end() )
        {
                return false ;
        }
        return ( *found ).second ;
  //## end CD_BasicStyle::FramePairPermEnabled%931892330.body
}

//allows access to the frame manager
FrameManager* CD_BasicStyle::GetFrameManager()
{
        return &theFrameManager;
}

void CD_BasicStyle::DeactivateAllFrames ()
{
  //## begin CD_BasicStyle::DeactivateAllFrames%940546750.body preserve=yes
        for( int i = 0; i <= DOF(); i++ )
        {
                for( int j = 0; j <= DOF(); j++ )
                {
                        DeactivateFrames( i, j ) ;
                }
        }
  //## end CD_BasicStyle::DeactivateAllFrames%940546750.body
}

void CD_BasicStyle::PermActivateAll ()
{
  //## begin CD_BasicStyle::PermActivateAll%941590723.body preserve=yes
        //at first all objects are able to interfere with one another - need to turn off inter frame collisions
        for( int i = 0; i <= DOF(); i++ ) 
        {
                for( int j = 0; j <= i; j++ )
                {
                        PermActivateFrames( i, j ) ;
                }
        }

/*      for( int i = 0; i < links.size(); i++ )         //IMPROVE: this used to say linkVcollide
        {
                LinkBase* link = links[ i ] ;
                //link->SetFrameManager( &theFrameManager ) ;
                int size = link->controlledFrames.size() ;
                for( int j = 0; j < link->controlledFrames.size(); j++ )
                {
                        unsigned int frameNum = link->controlledFrames[ j ] ;
                        unsigned int baseNum = link->BaseFrameNum() ;
                        PermDeactivateFrames( baseNum, baseNum ) ;                      
                        PermDeactivateFrames( frameNum, frameNum ) ;
                }
        }*/
  //## end CD_BasicStyle::PermActivateAll%941590723.body
}

void CD_BasicStyle::PermDeactivateFramesWithThemselves ()
{
  //## begin CD_BasicStyle::PermDeactivateFramesWithThemselves%941590724.body preserve=yes
        for( int i = 0; i <= DOF(); i++ )
        {
                PermDeactivateFrames( i, i ) ;
        }
  //## end CD_BasicStyle::PermDeactivateFramesWithThemselves%941590724.body
}

void CD_BasicStyle::PermDeactivateAll ()
{
  //## begin CD_BasicStyle::PermDeactivateAll%941688703.body preserve=yes
        for( int i = 0; i <= DOF(); i++ ) 
        {
                for( int j = 0; j <= i; j++ )
                {
                        PermDeactivateFrames( i, j ) ;
                }
        }
  //## end CD_BasicStyle::PermDeactivateAll%941688703.body
}

//The following two functions are added by Zhenwang.Yao
std::vector<LinkBase*> CD_BasicStyle::GetAllLinks() const 
{
  return links;
}

//The following two functions are added by Zhenwang.Yao
std::vector<Entity*> CD_BasicStyle::GetAllElements() const 
{
  return entities;
}

//=============================================================================
// CD_BasicStyle::GetRobot
//=============================================================================
RobotBase* CD_BasicStyle::GetRobot( const int index ) const
{
    int counter = 0;
    int i;
    int size = entities.size();
    for( i = 0; i < size; ++i )
    {
        Entity* testMe = entities[ i ];
        RobotBase* robot = dynamic_cast< RobotBase* >( testMe );
        if( robot != NULL )
        {
            if( counter == index )
            {
                return robot;
            }
            else
            {
                ++counter;
                continue;
            }
        }
    }
    return NULL;
}

//End Added by Zhenwang.Yao

// Additional Declarations
  //## begin CD_BasicStyle%37556E320142.declarations preserve=yes
  //## end CD_BasicStyle%37556E320142.declarations

//## begin module%37556E320142.epilog preserve=yes
//## end module%37556E320142.epilog

---