collisiondetectors/CD_Swiftpp.cpp

Go to the documentation of this file.

// CD_Swiftpp.cpp

#include "robots/r_openchain.h"
#include "CollisionDetectors/CD_Swiftpp.h"
#include "geometry/Facet.h"
#include "geometry/ObjectGroup.h"
#include "Kinematics/LinkBase.h"
#include "Universe/universe.h"
#include "geometry/geo_rangesensor/Range_Sensor.h"
#include "synchronization/semaphore.h"

#include <direct.h> 
#include <sys/types.h>
#include <sys/stat.h>
#include "SWIFTpp/include/SWIFT.h"
#include "SWIFTpp/decomposer/include/gui.h"

static const double OBS_TOL             = 1e-3;     
static const char POLY[]        = "POLY\n\n";   // first word in .poly file
static const char POLYEXT[]     = ".poly";  
static const char HIEREXT[]     = ".chr";   // hierarchy file extension
static const char FILENAME[]    = "object"; // poly file names: object##.poly

// decomposer variables
bool jitter = false;
SWIFT_Real jampl = 0.0;
bool ef = false;
char* ef_filename = NULL;
SWIFT_Real edge_flip_tol = 0.0; 
bool dfs = false; 
bool bfs = false; 
bool w = false;
bool one_piece = false; 
char* decomp_filename = NULL; 
bool hierarchy = true; 
char* hier_filename = NULL; 
SPLIT_TYPE split = MIDPOINT; 


CD_Swiftpp::CD_Swiftpp (const Universe& universe)
: 
    m_pUniversePointer( NULL ),
        CollisionDetectorBase( universe ),
        CD_BasicStyle( universe ),
        CD_Linear( universe ),
        CD_LinearContinuous( universe ),
        m_dObsTol(OBS_TOL)
{
    m_pSwiftScene = new SWIFT_Scene;
    
    //go through all the entities that are in the entity list, 
    //and add all the meshes to the swift
        bool success = true ;
        for( int j = 0; j < entities.size(); j++ )
        {
                Entity* theEntity = entities[ j ] ;
                entities[ j ]->SetFrameManager( &theFrameManager ) ;
                success &= AddEntityToSwiftScene( 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() ;                     // CD_BasicStyle only
        PermDeactivateFramesWithThemselves() ;  // CD_BasicStyle only

    // deactivate objects within the same frame
    std::vector< LinkBase* > links = universe.GetAllLinks();
    int i;
    int size = links.size();
    for( i = 0; i < size; i++ )
    {
        LinkBase* link = links[ i ];
        //** why is this here?  int baseframe = link->BaseFrameNum();
        int controlledSize = link->controlledFrames.size();

        for(int j = 0; j < controlledSize; j++)
        {
            int controlledFrame = link->controlledFrames[ j ];
            DeactivateFrames( controlledFrame, controlledFrame );
        }
    }

        // deactivate frames with their parents
    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 );
        }
    }
}

CD_Swiftpp::CD_Swiftpp (const CD_Swiftpp& right)
    : m_pUniversePointer(NULL),
    CollisionDetectorBase( right ),
        CD_BasicStyle( right ),
        CD_Linear( right ),
        CD_LinearContinuous( right )
{
        //the entities will be recopied later on - the ones copied in CD_BasicStyle are deleted
        m_pSwiftScene = new SWIFT_Scene;
        
        m_vMeshes.reserve( right.m_vMeshes.size() ) ;
        for( int i = 0; i < right.m_vMeshes.size(); i++ )
        {
                int numMeshes = m_vMeshes.size() ;
                Mesh* addMe = right.m_vMeshes[ i ] ; 
                this->AddEntityToSwiftScene( addMe ) ;
        }
}

CD_Swiftpp::~CD_Swiftpp()
{
    int i;
        for( i = 0; i < m_vMeshes.size(); i++ )
        {
                Mesh* deleteMe = m_vMeshes[ i ] ;
                delete deleteMe ;
                m_vMeshes[ i ] = NULL ;
        }
        m_vMeshes.clear() ;

        for( i = 0; i < m_vSwiftppIndexes.size(); i++ )
        {
                m_pSwiftScene->Delete_Object( m_vSwiftppIndexes[ i ] ) ;
        }
        m_vSwiftppIndexes.clear() ;
        for( i = 0; i < m_vLinkSwiftpp.size(); i++ )
        {
                m_vLinkSwiftpp[ i ].clear() ;
        }
        m_vLinkSwiftpp.clear() ;

        delete m_pSwiftScene ;
}

