bool EpaPolyhedron::Create( btPoint3* pInitialPoints,
btPoint3* pSupportPointsOnA, btPoint3* pSupportPointsOnB,
const int nbInitialPoints )
{
#ifndef EPA_POLYHEDRON_USE_PLANES
EPA_DEBUG_ASSERT( ( nbInitialPoints <= 4 ) ,"nbInitialPoints greater than 4!" );
#endif
if ( nbInitialPoints < 4 )
{
// Insufficient nb of points
return false;
}
////////////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES
int nbDiffCoords[ 3 ] = { 0, 0, 0 };
bool* pDiffCoords = new bool[ 3 * nbInitialPoints ];
int i;
for (i=0;i<nbInitialPoints*3;i++)
{
pDiffCoords[i] = false;
}
//::memset( pDiffCoords, 0, sizeof( bool ) * 3 * nbInitialPoints );
int axis;
for ( axis = 0; axis < 3; ++axis )
{
for ( int i = 0; i < nbInitialPoints; ++i )
{
bool isDifferent = true;
for ( int j = 0; j < i; ++j )
{
if ( pInitialPoints[ i ][ axis ] == pInitialPoints[ j ][ axis ] )
{
isDifferent = false;
break;
}
}
if ( isDifferent )
{
++nbDiffCoords[ axis ];
pDiffCoords[ axis * nbInitialPoints + i ] = true;
}
}
if ( nbDiffCoords[ axis ] <= 1 )
{
// The input is degenerate
return false;
}
}
int finalPointsIndices[ 4 ] = { -1, -1, -1, -1 };
int axisOrderIndices[ 3 ] = { 0, 1, 2 };
for ( i = 0; i < 2/*round( nbAxis / 2 )*/; ++i )
{
if ( nbDiffCoords[ i ] > nbDiffCoords[ i + 1 ] )
{
int tmp = nbDiffCoords[ i ];
nbDiffCoords[ i ] = nbDiffCoords[ i + 1 ];
nbDiffCoords[ i + 1 ] = tmp;
tmp = axisOrderIndices[ i ];
axisOrderIndices[ i ] = axisOrderIndices[ i + 1 ];
axisOrderIndices[ i + 1 ] = tmp;
}
}
int nbSuccessfullAxis = 0;
// The axes with less different coordinates choose first
int minsIndices[ 3 ] = { -1, -1, -1 };
int maxsIndices[ 3 ] = { -1, -1, -1 };
int finalPointsIndex = 0;
for ( axis = 0; ( axis < 3 ) && ( nbSuccessfullAxis < 2 ); ++axis )
{
int axisIndex = axisOrderIndices[ axis ];
btScalar axisMin = SIMD_INFINITY;
btScalar axisMax = -SIMD_INFINITY;
for ( int i = 0; i < 4; ++i )
{
// Among the diff coords pick the min and max coords
if ( pDiffCoords[ axisIndex * nbInitialPoints + i ] )
{
if ( pInitialPoints[ i ][ axisIndex ] < axisMin )
{
axisMin = pInitialPoints[ i ][ axisIndex ];
minsIndices[ axisIndex ] = i;
}
if ( pInitialPoints[ i ][ axisIndex ] > axisMax )
{
axisMax = pInitialPoints[ i ][ axisIndex ];
maxsIndices[ axisIndex ] = i;
}
}
}
//assert( ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) &&
// "min and max have the same index!" );
if ( ( minsIndices[ axisIndex ] != -1 ) && ( maxsIndices[ axisIndex ] != -1 ) &&
( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) )
{
++nbSuccessfullAxis;
finalPointsIndices[ finalPointsIndex++ ] = minsIndices[ axisIndex ];
finalPointsIndices[ finalPointsIndex++ ] = maxsIndices[ axisIndex ];
// Make the choosen points to be impossible for other axes to choose
//assert( ( minsIndices[ axisIndex ] != -1 ) && "Invalid index!" );
//assert( ( maxsIndices[ axisIndex ] != -1 ) && "Invalid index!" );
for ( int i = 0; i < 3; ++i )
{
pDiffCoords[ i * nbInitialPoints + minsIndices[ axisIndex ] ] = false;
pDiffCoords[ i * nbInitialPoints + maxsIndices[ axisIndex ] ] = false;
}
}
}
if ( nbSuccessfullAxis <= 1 )
{
// Degenerate input ?
EPA_DEBUG_ASSERT( false ,"nbSuccessfullAxis must be greater than 1!" );
return false;
}
delete[] pDiffCoords;
#endif
//////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES
btVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
btVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
#else
btVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
btVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
#endif
btVector3 planeNormal = v1.cross( v0 );
planeNormal.normalize();
#ifdef EPA_POLYHEDRON_USE_PLANES
btScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
#else
btScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
#endif
#ifdef EPA_POLYHEDRON_USE_PLANES
bool pointOnPlane0 = btEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane0)
{
EPA_DEBUG_ASSERT(0,"Point0 should be on plane!");
return false;
}
bool pointOnPlane1 = btEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane1)
{
EPA_DEBUG_ASSERT(0,"Point1 should be on plane!");
return false;
}
bool pointOnPlane2 = btEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane2)
{
EPA_DEBUG_ASSERT(0,"Point2 should be on plane!");
return false;
}
#endif
#ifndef EPA_POLYHEDRON_USE_PLANES
{
if ( planeDistance > 0 )
{
btVector3 tmp = pInitialPoints[ 1 ];
pInitialPoints[ 1 ] = pInitialPoints[ 2 ];
pInitialPoints[ 2 ] = tmp;
tmp = pSupportPointsOnA[ 1 ];
pSupportPointsOnA[ 1 ] = pSupportPointsOnA[ 2 ];
pSupportPointsOnA[ 2 ] = tmp;
tmp = pSupportPointsOnB[ 1 ];
pSupportPointsOnB[ 1 ] = pSupportPointsOnB[ 2 ];
pSupportPointsOnB[ 2 ] = tmp;
}
}
EpaVertex* pVertexA = CreateVertex( pInitialPoints[ 0 ], pSupportPointsOnA[ 0 ], pSupportPointsOnB[ 0 ] );
EpaVertex* pVertexB = CreateVertex( pInitialPoints[ 1 ], pSupportPointsOnA[ 1 ], pSupportPointsOnB[ 1 ] );
EpaVertex* pVertexC = CreateVertex( pInitialPoints[ 2 ], pSupportPointsOnA[ 2 ], pSupportPointsOnB[ 2 ] );
EpaVertex* pVertexD = CreateVertex( pInitialPoints[ 3 ], pSupportPointsOnA[ 3 ], pSupportPointsOnB[ 3 ] );
#else
finalPointsIndices[ 3 ] = -1;
btScalar absMaxDist = -SIMD_INFINITY;
btScalar maxDist;
for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex )
{
btScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance;
btScalar absDist = abs( dist );
if ( ( absDist > absMaxDist ) &&
!btEqual( dist, PLANE_THICKNESS ) )
{
absMaxDist = absDist;
maxDist = dist;
finalPointsIndices[ 3 ] = pointIndex;
}
}
if ( finalPointsIndices[ 3 ] == -1 )
{
Destroy();
return false;
}
if ( maxDist > PLANE_THICKNESS )
{
// Can swap indices only
btPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ];
pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ];
pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp;
tmp = pSupportPointsOnA[ finalPointsIndices[ 1 ] ];
pSupportPointsOnA[ finalPointsIndices[ 1 ] ] = pSupportPointsOnA[ finalPointsIndices[ 2 ] ];
pSupportPointsOnA[ finalPointsIndices[ 2 ] ] = tmp;
tmp = pSupportPointsOnB[ finalPointsIndices[ 1 ] ];
pSupportPointsOnB[ finalPointsIndices[ 1 ] ] = pSupportPointsOnB[ finalPointsIndices[ 2 ] ];
pSupportPointsOnB[ finalPointsIndices[ 2 ] ] = tmp;
}
EpaVertex* pVertexA = CreateVertex( pInitialPoints[ finalPointsIndices[ 0 ] ], pSupportPointsOnA[ finalPointsIndices[ 0 ] ], pSupportPointsOnB[ finalPointsIndices[ 0 ] ] );
EpaVertex* pVertexB = CreateVertex( pInitialPoints[ finalPointsIndices[ 1 ] ], pSupportPointsOnA[ finalPointsIndices[ 1 ] ], pSupportPointsOnB[ finalPointsIndices[ 1 ] ] );
EpaVertex* pVertexC = CreateVertex( pInitialPoints[ finalPointsIndices[ 2 ] ], pSupportPointsOnA[ finalPointsIndices[ 2 ] ], pSupportPointsOnB[ finalPointsIndices[ 2 ] ] );
EpaVertex* pVertexD = CreateVertex( pInitialPoints[ finalPointsIndices[ 3 ] ], pSupportPointsOnA[ finalPointsIndices[ 3 ] ], pSupportPointsOnB[ finalPointsIndices[ 3 ] ] );
#endif
EpaFace* pFaceA = CreateFace();
EpaFace* pFaceB = CreateFace();
EpaFace* pFaceC = CreateFace();
EpaFace* pFaceD = CreateFace();
EpaHalfEdge* pFaceAHalfEdges[ 3 ];
EpaHalfEdge* pFaceCHalfEdges[ 3 ];
EpaHalfEdge* pFaceBHalfEdges[ 3 ];
EpaHalfEdge* pFaceDHalfEdges[ 3 ];
pFaceAHalfEdges[ 0 ] = CreateHalfEdge();
pFaceAHalfEdges[ 1 ] = CreateHalfEdge();
pFaceAHalfEdges[ 2 ] = CreateHalfEdge();
pFaceBHalfEdges[ 0 ] = CreateHalfEdge();
pFaceBHalfEdges[ 1 ] = CreateHalfEdge();
pFaceBHalfEdges[ 2 ] = CreateHalfEdge();
pFaceCHalfEdges[ 0 ] = CreateHalfEdge();
pFaceCHalfEdges[ 1 ] = CreateHalfEdge();
pFaceCHalfEdges[ 2 ] = CreateHalfEdge();
pFaceDHalfEdges[ 0 ] = CreateHalfEdge();
pFaceDHalfEdges[ 1 ] = CreateHalfEdge();
pFaceDHalfEdges[ 2 ] = CreateHalfEdge();
pFaceA->m_pHalfEdge = pFaceAHalfEdges[ 0 ];
pFaceB->m_pHalfEdge = pFaceBHalfEdges[ 0 ];
pFaceC->m_pHalfEdge = pFaceCHalfEdges[ 0 ];
pFaceD->m_pHalfEdge = pFaceDHalfEdges[ 0 ];
pFaceAHalfEdges[ 0 ]->m_pNextCCW = pFaceAHalfEdges[ 1 ];
pFaceAHalfEdges[ 1 ]->m_pNextCCW = pFaceAHalfEdges[ 2 ];
pFaceAHalfEdges[ 2 ]->m_pNextCCW = pFaceAHalfEdges[ 0 ];
pFaceBHalfEdges[ 0 ]->m_pNextCCW = pFaceBHalfEdges[ 1 ];
pFaceBHalfEdges[ 1 ]->m_pNextCCW = pFaceBHalfEdges[ 2 ];
pFaceBHalfEdges[ 2 ]->m_pNextCCW = pFaceBHalfEdges[ 0 ];
pFaceCHalfEdges[ 0 ]->m_pNextCCW = pFaceCHalfEdges[ 1 ];
pFaceCHalfEdges[ 1 ]->m_pNextCCW = pFaceCHalfEdges[ 2 ];
pFaceCHalfEdges[ 2 ]->m_pNextCCW = pFaceCHalfEdges[ 0 ];
pFaceDHalfEdges[ 0 ]->m_pNextCCW = pFaceDHalfEdges[ 1 ];
pFaceDHalfEdges[ 1 ]->m_pNextCCW = pFaceDHalfEdges[ 2 ];
pFaceDHalfEdges[ 2 ]->m_pNextCCW = pFaceDHalfEdges[ 0 ];
pFaceAHalfEdges[ 0 ]->m_pFace = pFaceA;
pFaceAHalfEdges[ 1 ]->m_pFace = pFaceA;
pFaceAHalfEdges[ 2 ]->m_pFace = pFaceA;
pFaceBHalfEdges[ 0 ]->m_pFace = pFaceB;
pFaceBHalfEdges[ 1 ]->m_pFace = pFaceB;
pFaceBHalfEdges[ 2 ]->m_pFace = pFaceB;
pFaceCHalfEdges[ 0 ]->m_pFace = pFaceC;
pFaceCHalfEdges[ 1 ]->m_pFace = pFaceC;
pFaceCHalfEdges[ 2 ]->m_pFace = pFaceC;
pFaceDHalfEdges[ 0 ]->m_pFace = pFaceD;
pFaceDHalfEdges[ 1 ]->m_pFace = pFaceD;
pFaceDHalfEdges[ 2 ]->m_pFace = pFaceD;
pFaceAHalfEdges[ 0 ]->m_pVertex = pVertexA;
pFaceAHalfEdges[ 1 ]->m_pVertex = pVertexB;
pFaceAHalfEdges[ 2 ]->m_pVertex = pVertexC;
pFaceBHalfEdges[ 0 ]->m_pVertex = pVertexB;
pFaceBHalfEdges[ 1 ]->m_pVertex = pVertexD;
pFaceBHalfEdges[ 2 ]->m_pVertex = pVertexC;
pFaceCHalfEdges[ 0 ]->m_pVertex = pVertexD;
pFaceCHalfEdges[ 1 ]->m_pVertex = pVertexA;
pFaceCHalfEdges[ 2 ]->m_pVertex = pVertexC;
pFaceDHalfEdges[ 0 ]->m_pVertex = pVertexB;
pFaceDHalfEdges[ 1 ]->m_pVertex = pVertexA;
pFaceDHalfEdges[ 2 ]->m_pVertex = pVertexD;
//pVertexA->m_pHalfEdge = pFaceAHalfEdges[ 0 ];
//pVertexB->m_pHalfEdge = pFaceAHalfEdges[ 1 ];
//pVertexC->m_pHalfEdge = pFaceAHalfEdges[ 2 ];
//pVertexD->m_pHalfEdge = pFaceBHalfEdges[ 1 ];
pFaceAHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 0 ];
pFaceAHalfEdges[ 1 ]->m_pTwin = pFaceBHalfEdges[ 2 ];
pFaceAHalfEdges[ 2 ]->m_pTwin = pFaceCHalfEdges[ 1 ];
pFaceBHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 2 ];
pFaceBHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 2 ];
pFaceBHalfEdges[ 2 ]->m_pTwin = pFaceAHalfEdges[ 1 ];
pFaceCHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 1 ];
pFaceCHalfEdges[ 1 ]->m_pTwin = pFaceAHalfEdges[ 2 ];
pFaceCHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 1 ];
pFaceDHalfEdges[ 0 ]->m_pTwin = pFaceAHalfEdges[ 0 ];
pFaceDHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 0 ];
pFaceDHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 0 ];
if ( !pFaceA->Initialize() || !pFaceB->Initialize() ||
!pFaceC->Initialize() || !pFaceD->Initialize() )
{
EPA_DEBUG_ASSERT( false, "One initial face failed to initialize!" );
return false;
}
#ifdef EPA_POLYHEDRON_USE_PLANES
if ( nbInitialPoints > 4 )
{
for ( int i = 0; i < nbInitialPoints; ++i )
{
if ( ( i != finalPointsIndices[ 0 ] ) && ( i != finalPointsIndices[ 1 ] ) &&
( i != finalPointsIndices[ 2 ] ) && ( i != finalPointsIndices[ 3 ] ) )
{
std::list< EpaFace* >::iterator facesItr( m_faces.begin() );
while ( facesItr != m_faces.end() )
{
EpaFace* pFace = *facesItr;
btScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance;
if ( dist > PLANE_THICKNESS )
{
std::list< EpaFace* > newFaces;
bool expandOk = Expand( pInitialPoints[ i ], pSupportPointsOnA[ i ], pSupportPointsOnB[ i ],
pFace, newFaces );
if ( !expandOk )
{
// One or more new faces are affinely dependent
return false;
}
EPA_DEBUG_ASSERT( !newFaces.empty() ,"Polyhedron should have expanded!" );
break;
}
++facesItr;
}
}
}
}
#endif
return true;
}
bool EpaPolyhedron::_dbgSaveToFile( const char* pFileName )
{
FILE* fp = NULL;
if ( fopen_s( &fp, pFileName, "wb" ) != 0 )
{
return false;
}
unsigned long int nbBytesWritten;
unsigned long int byteIndex = 0;
unsigned long int fileID = 0xBADC0DE;
fwrite( &fileID, sizeof( fileID ), 1, fp );
nbBytesWritten = sizeof( fileID );
byteIndex += nbBytesWritten;
unsigned char reservedByte = 0;
fwrite( &reservedByte, sizeof( reservedByte ), 1, fp );
nbBytesWritten = sizeof( reservedByte );
byteIndex += nbBytesWritten;
fwrite( &reservedByte, sizeof( reservedByte ), 1, fp );
nbBytesWritten = sizeof( reservedByte );
byteIndex += nbBytesWritten;
fwrite( &reservedByte, sizeof( reservedByte ), 1, fp );
nbBytesWritten = sizeof( reservedByte );
byteIndex += nbBytesWritten;
fwrite( &reservedByte, sizeof( reservedByte ), 1, fp );
nbBytesWritten = sizeof( reservedByte );
byteIndex += nbBytesWritten;
fwrite( &reservedByte, sizeof( reservedByte ), 1, fp );
nbBytesWritten = sizeof( reservedByte );
byteIndex += nbBytesWritten;
fwrite( &reservedByte, sizeof( reservedByte ), 1, fp );
nbBytesWritten = sizeof( reservedByte );
byteIndex += nbBytesWritten;
unsigned char stringSize = 5;
fwrite( &stringSize, sizeof( stringSize ), 1, fp );
nbBytesWritten = sizeof( stringSize );
byteIndex += nbBytesWritten;
char exportedFrom[ 6 ] = "01234";
fwrite( exportedFrom, stringSize * sizeof( char ), 1, fp );
nbBytesWritten = stringSize * sizeof( char );
byteIndex += nbBytesWritten;
unsigned short int w = 0;
fwrite( &w, sizeof( w ), 1, fp );
nbBytesWritten = sizeof( w );
byteIndex += nbBytesWritten;
fwrite( &w, sizeof( w ), 1, fp );
nbBytesWritten = sizeof( w );
byteIndex += nbBytesWritten;
fwrite( &w, sizeof( w ), 1, fp );
nbBytesWritten = sizeof( w );
byteIndex += nbBytesWritten;
fwrite( &w, sizeof( w ), 1, fp );
nbBytesWritten = sizeof( w );
byteIndex += nbBytesWritten;
fwrite( &w, sizeof( w ), 1, fp );
nbBytesWritten = sizeof( w );
byteIndex += nbBytesWritten;
fwrite( &w, sizeof( w ), 1, fp );
nbBytesWritten = sizeof( w );
byteIndex += nbBytesWritten;
unsigned long int geometryOffsetAtByteNb = byteIndex;
unsigned long int geometryOffset = 0;
unsigned long int geometrySize = 0;
fseek( fp, sizeof( geometryOffset ) + sizeof( geometrySize ), SEEK_CUR );
byteIndex += sizeof( geometryOffset ) + sizeof( geometrySize );
unsigned long int mappingOffset = 0;
unsigned long int mappingSize = 0;
fwrite( &mappingOffset, sizeof( mappingOffset ), 1, fp );
nbBytesWritten = sizeof( mappingOffset );
byteIndex += nbBytesWritten;
fwrite( &mappingSize, sizeof( mappingSize ), 1, fp );
nbBytesWritten = sizeof( mappingSize );
byteIndex += nbBytesWritten;
unsigned long int animationOffset = 0;
unsigned long int animationSize = 0;
fwrite( &animationOffset, sizeof( animationOffset ), 1, fp );
nbBytesWritten = sizeof( animationOffset );
byteIndex += nbBytesWritten;
fwrite( &animationSize, sizeof( animationSize ), 1, fp );
nbBytesWritten = sizeof( animationSize );
byteIndex += nbBytesWritten;
unsigned long int reservedDword = 0;
fwrite( &reservedDword, sizeof( reservedDword ), 1, fp );
nbBytesWritten = sizeof( reservedDword );
byteIndex += nbBytesWritten;
fwrite( &reservedDword, sizeof( reservedDword ), 1, fp );
nbBytesWritten = sizeof( reservedDword );
byteIndex += nbBytesWritten;
geometryOffset = byteIndex;
unsigned short int nbMeshs = 1;
fwrite( &nbMeshs, sizeof( nbMeshs ), 1, fp );
nbBytesWritten = sizeof( nbMeshs );
byteIndex += nbBytesWritten;
char meshName[] = "noname mesh";
unsigned char meshNameSize = strlen( meshName );
fwrite( &meshNameSize, sizeof( meshNameSize ), 1, fp );
nbBytesWritten = sizeof( meshNameSize );
byteIndex += nbBytesWritten;
fwrite( meshName, meshNameSize * sizeof( char ), 1, fp );
nbBytesWritten = meshNameSize * sizeof( char );
byteIndex += nbBytesWritten;
stdext::hash_map< unsigned long int, int > verticesMap;
typedef std::pair< unsigned long int, int > PR;
int vertexIndex = 0;
// Hash only vertices from faces that are not deleted
std::list< EpaFace* >::iterator facesItr( m_faces.begin() );
int nbFaces = 0;
while ( facesItr != m_faces.end() )
{
EpaFace* pFace = *facesItr;
if ( !pFace->m_deleted )
{
stdext::hash_map< unsigned long int, int >::const_iterator vertexItr;
vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 0 ] );
if ( vertexItr == verticesMap.end() )
{
verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 0 ], vertexIndex ) );
++vertexIndex;
}
vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 1 ] );
if ( vertexItr == verticesMap.end() )
{
verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 1 ], vertexIndex ) );
++vertexIndex;
}
vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 2 ] );
if ( vertexItr == verticesMap.end() )
{
verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 2 ], vertexIndex ) );
++vertexIndex;
}
++nbFaces;
}
++facesItr;
}
unsigned long int nbVertices = verticesMap.size();
fwrite( &nbVertices, sizeof( nbVertices ), 1, fp );
nbBytesWritten = sizeof( nbVertices );
byteIndex += nbBytesWritten;
// Collect all safe vertices
float* pVertices = new float[ verticesMap.size() * 3 ];
stdext::hash_map< unsigned long int, int >::iterator verticesItr( verticesMap.begin() );
while ( verticesItr != verticesMap.end() )
{
stdext::hash_map< unsigned long int, int >::const_iterator vertexItr;
PR pr = *verticesItr;
vertexItr = verticesMap.find( pr.first );
assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" );
EpaVertex* pVertex = ( EpaVertex* ) vertexItr->first;
pVertices[ vertexItr->second * 3 ] = pVertex->m_point.x();
pVertices[ vertexItr->second * 3 + 1 ] = pVertex->m_point.y();
pVertices[ vertexItr->second * 3 + 2 ] = pVertex->m_point.z();
++verticesItr;
}
unsigned long int* pIndices = new unsigned long int[ nbFaces * 3 ];
facesItr = m_faces.begin();
int facesIndex = 0;
while ( facesItr != m_faces.end() )
{
EpaFace* pFace = *facesItr;
if ( !pFace->m_deleted )
{
stdext::hash_map< unsigned long int, int >::const_iterator vertexItr;
int verticesIndices[ 3 ];
vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 0 ] );
assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" );
verticesIndices[ 0 ] = vertexItr->second;
vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 1 ] );
assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" );
verticesIndices[ 1 ] = vertexItr->second;
vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 2 ] );
assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" );
verticesIndices[ 2 ] = vertexItr->second;
pIndices[ facesIndex * 3 ] = verticesIndices[ 0 ];
pIndices[ facesIndex * 3 + 1 ] = verticesIndices[ 1 ];
pIndices[ facesIndex * 3 + 2 ] = verticesIndices[ 2 ];
++facesIndex;
}
++facesItr;
}
fwrite( &nbFaces, sizeof( nbFaces ), 1, fp );
nbBytesWritten = sizeof( nbFaces );
byteIndex += nbBytesWritten;
bool hasSmoothingGroups = false;
fwrite( &hasSmoothingGroups, sizeof( hasSmoothingGroups ), 1, fp );
nbBytesWritten = sizeof( hasSmoothingGroups );
byteIndex += nbBytesWritten;
fwrite( pVertices, verticesMap.size() * 3 * sizeof( float ), 1, fp );
nbBytesWritten = verticesMap.size() * 3 * sizeof( float );
byteIndex += nbBytesWritten;
// write indices
fwrite( pIndices, nbFaces * 3 * sizeof( unsigned long int ), 1, fp );
nbBytesWritten = nbFaces * 3 * sizeof( unsigned long int );
byteIndex += nbBytesWritten;
delete[] pVertices;
delete[] pIndices;
geometrySize = byteIndex - geometryOffset;
fseek( fp, geometryOffsetAtByteNb, SEEK_SET );
fwrite( &geometryOffset, sizeof( geometryOffset ), 1, fp );
nbBytesWritten = sizeof( geometryOffset );
byteIndex += nbBytesWritten;
fwrite( &geometrySize, sizeof( geometrySize ), 1, fp );
nbBytesWritten = sizeof( geometrySize );
byteIndex += nbBytesWritten;
fseek( fp, byteIndex, SEEK_SET );
fclose( fp );
return true;
}
bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
{
// Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO
SimdVector3 v( 1, 0, 0 );
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
simplexSolver.reset();
int nbIterations = 0;
while ( true )
{
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" );
SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = v * m_transformB.getBasis();
SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( pInA );
SimdPoint3 qWorld = m_transformB( qInB );
SimdVector3 w = pWorld - qWorld;
delta = v.dot( w );
EPA_DEBUG_ASSERT( ( delta <= 0 ) ,"Shapes are disjoint, EPA should have never been called!" );
if ( delta > 0.f )
return false;
EPA_DEBUG_ASSERT( !simplexSolver.inSimplex( w ) ,"Shapes are disjoint, EPA should have never been called!" );
if (simplexSolver.inSimplex( w ))
return false;
// Add support point to simplex
simplexSolver.addVertex( w, pWorld, qWorld );
bool closestOk = simplexSolver.closest( v );
EPA_DEBUG_ASSERT( closestOk ,"Shapes are disjoint, EPA should have never been called!" );
if (!closestOk)
return false;
SimdScalar prevVSqrd = squaredDistance;
squaredDistance = v.length2();
// Is v converging to v(A-B) ?
EPA_DEBUG_ASSERT( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) ,
"Shapes are disjoint, EPA should have never been called!" );
if (( ( prevVSqrd - squaredDistance ) <= SIMD_EPSILON * prevVSqrd ))
return false;
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
{
break;
}
++nbIterations;
}
SimdPoint3 simplexPoints[ 5 ];
SimdPoint3 wSupportPointsOnA[ 5 ];
SimdPoint3 wSupportPointsOnB[ 5 ];
int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints );
// nbSimplexPoints can't be one because cases where the origin is on the boundary are handled
// by hybrid penetration depth
EPA_DEBUG_ASSERT( ( ( nbSimplexPoints > 1 ) ,( nbSimplexPoints <= 4 ) ) ,
"Hybrid Penetration Depth algorithm failed!" );
int nbPolyhedronPoints = nbSimplexPoints;
#ifndef EPA_POLYHEDRON_USE_PLANES
int initTetraIndices[ 4 ] = { 0, 1, 2, 3 };
#endif
// Prepare initial polyhedron to start EPA from
if ( nbSimplexPoints == 1 )
{
return false;
}
else if ( nbSimplexPoints == 2 )
{
// We have a line segment inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points
SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
d.normalize();
SimdVector3 v1;
SimdVector3 v2;
SimdVector3 v3;
SimdVector3 e1;
SimdScalar absx = abs( d.getX() );
SimdScalar absy = abs( d.getY() );
SimdScalar absz = abs( d.getZ() );
if ( absx < absy )
{
if ( absx < absz )
{
e1.setX( 1 );
}
else
{
e1.setZ( 1 );
}
}
else
{
if ( absy < absz )
{
e1.setY( 1 );
}
else
{
e1.setZ( 1 );
}
}
v1 = d.cross( e1 );
v1.normalize();
v2 = v1.rotate( d, 120 * SIMD_RADS_PER_DEG );
v3 = v2.rotate( d, 120 * SIMD_RADS_PER_DEG );
nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = v1 * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 2 ] = pWorld;
wSupportPointsOnB[ 2 ] = qWorld;
simplexPoints[ 2 ] = wSupportPointsOnA[ 2 ] - wSupportPointsOnB[ 2 ];
seperatingAxisInA = v2 * m_transformA.getBasis();
seperatingAxisInB = -v2 * m_transformB.getBasis();
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
pWorld = m_transformA( p );
qWorld = m_transformB( q );
wSupportPointsOnA[ 3 ] = pWorld;
wSupportPointsOnB[ 3 ] = qWorld;
simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
seperatingAxisInA = v3 * m_transformA.getBasis();
seperatingAxisInB = -v3 * m_transformB.getBasis();
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
pWorld = m_transformA( p );
qWorld = m_transformB( q );
wSupportPointsOnA[ 4 ] = pWorld;
wSupportPointsOnB[ 4 ] = qWorld;
simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
#ifndef EPA_POLYHEDRON_USE_PLANES
if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
{
initTetraIndices[ 1 ] = 2;
initTetraIndices[ 2 ] = 3;
initTetraIndices[ 3 ] = 4;
}
else
{
if ( TetrahedronContainsOrigin( simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) )
{
initTetraIndices[ 0 ] = 1;
initTetraIndices[ 1 ] = 2;
initTetraIndices[ 2 ] = 3;
initTetraIndices[ 3 ] = 4;
}
else
{
// No tetrahedron contains the origin
assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
return false;
}
}
#endif
}
else if ( nbSimplexPoints == 3 )
{
// We have a triangle inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points
SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
SimdVector3 triangleNormal = v0.cross( v1 );
triangleNormal.normalize();
nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 3 ] = pWorld;
wSupportPointsOnB[ 3 ] = qWorld;
simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ];
#ifndef EPA_POLYHEDRON_USE_PLANES
// We place this check here because if the tetrahedron contains the origin
// there is no need to sample another support point
if ( !TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ] ) )
{
#endif
seperatingAxisInA = -triangleNormal * m_transformA.getBasis();
seperatingAxisInB = triangleNormal * m_transformB.getBasis();
p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
pWorld = m_transformA( p );
qWorld = m_transformB( q );
wSupportPointsOnA[ 4 ] = pWorld;
wSupportPointsOnB[ 4 ] = qWorld;
simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ];
#ifndef EPA_POLYHEDRON_USE_PLANES
if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 4 ] ) )
{
initTetraIndices[ 3 ] = 4;
}
else
{
// No tetrahedron contains the origin
assert( false && "Unable to find an initial tetrahedron that contains the origin!" );
return false;
}
}
#endif
}
#ifdef _DEBUG
else if ( nbSimplexPoints == 4 )
{
EPA_DEBUG_ASSERT( TetrahedronContainsOrigin( simplexPoints ) ,"Initial tetrahedron does not contain the origin!" );
}
#endif
#ifndef EPA_POLYHEDRON_USE_PLANES
SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
simplexPoints[ initTetraIndices[ 1 ] ],
simplexPoints[ initTetraIndices[ 2 ] ],
simplexPoints[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
wSupportPointsOnA[ initTetraIndices[ 1 ] ],
wSupportPointsOnA[ initTetraIndices[ 2 ] ],
wSupportPointsOnA[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
wSupportPointsOnB[ initTetraIndices[ 1 ] ],
wSupportPointsOnB[ initTetraIndices[ 2 ] ],
wSupportPointsOnB[ initTetraIndices[ 3 ] ] };
#endif
#ifdef EPA_POLYHEDRON_USE_PLANES
if ( !m_polyhedron.Create( simplexPoints, wSupportPointsOnA, wSupportPointsOnB, nbPolyhedronPoints ) )
#else
if ( !m_polyhedron.Create( wTetraPoints, wTetraSupportPointsOnA, wTetraSupportPointsOnB, 4 ) )
#endif
{
// Failed to create initial polyhedron
EPA_DEBUG_ASSERT( false ,"Failed to create initial polyhedron!" );
return false;
}
// Add initial faces to priority queue
#ifdef _DEBUG
//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
#endif
std::list< EpaFace* >& faces = m_polyhedron.GetFaces();
std::list< EpaFace* >::iterator facesItr( faces.begin() );
while ( facesItr != faces.end() )
{
EpaFace* pFace = *facesItr;
if ( !pFace->m_deleted )
{
//#ifdef EPA_POLYHEDRON_USE_PLANES
// if ( pFace->m_planeDistance >= 0 )
// {
// m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
// assert( false && "Face's plane distance equal or greater than 0!" );
// }
//#endif
if ( pFace->IsAffinelyDependent() )
{
EPA_DEBUG_ASSERT( false ,"One initial face is affinely dependent!" );
return false;
}
if ( pFace->m_vSqrd <= 0 )
{
EPA_DEBUG_ASSERT( false ,"Face containing the origin!" );
return false;
}
if ( pFace->IsClosestPointInternal() )
{
m_faceEntries.push_back( pFace );
std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
}
}
++facesItr;
}
#ifdef _DEBUG
//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
#endif
EPA_DEBUG_ASSERT( !m_faceEntries.empty() ,"No faces added to heap!" );
return true;
}
