collisiondetectors/CD_Solid.cpp

Go to the documentation of this file.

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

//## begin module%377944220020.cp preserve=no
//## end module%377944220020.cp

//## Module: CD_Solid%377944220020; Pseudo Package body
//## Source file: C:\project\mpk\code\CollisionDetectors\CD_Solid.cpp

//## begin module%377944220020.additionalIncludes preserve=no
//## end module%377944220020.additionalIncludes

//## begin module%377944220020.includes preserve=yes
#pragma warning( disable : 4786 )
#pragma message ( "Solid has been disabled as a collision detector" )
//#include "SOLID-2.0\include\SOLID\solid.h"    //IMPROVE: make this a relational pathl
#include <stdio.h>
#include "robots\robotbase.h"
//## end module%377944220020.includes

// CD_Solid
#include "CollisionDetectors\CD_Solid.h"
// Entity
#include "Universe\Entity.h"
// MPK_Box
#include "geometry\MPK_Box.h"
// MPK_Cylinder
#include "geometry\MPK_Cylinder.h"
// Facet
#include "geometry\Facet.h"
// Mesh
#include "geometry\Mesh.h"
// ObjectGroup
#include "geometry\ObjectGroup.h"
//## begin module%377944220020.additionalDeclarations preserve=yes

//IMPROVE: this global function is BAD put it elsewhere
//void collide1(void * client_data, DtObjectRef obj1, DtObjectRef obj2,
//      const DtCollData *coll_data) {
//}
//## end module%377944220020.additionalDeclarations


// Class CD_Solid 

//## begin CD_Solid::allIndexesGlobal%377D55C1011B.attr preserve=no  private: static std::set< unsigned int> {UA} 
std::set< unsigned int> CD_Solid::allIndexesGlobal;
//## end CD_Solid::allIndexesGlobal%377D55C1011B.attr


//## begin CD_Solid::firstUnusedIndex%377D574A0057.attr preserve=no  private: static unsigned int {UA} 0
unsigned int CD_Solid::firstUnusedIndex = 0;
//## end CD_Solid::firstUnusedIndex%377D574A0057.attr


//## begin CD_Solid::activeCollisionDetector%378115800299.attr preserve=no  private: static int {UA} -1
int CD_Solid::activeCollisionDetector = -1;
//## end CD_Solid::activeCollisionDetector%378115800299.attr

//## begin CD_Solid::maxCollisionDetector%378115A60276.attr preserve=no  private: static int {UA} -1
int CD_Solid::maxCollisionDetector = -1;
//## end CD_Solid::maxCollisionDetector%378115A60276.attr


//## begin CD_Solid::currentCollisionDetectorID%3781428500B1.attr preserve=no  private: static int {UA} -1
int CD_Solid::currentCollisionDetectorID = -1;
//## end CD_Solid::currentCollisionDetectorID%3781428500B1.attr



CD_Solid::CD_Solid (Universe& universe)
  //## begin CD_Solid::CD_Solid%930942586.hasinit preserve=no
  //## end CD_Solid::CD_Solid%930942586.hasinit
  //## begin CD_Solid::CD_Solid%930942586.initialization preserve=yes
  : 
        CollisionDetectorBase( universe ),
        CD_BasicStyle( universe ),
        CD_Linear( universe ),
        CD_LinearContinuous( universe ),
        collisionDetectorID( maxCollisionDetector + 1 )
  //## end CD_Solid::CD_Solid%930942586.initialization
{
  //## begin CD_Solid::CD_Solid%930942586.body preserve=yes
        maxCollisionDetector++ ;
        for( int j = 0; j < entities.size(); j++ )
        {
                entities[ j ]->SetFrameManager( &theFrameManager ) ;
                Entity* addme = entities[ j ] ;
//              Entity* cloned = entities[ j ]->Clone() ;
                CD_Bool::allGeometryUsed &= AddEntityToSolid( addme ) ;
        }
        //IMPROVE: this will cause memory leaks
  //## end CD_Solid::CD_Solid%930942586.body
}