CollisionDetectorBase* CD_Swiftpp::Clone () const
{
    CD_Swiftpp* newSP = new CD_Swiftpp( *this ) ;
        CD_InterfaceToCollisionQueries* CDitc = dynamic_cast< CD_Bool* >( newSP ) ;
        CollisionDetectorBase* returnMe = dynamic_cast< CollisionDetectorBase* >( CDitc );
        return returnMe ;
}
 
bool CD_Swiftpp::AddEntityToSwiftScene(const Entity* entity)
{
    int baseFrame = entity->BaseFrame() ;
        while( m_vLinkSwiftpp.size() < baseFrame + 1 )     
        {
                std::set< unsigned int > setOfSwiftppMeshes ;
                m_vLinkSwiftpp.push_back( setOfSwiftppMeshes );
        }

        const Mesh* mesh = dynamic_cast< const Mesh* >( entity ) ;
        if( mesh != NULL )
        {
                int SwiftppIndex = AddMeshToSwiftScene( *mesh, (baseFrame == 0)) ;  
                // if entity is in baseFrame 0, then it is fixed (obstacle); else moving
        if (SwiftppIndex != -1) {return true ;}     
        else {return false;}
        }
        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 &= AddEntityToSwiftScene( addme ) ;
                }
                return returnMe ;
        }

    const R_OpenChain* pRobot = dynamic_cast<R_OpenChain*>(entity->Clone());
    if (pRobot != NULL)
    {              
        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 m_pSwiftScene, only meshes
                return true ;
        }

        return false ;
}

