collisiondetectors/CD_Vcollide.cpp

Go to the documentation of this file.

#pragma warning( disable : 4786 )
#include "synchronization\semaphore.h"
#include "robots\r_openchain.h"
#include "CollisionDetectors\CD_Vcollide.h"
#include "geometry\Facet.h"
#include "geometry\ObjectGroup.h"
#include "Kinematics\LinkBase.h"
#include "Universe/universe.h"
#include "geometry\geo_rangesensor\Range_Sensor.h"


// Class CD_Vcollide 


CD_Vcollide::CD_Vcollide (Universe& universe)
: 
    universePointer( NULL ),
        CollisionDetectorBase( universe ),
        CD_BasicStyle( universe ),
        CD_Linear( universe ),
        CD_LinearContinuous( universe ),
        vcollide( NULL )
{

        vcollide = new VCollide ;

        //go through all the entities that are in the entity list, and add all the meshes to the vcollide
        bool success = true ;
        for( int j = 0; j < entities.size(); j++ )
        {
                Entity* theEntity = entities[ j ] ;
                entities[ j ]->SetFrameManager( &theFrameManager ) ;
                success &= AddEntityToVcollide( entities[ j ] ) ;
        }
        CD_Bool::allGeometryUsed = success ;    //this tells us if all the geometry was used in the collision detector

        //unfortunately, it appears we need to call these here, as well as in the CD_BasicStyle constructor
        PermActivateAll() ;
        PermDeactivateFramesWithThemselves() ;

    //
    // deactivaste frames with their parents
    //
    std::vector< LinkBase* > links = universe.GetAllLinks();
    int i;
    int size = links.size();
    for( i = 0; i < size; i++ )
    {
        LinkBase* link = links[ i ];
        int baseframe = link->BaseFrameNum();

        int j;
        int controlledSize = link->controlledFrames.size();
        for( j = 0; j < controlledSize; j++ )
        {
            int controlledFrame = link->controlledFrames[ j ];
            DeactivateFrames( baseframe, controlledFrame );
        }
    }
        
  //## end CD_Vcollide::CD_Vcollide%928344296.body
}

CD_Vcollide::CD_Vcollide (const CD_Vcollide& right)
  //## begin CD_Vcollide::CD_Vcollide%930249730.hasinit preserve=no
      : universePointer(NULL)
  //## end CD_Vcollide::CD_Vcollide%930249730.hasinit
  //## begin CD_Vcollide::CD_Vcollide%930249730.initialization preserve=yes
        ,CollisionDetectorBase( right ),
        CD_BasicStyle( right ),
        CD_Linear( right ),
        CD_LinearContinuous( right ),
        vcollide( NULL )
  //## end CD_Vcollide::CD_Vcollide%930249730.initialization
{
  //## begin CD_Vcollide::CD_Vcollide%930249730.body preserve=yes

        //IMPROVE: cheat by using a retained pointer to the universe

        //the entities will be recopied later on - the ones copied in CD_BasicStyle are deleted
        vcollide = new VCollide ;
        
        meshes.reserve( right.meshes.size() ) ;
        for( int i = 0; i < right.meshes.size(); i++ )
        {
                int numMeshes = meshes.size() ;
                Mesh* addMe = right.meshes[ i ] ; 
                this->AddEntityToVcollide( addMe ) ;
                //meshes.push_back( dynamic_cast< Mesh* >( right.meshes[ i ]->Clone() ) ) ;
        }

  //## end CD_Vcollide::CD_Vcollide%930249730.body
}


CD_Vcollide::~CD_Vcollide()
{
  //## begin CD_Vcollide::~CD_Vcollide%.body preserve=yes
    int i;
        for( i = 0; i < meshes.size(); i++ )
        {
                Mesh* deleteMe = meshes[ i ] ;
                delete deleteMe ;
                meshes[ i ] = NULL ;
        }
        meshes.clear() ;

        for( i = 0; i < vcollideIndexes.size(); i++ )
        {
                vcollide->DeleteObject( vcollideIndexes[ i ] ) ;
        }
        vcollideIndexes.clear() ;
        dirtyElements.clear() ;
        for( i = 0; i < linkVcollide.size(); i++ )
        {
                linkVcollide[ i ].clear() ;
        }
        linkVcollide.clear() ;

        delete vcollide ;
  //## end CD_Vcollide::~CD_Vcollide%.body
}



//## Other Operations (implementation)
CollisionDetectorBase* CD_Vcollide::Clone () const
{
  //## begin CD_Vcollide::Clone%928344287.body preserve=yes
        CD_Vcollide* newVC = new CD_Vcollide( *this ) ;
        CD_InterfaceToCollisionQueries* CDitc = dynamic_cast< CD_Bool* >( newVC ) ;
        CollisionDetectorBase* returnMe = dynamic_cast< CollisionDetectorBase* >( CDitc );
        return returnMe ;
  //## end CD_Vcollide::Clone%928344287.body
}