CD_Solid::CD_Solid (const CD_Solid& right)
  //## begin CD_Solid::CD_Solid%930942587.hasinit preserve=no
  //## end CD_Solid::CD_Solid%930942587.hasinit
  //## begin CD_Solid::CD_Solid%930942587.initialization preserve=yes
  : 
        CollisionDetectorBase( right ),
        CD_BasicStyle( right ),
        CD_Linear( right ),
        CD_LinearContinuous( right )
  //## end CD_Solid::CD_Solid%930942587.initialization
{
  //## begin CD_Solid::CD_Solid%930942587.body preserve=yes
        assert( false );
  //## end CD_Solid::CD_Solid%930942587.body
}


CD_Solid::~CD_Solid()
{
/*  //## begin CD_Solid::~CD_Solid%.body preserve=yes
        
        //go through and delete all the objects that have been instanced
        for( int i = 0; i < shapeIDs.size(); i++ )
        {
                void* deleteMe = shapeIDs[ i ] ;
                dtDeleteShape( shapeIDs[ i ] );         
        }
        shapeIDs.clear() ;

        //remove the references to these objects from the global list
        for( i = 0; i < indexes.size(); i++ )
        {
                unsigned int eraseMe = indexes[ i ] ;
                allIndexesGlobal.erase( allIndexesGlobal.find( eraseMe ) ) ;
                dtDeleteObject( ( DtObjectRef )indexes[ i ] ) ;
        }
        indexes.clear() ;
        frameSolid.clear() ;

        currentCollisionDetectorID = -1 ;
  //## end CD_Solid::~CD_Solid%.body
  */
}



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

bool CD_Solid::IsInterfering (const Configuration& config)
{
/*  //## begin CD_Solid::IsInterfering%930942585.body preserve=yes
        SetConfiguration( config ) ;

        //go through all the frames 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() ; 

                void* index = ( void* )indexes[ i ] ;
                assert( index >= 0 ) ;

                //IMPROVE: find a way to do this via a funciton call
                dtSelectObject( index ) ;
                double matrix[ 16 ] ;
                for( int j = 0; j < 16; j++ )
                {
                        int r = j % 4 ;
                        int c = j / 4 ;
                        matrix[ j ] = m1( r, c ) ;
                }
                dtLoadMatrixd( ( const double* ) matrix ) ;
        }

        //check if this is the currently selected collision detector
        if( activeCollisionDetector != collisionDetectorID ) 
        {
                activeCollisionDetector = collisionDetectorID ;
        }
        SetCollidingPairs() ;

        int number = dtTest() ;
        if( number > 0 )
        {
                return true ;
        }
        return false ;
  //## end CD_Solid::IsInterfering%930942585.body
*/
        return false;
}

bool CD_Solid::AddEntityToSolid (const Entity* entity)
{
  //## begin CD_Solid::AddEntityToSolid%930942588.body preserve=yes

        //check if the entity is a mesh
        const Mesh* mesh = dynamic_cast< const Mesh* >( entity ) ;
        if( mesh != NULL )
        {
                AddMeshToSolid( *mesh ) ;
                return true ;
        }

        //check if the entity is a group
        const ObjectGroup* group = dynamic_cast< const ObjectGroup* >( entity ) ;
        if( group != NULL )
        {
                bool returnMe = true ;
                for( int i = 0; i < group->Size(); i++ )
                {
                        const Entity* addme = ( *group )[ i ] ;
                        returnMe &= AddEntityToSolid( addme ) ;
                }
                return returnMe ;
        }

        //check if the entity is a cylinder
        const MPK_Cylinder* cylinder = dynamic_cast< const MPK_Cylinder* >( entity ) ;
        if( cylinder != NULL )
        {
                int index = AddCylinderToSolid( cylinder ) ;
                //IMPROVE: check the response of AddCylinder...
                return true ;
        }

        //check if the entity is a box
        const MPK_Box* box = dynamic_cast< const MPK_Box* >( entity ) ;
        if( box != NULL )
        {
                //int index = AddCylinderToSolid( cylinder ) ;
                //IMPROVE: check the response of AddCylinder...
                return true ;
        }

        //check if the entity is a box
        const MPK_Sphere* sphere = dynamic_cast< const MPK_Sphere* >( entity ) ;
        if( sphere != NULL )
        {
                //int index = AddCylinderToSolid( cylinder ) ;
                //IMPROVE: check the response of AddCylinder...
                return true ;
        }

        //check if the entity is a robot - in which case do nothing
        const RobotBase* robot = dynamic_cast< const RobotBase* >( entity ) ;
        if( robot != NULL )
        {
                return true ;
        }

        //improve: need to mark down what frames all the vcollide indexes are in
        return( false ) ;

  //## end CD_Solid::AddEntityToSolid%930942588.body
}