int CD_Swiftpp::AddMeshToSwiftScene(const Mesh& mesh, const bool bFixed)
// even though writing to a .poly file, decomposing to a hierarchy file and then 
// adding it to the scene seems to be a lot more work than just using 
// Add_Convex_Object, this method will give greater flexibility in the meshes 
// allowed to be added to the scene (non-triangular faces, non-convex objects), 
// thus minimizing the amount of code changed in the future (hopefully!).
{
/*
1) create a new .poly file -> "object1.poly"
2) fwrite POLY on first line
3) fwrite number of points
4) fwrite number of faces
5) go through all the vertices' xyz's, fwrite them
6a) note the number of vertices making up the face
6b) go through the mesh/entity's faces' vertexNumbers
7) save file
8) decompose file - this allows the original shape to be non-convex
9) add object to the scene
10) push the object id into the vector in the same order as the mesh is stored
*/

        Mesh* addMe = dynamic_cast< Mesh*>( mesh.Clone() ) ;
    if (addMe == NULL)
    {
        return -1;
    }
        m_vMeshes.push_back( addMe ) ;

        int nObjectId = -1;     

    char sFileName[MAX_FILENAME_LENGTH], 
         sPolyFileName[MAX_FILENAME_LENGTH],
                 sHierFileName[MAX_FILENAME_LENGTH],
                 sCurDir[MAX_FILENAME_LENGTH];
         
        if (_getcwd(sCurDir, MAX_FILENAME_LENGTH) == NULL)
        {
                return nObjectId;
        }
    
        sprintf(sFileName, "%s\\%s", sCurDir, FILENAME);
        int length = sprintf(sPolyFileName, "%s%s", sFileName, POLYEXT);
    if (length < MIN_FILENAME_LENGTH)   // sprintf didn't concat properly
    {
        return nObjectId;
    }

    FILE* pFile = fopen(sPolyFileName, "w");
    if (pFile == NULL)
    {
        return nObjectId;
    }

    int nWritten = fwrite(POLY, sizeof(char), sizeof(POLY)-1, pFile);
    if (nWritten < (sizeof(POLY)-1))
    {
        assert(fflush(pFile) == 0);
        assert(fclose(pFile) == 0);
        return nObjectId;
    }

    int nVertices = addMe->vertexes.size();
    int nFaces = addMe->facets.size();

    length = fprintf(pFile, "%d\n\n%d\n\n", nVertices, nFaces);
    if (length < 6)         // minimum of 6 characters written
    {
        assert(fflush(pFile) == 0);
        assert(fclose(pFile) == 0);
        return nObjectId;
    }
    
    // write vertices info
    for (int i = 0; i < nVertices; i++)
    {
        length = fprintf(pFile, "%f %f %f\n", 
            (addMe->vertexes)[i][0],
            (addMe->vertexes)[i][1],
            (addMe->vertexes)[i][2]);
        if (length < 5)     // minimum of 5 characters written
        {
            assert(fflush(pFile) == 0);
            assert(fclose(pFile) == 0);
            return nObjectId;
        }
    }

    // write face info
    int vn;
    for (i = 0; i < nFaces; i++)
    {
        vn = (addMe->facets)[i].vertexNumbers.size();
        length = fprintf(pFile, "\n%d ", vn);
        for (int j = 0; j < vn; j++)
        {
            length += fprintf(pFile, "%d ", (addMe->facets)[i].vertexNumbers[j]);            
        }

        if (length < 7)
        {
            assert(fflush(pFile) == 0);
            assert(fclose(pFile) == 0);
            return nObjectId;
        }
    }  

    // done writing .poly file so close it
    assert(fflush(pFile) == 0);
    assert(fclose(pFile) == 0);

    // decompose file
    sprintf( sHierFileName, "%s%s", sFileName, HIEREXT);
    
    hier_filename = new char[strlen(sHierFileName)+1];
        strcpy(hier_filename, sHierFileName);

    SWIFT_Scene scene;
    
    if (Gui_Init_Before_TclTk(sPolyFileName) == false) 
    {return nObjectId;}

        // make sure the hierarchy file is newer than the poly file 
        struct _stat bufPoly, bufHier;
    int result = _stat(sPolyFileName, &bufPoly);
    if (result != 0){return nObjectId;}
    result = _stat(sHierFileName, &bufHier);
    if (result != 0){return nObjectId;}
    // .poly file has later time stamp
    if (bufPoly.st_mtime > bufHier.st_mtime){return nObjectId;}  
        
    // add hierarchy file to scene
    if( m_pSwiftScene->Add_General_Object( sHierFileName, nObjectId, bFixed,
                        DEFAULT_ORIENTATION, DEFAULT_TRANSLATION, DEFAULT_SCALE, 
                        DEFAULT_SPLIT_TYPE, BOX_SETTING_CHOOSE, 
                        DEFAULT_BOX_ENLARGE_REL, m_dObsTol) == false) 
    {
        return -1;
    } 
    
        //need to update the list of baseframe - mesh references
    int baseFrame = mesh.BaseFrame() ;
        std::set< unsigned int > theSet = m_vLinkSwiftpp[ baseFrame ] ;
        theSet.insert( nObjectId ) ;
        m_vLinkSwiftpp[ baseFrame ] = theSet ;  
        
        m_vSwiftppIndexes.push_back(nObjectId) ;

        return nObjectId;

}

void CD_Swiftpp::DeactivateFrames (const unsigned int frame1, const int frame2)
{
    if( frame2 > frame1 )
        {
                DeactivateFrames( frame2, frame1 ) ;
                return ;
        }
        if (frame2 != -1)
        {
                CD_BasicStyle::DeactivateFrames( frame1, frame2 ) ;     
        }
        int lSize = m_vLinkSwiftpp.size() ;

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

        std::set< unsigned int >::iterator i1 ;
        std::set< unsigned int >::iterator i2 ;

        for( i1 = m_vLinkSwiftpp[ frame1 ].begin(); i1 != m_vLinkSwiftpp[ frame1 ].end() ; i1++ )
        {
                int index1 = *i1 ;
        if (frame2 == -1)
        {
            m_pSwiftScene->Deactivate(index1);
        }
        else
        {
                    for( i2 = m_vLinkSwiftpp[ frame2 ].begin(); i2 != m_vLinkSwiftpp[ frame2 ].end(); i2++ )
                    {
                            int index2 = *i2 ;
                            if( index1 != index2 )
                            {
                                    m_pSwiftScene->Deactivate( index1, index2 ) ;
                                    m_pSwiftScene->Deactivate( index2, index1 ) ;                               
                            }
                    }
        }
        }
}