bool CD_Vcollide::IsInterfering (const Configuration& config)
{
  //## begin CD_Vcollide::IsInterfering%928344289.body preserve=yes
        CD_Bool::IncrementCallCount();

        Semaphore semaphore( this->guid );
        semaphore.Lock();
        SetConfiguration( config ) ;

        //improve: you only need to update the frames that have actually changed, no?

        //go through all the links and look at the meshes that are there to update their frames
        for( int i = 0; i < meshes.size(); i++ )
        {
                const Mesh* mesh = meshes[ i ] ;

                Matrix4x4 m1 = mesh->GetTransform() ; 

                int vcollideIndex = vcollideIndexes[ i ] ;
                assert( vcollideIndex >= 0 ) ;

                //IMPROVE: find a way to do this via a funciton call
                vcollide->UpdateTrans( vcollideIndex, m1.values ) ;
        }
        
        //IMPROVE: this is a hack
//      this->DeactivateAllFrames() ;

        int success = vcollide->Collide() ;
        assert( success == VC_OK ) ;
        VCReportType vcrep ;
        int numCollisions = vcollide->Report( 1, &vcrep ) ;
        bool returnMe = false;
        if( numCollisions == 0 )                //IMPROVE: don't bother with the report
        {
                returnMe = false;
        }
        else
        {
                returnMe = true;
        }
        semaphore.Unlock();
        return returnMe;
  //## end CD_Vcollide::IsInterfering%928344289.body
}

bool CD_Vcollide::IsInterfering (const Frame &pose)
{
        Semaphore semaphore( this->guid );
        semaphore.Lock();

        int vcollideIndex = -1;
        for( int i = 0; i < meshes.size(); i++ )
        {               
                if (meshes[ i ]->BaseFrame() == 1)
                {
                        vcollideIndex = vcollideIndexes[ i ] ;
                        break;
                }
        }

        assert( vcollideIndex != -1 );
        if (vcollideIndex == -1)
                return false;

        Matrix4x4 m1;
        m1.SetValues(pose);
        vcollide->UpdateTrans( vcollideIndex, m1.values ) ;
        
        int success = vcollide->Collide() ;
        assert( success == VC_OK ) ;
        VCReportType vcrep ;
        int numCollisions = vcollide->Report( 1, &vcrep ) ;
        bool returnMe = false;
        if( numCollisions == 0 )                //IMPROVE: don't bother with the report
        {
                returnMe = false;
        }
        else
        {
                returnMe = true;
        }
        semaphore.Unlock();
        return returnMe;
}

bool CD_Vcollide::AddEntityToVcollide (const Entity* entity)
{
  //## begin CD_Vcollide::AddEntityToVcollide%928344302.body preserve=yes
        //IMPROVE: I think there's a memory leak in here

        //make sure we have this base frame set up properly in the map
        int size = linkVcollide.size() ;
        int baseFrame = entity->BaseFrame() ;
        while( linkVcollide.size() < baseFrame + 1 )
        {
                std::set< unsigned int > setOfVcollideMeshes ;
                linkVcollide.push_back( setOfVcollideMeshes );
        }

        const Mesh* mesh = dynamic_cast< const Mesh* >( entity ) ;
        if( mesh != NULL )
        {
                int vcollideIndex = AddMeshToVcollide( *mesh ) ;
                return true ;
        }
        const ObjectGroup* group = dynamic_cast< const ObjectGroup* >( entity ) ;
        if( group != NULL )
        {
                bool returnMe = true ;
                for( int i = 0; i < group->Size(); i++ )
                {
                        const ObjectBase* addme = ( *group )[ i ] ;
                        returnMe &= AddEntityToVcollide( addme ) ;
                }
                return returnMe ;
        }

        const R_OpenChain* rOpenChain = dynamic_cast< const R_OpenChain* >( entity ) ;
        if( rOpenChain != NULL )
        {
                //IMPROVE: do some extra stuff here
                return true ;
        }

        const Range_Sensor* camera = dynamic_cast< const Range_Sensor* >( entity ) ;
        if( camera != NULL )
        {
                //cameras don't need to be added to the simple version of vcollide, only meshes
                return true ;
        }

        return false ;
  //## end CD_Vcollide::AddEntityToVcollide%928344302.body
}