int CD_Solid::AddMeshToSolid (const Mesh& mesh)
{
/*
        meshes.push_back( &mesh ) ;

        //create an array of doubles that list the xyz values of the vertexes
        int numberOfVertexes = mesh.vertexes.size() ;
        double* vertexList = new double[ numberOfVertexes * 3 ]  ;
        for( int i = 0; i < numberOfVertexes; i++ )
        {
                vertexList[ i * 3 + 0 ] = ( mesh.vertexes[ i ] )[ 0 ] ;
                vertexList[ i * 3 + 1 ] = ( mesh.vertexes[ i ] )[ 1 ] ;
                vertexList[ i * 3 + 2 ] = ( mesh.vertexes[ i ] )[ 2 ] ;
        }

        DtShapeRef shape = dtNewComplexShape();
        dtVertexBase( vertexList );
        int fsize = mesh.facets.size() ;
        for( int j = 0; j < fsize; j++ )
        {
                dtBegin( DT_POLYGON );
                Facet f = mesh.facets[ j ] ;
                int size = f.vertexNumbers.size() ;
                for( int k = 0; k < size; k++ )
                {
                        dtVertexIndex( f.vertexNumbers[ k ] ) ;
                }
                dtEnd();
        }
        dtEndComplexShape();

        unsigned int index = GetNewIndex() ;
        dtCreateObject( (void*) index, shape ) ;
        dtSelectObject( (void*) index ) ;
        shapeIDs.push_back( shape ) ;
        delete[] vertexList ;

        this->MarkIndexEntityRelationship( index, &mesh ) ;
        //update the fact that this mesh doesn't collide with anything
        return index ;
*/
        return false;
}

void CD_Solid::SetCollidingPairs () const
{
/*
  //## begin CD_Solid::SetCollidingPairs%931206292.body preserve=yes
        if( currentCollisionDetectorID == collisionDetectorID )
        {
                return ;
        }
        currentCollisionDetectorID = collisionDetectorID ;

        for( int i = 0; i < meshes.size(); i++ )
        {
                for( int j = i; j < meshes.size(); j++ )
                {
                        bool shouldCollide = true ;
                        unsigned int base1 = meshes[ i ]->BaseFrame() ;
                        unsigned int base2 = meshes[ j ]->BaseFrame() ;
                        //
                        if( base1 == base2 )                                                            shouldCollide = false ;
                        if( theFrameManager.BaseFrame( base1 ) == base2 )       shouldCollide = false ;
                        if( theFrameManager.BaseFrame( base2 ) == base1 )       shouldCollide = false ;

                        if( shouldCollide )
                        {
                                void* index1 = (void*) indexes[ i ] ;
                                void* index2 = (void*) indexes[ j ] ;
                                dtSetPairResponse
                                ( 
                                        index1, 
                                        index2, 
                                        collide1, 
                                        DT_SIMPLE_RESPONSE,
                                        stdout
                                ) ;
                        }
                }
        }
  //## end CD_Solid::SetCollidingPairs%931206292.body
  */
}