void CD_Swiftpp::ActivateFrames (const unsigned int frame1, const int frame2)
{
    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 ;
        }

        if (frame2 != -1)
        {
                CD_BasicStyle::ActivateFrames( frame1, frame2 ) ;
        }
        int lSize = m_vLinkSwiftpp.size() ;

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

        std::set< unsigned int >::iterator i1 ;
        std::set< unsigned int >::iterator i2 ;

        for( i1 = m_vLinkSwiftpp[ frame1 ].begin(); i1 != m_vLinkSwiftpp[ frame1 ].end() ; i1++ )
        {
                int index1 = *i1 ;
        if (frame2 == -1)
        {
            m_pSwiftScene->Activate(index1);
        }
        else
        {
                    for( i2 = m_vLinkSwiftpp[ frame2 ].begin(); i2 != m_vLinkSwiftpp[ frame2 ].end(); i2++ )
                    {
                            int index2 = *i2 ;
                            if( index1 != index2 )
                            {
                                    m_pSwiftScene->Activate( index1, index2 ) ;
                                    m_pSwiftScene->Activate( index2, index1 ) ;
                            }
                    }
        }
        }
}

/*
bool CD_Swiftpp::QueryIntersection(bool bEarlyExit, int& num_pairs, int** oids)
{
    bool bReturn;
    return bReturn;
}*/

bool CD_Swiftpp::QueryContactDetermination(const Configuration& config, bool bEarlyExit, 
    SWIFT_Real tolerance, int& num_pairs, int** oids, int** num_contacts, 
    SWIFT_Real** distances,  SWIFT_Real** nearest_pts, SWIFT_Real** normals)
{
    
        CD_Bool::IncrementCallCount();

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

    // go through all the links and look at the meshes that are there to update their frames
    // SWIFT++ uses an array of size 12 to represent the R and T of transform matrix (row major).
    double RT[12];
    int nObjectId; 
        for( int i = 0; i < m_vMeshes.size(); i++ )
        {
                const Mesh* mesh = m_vMeshes[ i ] ;

                Matrix4x4 m1 = mesh->GetTransform(); 

        for (int j = 0; j < 3; j++)
        {
            for (int k = 0; k < 4; k++)
            {
                RT[4*j+k] = m1(j,k);
            }
        }

        nObjectId = m_vSwiftppIndexes[i];
                assert( nObjectId >= 0 ) ;

                m_pSwiftScene->Set_Object_Transformation(nObjectId, RT);
        }

    bool bIntersect = m_pSwiftScene->Query_Intersection(bEarlyExit, num_pairs, oids);
    
    if ((bEarlyExit != false) && (bIntersect != false))     
        // user only wants to know if there's an intersection
    {
        semaphore.Unlock();
        return true;
    }
    
    bIntersect = m_pSwiftScene->Query_Contact_Determination(bEarlyExit, tolerance, 
        num_pairs, oids, num_contacts, distances, nearest_pts, normals);
    
    semaphore.Unlock();
    return bIntersect;
}

bool CD_Swiftpp::IsInterfering (const Configuration& config)
{
    int np, *oids;
    
    CD_Bool::IncrementCallCount();

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

    // go through all the links and look at the meshes that are there to update their frames
    // SWIFT++ uses an array of size 12 to represent the R and T of transform matrix (row major).
    double RT[12];
    int nObjectId;     
    Matrix4x4 m1;
        for( int i = 0; i < m_vMeshes.size(); i++ )
        {
                const Mesh* mesh = m_vMeshes[ i ] ;

                m1 = mesh->GetTransform(); 

        for (int j = 0; j < 3; j++)
        {
            for (int k = 0; k < 4; k++)
            {
                RT[4*j+k] = m1(j,k);
            }
        }

        nObjectId = m_vSwiftppIndexes[i];
                assert( nObjectId >= 0 ) ;

                m_pSwiftScene->Set_Object_Transformation(nObjectId, RT);
        }

    bool bIntersect = m_pSwiftScene->Query_Tolerance_Verification(true, m_dObsTol, np, &oids);
    
    semaphore.Unlock();

    return bIntersect;
}

int CD_Swiftpp::GetRobotLinkBaseFrame(int objectId)
{
    std::set< unsigned int >::iterator it = m_vLinkSwiftpp[0].begin();
    for (int i = 0; i < m_vLinkSwiftpp.size(); i++)
    {
        it = m_vLinkSwiftpp[i].find(objectId);
        if (it != m_vLinkSwiftpp[i].end())
        {
            return i;
        }
    }    
    return -1;
}

---