int CD_Vcollide::AddMeshToVcollide (const Mesh& mesh)
{
  //## begin CD_Vcollide::AddMeshToVcollide%928373069.body preserve=yes
        //IMPROVE: Function contains a memory leak

        Mesh* addMe = dynamic_cast< Mesh*>( mesh.Clone() ) ;
        meshes.push_back( addMe ) ;

        //ask vcollide to create a new object
        int vcollideIndex = -1 ;
        int success = vcollide->NewObject( &vcollideIndex );
        assert( success == VC_OK ) ;

        //need to add each triangle to vcollide individually
    int size = mesh.GetFacets().size();
    int i;
        for( i = 0; i < size; ++i )
        {
                Facet face = mesh.GetFacets()[ i ] ;
                int edges = face.vertexNumbers.size() ;
                assert( edges == 3 ) ;
                double v1[ 3 ] ;
                double v2[ 3 ] ;
                double v3[ 3 ] ;

                //copy the vertex points of the mesh into the arrays above
                for( int j = 0; j < 3; j++ )
                {
                        int meshSize  = mesh.GetVertexes().size() ;
                        int faceSize = face.vertexNumbers.size() ;
                        //int vertexNumber = face.vertexNumbers[ 2 ] ;
                        v1[ j ] = mesh.GetVertexes()[ face.vertexNumbers[ 0 ] ][ j ] ;
                        v2[ j ] = mesh.GetVertexes()[ face.vertexNumbers[ 1 ] ][ j ] ;
                        v3[ j ] = mesh.GetVertexes()[ face.vertexNumbers[ 2 ] ][ j ] ;
                }
                vcollide->AddTri( v1, v2, v3 ) ;
        }

        //tell VCollide that we're done adding the object
        success = vcollide->EndObject() ;
        if( success == VC_ERR_EMPTY_OBJECT )
        {
                assert( false ) ;
        }
        assert( success == VC_OK ) ;

        //need to update the list of baseframe - mesh references
        int baseFrame = mesh.BaseFrame() ;

        std::set< unsigned int > theSet = linkVcollide[ baseFrame ] ;
        theSet.insert( vcollideIndex ) ;
        linkVcollide[ baseFrame ] = theSet ;    //IMPROVE: use a reference here instead
        size = theSet.size() ;
        size = linkVcollide[ baseFrame ].size() ;

        vcollideIndexes.push_back( vcollideIndex ) ;
        return vcollideIndex ;
  //## end CD_Vcollide::AddMeshToVcollide%928373069.body
}

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

        //if we're deactivating a frame with no geometry, just return
        if( lSize <= frame1 )
        {
                return ;
        }
        if( lSize <= frame2 )
        {
                return ;
        }

        int size1 = linkVcollide[ frame1 ].size() ;
        std::set< unsigned int >::iterator i1 ;
        std::set< unsigned int >::iterator i2 ;

        for( i1 = linkVcollide[ frame1 ].begin(); i1 != linkVcollide[ frame1 ].end() ; i1++ )
        {
                int index1 = *i1 ;
                for( i2 = linkVcollide[ frame2 ].begin(); i2 != linkVcollide[ frame2 ].end(); i2++ )
                {
                        int index2 = *i2 ;
                        if( index1 != index2 )
                        {
                                int success = vcollide->DeactivatePair( index1, index2 ) ;
                                success &= vcollide->DeactivatePair( index2, index1 ) ;
                                assert( success == 1 ) ;
                        }
                }
        }
  //## end CD_Vcollide::DeactivateFrames%928449848.body
}

void CD_Vcollide::UpdateMovedLink (const unsigned int linkNum) const
{
  //## begin CD_Vcollide::UpdateMovedLink%928449851.body preserve=yes
        //IMPROVE: is this ever called?
        links[ linkNum ]->UpdateFrames() ;
  //## end CD_Vcollide::UpdateMovedLink%928449851.body
}

void CD_Vcollide::ActivateFrames (const unsigned int frame1, const unsigned int frame2)
{
  //## begin CD_Vcollide::ActivateFrames%931291665.body preserve=yes
        if( !FramePairPermEnabled( frame1, frame2 ) )   //IMPROVE: NOT SIGN?
        {
                return ;        //if the pair is permanantly disabled, then don't activate it
        }

        if( frame2 > frame1 )
        {
                ActivateFrames( frame2, frame1 ) ;
                return ;
        }

        CD_BasicStyle::ActivateFrames( frame1, frame2 ) ;
        int lSize = linkVcollide.size() ;

        //if we're activating a frame with no geometry, just return
        if( lSize <= frame1 )
        {
                return ;
        }
        if( lSize <= frame2 )
        {
                return ;
        }

        int size1 = linkVcollide[ frame1 ].size() ;
        std::set< unsigned int >::iterator i1 ;
        std::set< unsigned int >::iterator i2 ;

        int geometriesInFrame1 = linkVcollide[ frame1 ].size() ;
        int geometriesInFrame2 = linkVcollide[ frame2 ].size() ;

        for( i1 = linkVcollide[ frame1 ].begin(); i1 != linkVcollide[ frame1 ].end() ; i1++ )
        {
                int index1 = *i1 ;
                for( i2 = linkVcollide[ frame2 ].begin(); i2 != linkVcollide[ frame2 ].end(); i2++ )
                {
                        int index2 = *i2 ;
                        if( index1 != index2 )
                        {
                                vcollide->ActivatePair( index1, index2 ) ;
                                vcollide->ActivatePair( index2, index1 ) ;
                                //IMPROVE: this has not been tested
                        }
                }
        }
  //## end CD_Vcollide::ActivateFrames%931291665.body
}

// Additional Declarations
  //## begin CD_Vcollide%37548423012E.declarations preserve=yes
  //## end CD_Vcollide%37548423012E.declarations

//## begin module%37548423012E.epilog preserve=yes
//## end module%37548423012E.epilog

---