unsigned int CD_Solid::GetNewIndex ()
{
  //## begin CD_Solid::GetNewIndex%930961054.body preserve=yes
        unsigned int returnMe = firstUnusedIndex ;
        firstUnusedIndex ++ ;   //IMPROVE: Do we need to worry about running out of indexes? Maybe?
        return returnMe ;
  //## end CD_Solid::GetNewIndex%930961054.body
}

void CD_Solid::ActivateFrames (const unsigned int frame1, const unsigned int frame2)
{
/*
  //## begin CD_Solid::ActivateFrames%931291666.body preserve=yes
        if( !FramePairPermEnabled( frame1, frame2 ) )
        {
                return ;
        }
        CD_BasicStyle::ActivateFrames( frame1, frame2 ) ;
        std::set< unsigned int > s1 = frameSolid[ frame1 ] ;
        std::set< unsigned int > s2 = frameSolid[ frame2 ] ;
        std::set< unsigned int >::iterator i1 = s1.begin();             //IMPROVE: added the s1.begin()
        std::set< unsigned int >::iterator i2 = s2.begin();

        for( ;i1 != s1.end(); i1++ )
        {
                for( ;i2 != s2.end(); i2++ )
                {
                        dtSetPairResponse
                        ( 
                                (void*)*i1, 
                                (void*)*i2, 
                                collide1, 
                                DT_SIMPLE_RESPONSE,
                                stdout
                        ) ;             
                }
        }

  //## end CD_Solid::ActivateFrames%931291666.body
  */
}

void CD_Solid::DeactivateFrames (const unsigned int frame1, const unsigned int frame2)
{
        /*
  //## begin CD_Solid::DeactivateFrames%931291667.body preserve=yes
        CD_BasicStyle::DeactivateFrames( frame1, frame2 ) ;
        std::set< unsigned int > s1 = frameSolid[ frame1 ] ;
        std::set< unsigned int > s2 = frameSolid[ frame2 ] ;
        std::set< unsigned int >::iterator i1 = s1.begin() ;
        std::set< unsigned int >::iterator i2 = s2.begin() ;

        for( ;i1 != s1.end(); i1++ )
        {
                for( ;i2 != s2.end(); i2++ )
                {
                        dtClearPairResponse
                        ( 
                                (void*)*i1, 
                                (void*)*i2
                        ) ;             
                }
        }

  //## end CD_Solid::DeactivateFrames%931291667.body
  */
}

// Additional Declarations
  //## begin CD_Solid%377944220020.declarations preserve=yes
int CD_Solid::AddCylinderToSolid( const MPK_Cylinder* cylinder )
{
        /*
        DtShapeRef shape = dtCylinder( cylinder->Radius(), cylinder->Height() );
        unsigned int index = GetNewIndex() ;
        dtCreateObject( (void*) index, shape ) ;
        dtSelectObject( (void*) index ) ;
        shapeIDs.push_back( shape ) ;

        //assert( this should be in the calling function or alter the return valuel
        this->MarkIndexEntityRelationship( index, cylinder) ;
        return index ;
        */
        return 0;
}

void CD_Solid::MarkIndexEntityRelationship( int index, const Entity* entity )
{
        //update the fact that we just used a new index
        firstUnusedIndex = index + 1 ;
        allIndexesGlobal.insert( index ) ;
        indexes.push_back( index ) ;

        //update the index frame list
        int baseFrame = entity->BaseFrame() ;
        //check if we've seen a frame number this high before - IMPROVE: use a mapping?
        if( frameSolid.size() < baseFrame + 1 )
        {
                frameSolid.resize( baseFrame + 1 ) ;
        }
        frameSolid[ baseFrame ].insert( index ) ;
}

  //## end CD_Solid%377944220020.declarations
//## begin module%377944220020.epilog preserve=yes
//## end module%377944220020.epilog

---