SimdVector3 PolyhedralConvexShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
{
int i;
SimdVector3 supVec(0,0,0);
SimdScalar maxDot(-1e30f);
SimdVector3 vec = vec0;
SimdScalar lenSqr = vec.length2();
if (lenSqr < 0.0001f)
{
vec.setValue(1,0,0);
} else
{
float rlen = 1.f / SimdSqrt(lenSqr );
vec *= rlen;
}
SimdVector3 vtx;
SimdScalar newDot;
for (i=0;i<GetNumVertices();i++)
{
GetVertex(i,vtx);
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex)
{
for (int i=0;i<3;i++)
{
SimdScalar dot = m_supportVecLocal.dot(triangle[i]);
if (dot > m_maxDot)
{
m_maxDot = dot;
m_supportVertexLocal = triangle[i];
}
}
}
void ReplaceMeFacet::silhouette(int index, const SimdVector3& w,
ReplaceMeEdgeBuffer& edgeBuffer)
{
if (!m_obsolete) {
if (m_closest.dot(w) < m_dist2) {
edgeBuffer.push_back(ReplaceMeEdge(this, index));
}
else {
m_obsolete = true; // Facet is visible
int next = incMod3(index);
m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer);
next = incMod3(next);
m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer);
}
}
}
bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
SimdScalar& penDepth, SimdVector3& v )
{
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
const SimdScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin();
const SimdScalar marginSqrd = margin * margin;
simplexSolver.reset();
int nbIterations = 0;
while ( true )
{
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
SimdVector3 seperatingAxisInA = -v * transformA.getBasis();
SimdVector3 seperatingAxisInB = v * transformB.getBasis();
SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
SimdPoint3 pWorld = transformA( pInA );
SimdPoint3 qWorld = transformB( qInB );
SimdVector3 w = pWorld - qWorld;
delta = v.dot( w );
// potential exit, they don't overlap
if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) )
{
// Convex shapes do not overlap
// Returning true means that Hybrid's result is ok and there's no need to run EPA
penDepth = 0;
return true;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) )
{
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
//add current vertex to simplex
simplexSolver.addVertex( w, pWorld, qWorld );
//calculate the closest point to the origin (update vector v)
if ( !simplexSolver.closest( v ) )
{
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
SimdScalar previousSquaredDistance = squaredDistance;
squaredDistance = v.length2();
//are we getting any closer ?
if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance )
{
simplexSolver.backup_closest( v );
squaredDistance = v.length2();
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
{
// Convex Shapes intersect - we need to run EPA
// Returning false means that Hybrid couldn't do anything for us
// and that we need to run EPA to calculate the pen depth
return false;
}
++nbIterations;
}
}
void Raytracer::displayCallback()
{
updateCamera();
for (int i=0;i<numObjects;i++)
{
transforms[i].setIdentity();
SimdVector3 pos(-3.5f+i*2.5f,0.f,0.f);
transforms[i].setOrigin( pos );
SimdQuaternion orn;
if (i < 2)
{
orn.setEuler(yaw,pitch,roll);
transforms[i].setRotation(orn);
}
}
myMink.SetTransformA(SimdTransform(transforms[0].getRotation()));
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
if (once)
{
glGenTextures(1, &glTextureId);
glBindTexture(GL_TEXTURE_2D,glTextureId );
once = 0;
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
}
glDisable(GL_TEXTURE_2D);
glDisable(GL_BLEND);
#define RAYTRACER
#ifdef RAYTRACER
SimdVector4 rgba(1.f,0.f,0.f,0.5f);
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = 2.0 * atanf (tanFov);
SimdVector3 rayFrom = getCameraPosition();
SimdVector3 rayForward = getCameraTargetPosition()-getCameraPosition();
rayForward.normalize();
float farPlane = 600.f;
rayForward*= farPlane;
SimdVector3 rightOffset;
SimdVector3 vertical(0.f,1.f,0.f);
SimdVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
SimdVector3 rayToCenter = rayFrom + rayForward;
SimdVector3 dHor = hor * 1.f/float(screenWidth);
SimdVector3 dVert = vertical * 1.f/float(screenHeight);
SimdTransform rayFromTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom);
SimdTransform rayFromLocal;
SimdTransform rayToLocal;
SphereShape pointShape(0.0f);
///clear texture
for (int x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
SimdVector4 rgba(0.f,0.f,0.f,0.f);
raytracePicture->SetPixel(x,y,rgba);
}
}
ConvexCast::CastResult rayResult;
SimdTransform rayToTrans;
rayToTrans.setIdentity();
SimdVector3 rayTo;
for (int x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += x * dHor;
rayTo -= y * dVert;
rayToTrans.setOrigin(rayTo);
for (int s=0;s<numObjects;s++)
{
// rayFromLocal = transforms[s].inverse()* rayFromTrans;
// rayToLocal = transforms[s].inverse()* rayToTrans;
//choose the continuous collision detection method
SubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
//GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0);
// BU_Simplex1to4 ptShape(SimdVector3(0,0,0));//algebraic needs features, doesnt use 'supporting vertex'
// BU_CollisionPair convexCaster(&ptShape,shapePtr[0]);
//reset previous result
rayResult.m_fraction = 1.f;
if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult))
{
//float fog = 1.f - 0.1f * rayResult.m_fraction;
rayResult.m_normal.normalize();
SimdVector3 worldNormal;
worldNormal = transforms[s].getBasis() *rayResult.m_normal;
float light = worldNormal.dot(SimdVector3(0.4f,-1.f,-0.4f));
if (light < 0.2f)
light = 0.2f;
if (light > 1.f)
light = 1.f;
rgba = SimdVector4(light,light,light,1.f);
raytracePicture->SetPixel(x,y,rgba);
} else
{
//clear is already done
//rgba = SimdVector4(0.f,0.f,0.f,0.f);
//raytracePicture->SetPixel(x,y,rgba);
}
}
}
}
#define TEST_PRINTF
#ifdef TEST_PRINTF
extern BMF_FontData BMF_font_helv10;
raytracePicture->Printf("CCD RAYTRACER",&BMF_font_helv10);
char buffer[256];
sprintf(buffer,"%d RAYS / Frame",screenWidth*screenHeight*numObjects);
raytracePicture->Printf(buffer,&BMF_font_helv10,0,10);
#endif //TEST_PRINTF
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity(); // Reset The Modelview Matrix
glTranslatef(0.0f,0.0f,-3.0f); // Move Into The Screen 5 Units
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,glTextureId );
const unsigned char *ptr = raytracePicture->GetBuffer();
glTexImage2D(GL_TEXTURE_2D,
0,
GL_RGBA,
raytracePicture->GetWidth(),raytracePicture->GetHeight(),
0,
GL_RGBA,
GL_UNSIGNED_BYTE,
ptr);
glEnable (GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor4f (1,1,1,1); // alpha=0.5=half visible
glBegin(GL_QUADS);
glTexCoord2f(0.0f, 0.0f);
glVertex2f(-1,1);
glTexCoord2f(1.0f, 0.0f);
glVertex2f(1,1);
glTexCoord2f(1.0f, 1.0f);
glVertex2f(1,-1);
glTexCoord2f(0.0f, 1.0f);
glVertex2f(-1,-1);
glEnd();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
#endif //RAYRACER
glDisable(GL_TEXTURE_2D);
glDisable(GL_DEPTH_TEST);
GL_ShapeDrawer::DrawCoordSystem();
glPushMatrix();
/*
/// normal opengl rendering
float m[16];
int i;
for (i=0;i<numObjects;i++)
{
transA.getOpenGLMatrix( m );
/// draw the simplex
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1));
/// calculate closest point from simplex to the origin, and draw this vector
simplex.CalcClosest(m);
}
*/
glPopMatrix();
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
{
SimdVector3 v;
SimdScalar upperBoundSqrd = SIMD_INFINITY;
SimdScalar vSqrd = 0;
#ifdef _DEBUG
SimdScalar prevVSqrd;
#endif
SimdScalar delta;
bool isCloseEnough = false;
EpaFace* pEpaFace = NULL;
int nbIterations = 0;
//int nbMaxIterations = 1000;
do
{
pEpaFace = m_faceEntries.front();
std::pop_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
m_faceEntries.pop_back();
if ( !pEpaFace->m_deleted )
{
#ifdef _DEBUG
prevVSqrd = vSqrd;
#endif
vSqrd = pEpaFace->m_vSqrd;
if ( pEpaFace->m_planeDistance >= 0 )
{
v = pEpaFace->m_planeNormal;
}
else
{
v = pEpaFace->m_v;
}
#ifdef _DEBUG
//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
EPA_DEBUG_ASSERT( ( vSqrd >= prevVSqrd ) ,"vSqrd decreased!" );
#endif //_DEBUG
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Zero vector not allowed!" );
SimdVector3 seperatingAxisInA = v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v * 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 );
SimdPoint3 w = pWorld - qWorld;
delta = v.dot( w );
// Keep tighest upper bound
upperBoundSqrd = SimdMin( upperBoundSqrd, delta * delta / vSqrd );
//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd );
if ( !isCloseEnough )
{
std::list< EpaFace* > newFaces;
bool expandOk = m_polyhedron.Expand( w, pWorld, qWorld, pEpaFace, newFaces );
if ( expandOk )
{
EPA_DEBUG_ASSERT( !newFaces.empty() ,"EPA polyhedron not expanding ?" );
bool check = true;
bool areEqual = false;
while ( !newFaces.empty() )
{
EpaFace* pNewFace = newFaces.front();
EPA_DEBUG_ASSERT( !pNewFace->m_deleted ,"New face is deleted!" );
if ( !pNewFace->m_deleted )
{
EPA_DEBUG_ASSERT( ( pNewFace->m_vSqrd > 0 ) ,"Face containing the origin!" );
EPA_DEBUG_ASSERT( !pNewFace->IsAffinelyDependent() ,"Face is affinely dependent!" );
//#ifdef EPA_POLYHEDRON_USE_PLANES
//// if ( pNewFace->m_planeDistance >= 0 )
//// {
// // assert( false && "Face's plane distance greater than 0!" );
//#ifdef _DEBUG
//// m_polyhedron._dbgSaveToFile( "epa_beforeFix.dbg" );
//#endif
// //pNewFace->FixOrder();
//#ifdef _DEBUG
// //m_polyhedron._dbgSaveToFile( "epa_afterFix.dbg" );
//#endif
//// }
//#endif
//
//#ifdef EPA_POLYHEDRON_USE_PLANES
// //assert( ( pNewFace->m_planeDistance < 0 ) && "Face's plane distance equal or greater than 0!" );
//#endif
if ( pNewFace->IsClosestPointInternal() && ( vSqrd <= pNewFace->m_vSqrd ) && ( pNewFace->m_vSqrd <= upperBoundSqrd ) )
{
m_faceEntries.push_back( pNewFace );
std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries );
}
}
newFaces.pop_front();
}
}
else
{
pEpaFace->CalcClosestPointOnA( wWitnessOnA );
pEpaFace->CalcClosestPointOnB( wWitnessOnB );
#ifdef _DEBUG
//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
#endif
return v.length();
}
}
}
++nbIterations;
}
while ( ( m_polyhedron.GetNbFaces() < EPA_MAX_FACE_ENTRIES ) &&/*( nbIterations < nbMaxIterations ) &&*/
!isCloseEnough && ( m_faceEntries.size() > 0 ) && ( m_faceEntries[ 0 ]->m_vSqrd <= upperBoundSqrd ) );
#ifdef _DEBUG
//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
#endif
EPA_DEBUG_ASSERT( pEpaFace ,"Invalid epa face!" );
pEpaFace->CalcClosestPointOnA( wWitnessOnA );
pEpaFace->CalcClosestPointOnB( wWitnessOnB );
return v.length();
}
bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB,
const int nbInitialPoints )
{
#ifndef EPA_POLYHEDRON_USE_PLANES
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 ];
SimdScalar axisMin = SIMD_INFINITY;
SimdScalar 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 ?
assert( false && "nbSuccessfullAxis must be greater than 1!" );
return false;
}
delete[] pDiffCoords;
#endif
//////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
SimdVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
#else
SimdVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
SimdVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
#endif
SimdVector3 planeNormal = v1.cross( v0 );
planeNormal.normalize();
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
#else
SimdScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
#endif
#ifdef EPA_POLYHEDRON_USE_PLANES
assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) &&
"Point should be on plane!" );
assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) &&
"Point should be on plane!" );
assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) &&
"Point should be on plane!" );
#endif
#ifndef EPA_POLYHEDRON_USE_PLANES
{
if ( planeDistance > 0 )
{
SimdVector3 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;
SimdScalar absMaxDist = -SIMD_INFINITY;
SimdScalar maxDist;
for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex )
{
SimdScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance;
SimdScalar absDist = abs( dist );
if ( ( absDist > absMaxDist ) &&
!SimdEqual( 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
SimdPoint3 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() )
{
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;
SimdScalar 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;
}
assert( !newFaces.empty() && "Polyhedron should have expanded!" );
break;
}
++facesItr;
}
}
}
}
#endif
return true;
}
bool VoronoiSimplexSolver::UpdateClosestVectorAndPoints()
{
if (m_needsUpdate)
{
m_cachedBC.Reset();
m_needsUpdate = false;
switch (numVertices())
{
case 0:
m_cachedValidClosest = false;
break;
case 1:
{
m_cachedP1 = m_simplexPointsP[0];
m_cachedP2 = m_simplexPointsQ[0];
m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
m_cachedBC.Reset();
m_cachedBC.SetBarycentricCoordinates(1.f,0.f,0.f,0.f);
m_cachedValidClosest = m_cachedBC.IsValid();
break;
};
case 2:
{
//closest point origin from line segment
const SimdVector3& from = m_simplexVectorW[0];
const SimdVector3& to = m_simplexVectorW[1];
SimdVector3 nearest;
SimdVector3 p (0.f,0.f,0.f);
SimdVector3 diff = p - from;
SimdVector3 v = to - from;
float t = v.dot(diff);
if (t > 0) {
float dotVV = v.dot(v);
if (t < dotVV) {
t /= dotVV;
diff -= t*v;
m_cachedBC.m_usedVertices.usedVertexA = true;
m_cachedBC.m_usedVertices.usedVertexB = true;
} else {
t = 1;
diff -= v;
//reduce to 1 point
m_cachedBC.m_usedVertices.usedVertexB = true;
}
} else
{
t = 0;
//reduce to 1 point
m_cachedBC.m_usedVertices.usedVertexA = true;
}
m_cachedBC.SetBarycentricCoordinates(1-t,t);
nearest = from + t*v;
m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
m_cachedV = m_cachedP1 - m_cachedP2;
ReduceVertices(m_cachedBC.m_usedVertices);
m_cachedValidClosest = m_cachedBC.IsValid();
break;
}
case 3:
{
//closest point origin from triangle
SimdVector3 p (0.f,0.f,0.f);
const SimdVector3& a = m_simplexVectorW[0];
const SimdVector3& b = m_simplexVectorW[1];
const SimdVector3& c = m_simplexVectorW[2];
ClosestPtPointTriangle(p,a,b,c,m_cachedBC);
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
m_cachedV = m_cachedP1-m_cachedP2;
ReduceVertices (m_cachedBC.m_usedVertices);
m_cachedValidClosest = m_cachedBC.IsValid();
break;
}
case 4:
{
SimdVector3 p (0.f,0.f,0.f);
const SimdVector3& a = m_simplexVectorW[0];
const SimdVector3& b = m_simplexVectorW[1];
const SimdVector3& c = m_simplexVectorW[2];
const SimdVector3& d = m_simplexVectorW[3];
bool hasSeperation = ClosestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
if (hasSeperation)
{
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
m_cachedV = m_cachedP1-m_cachedP2;
ReduceVertices (m_cachedBC.m_usedVertices);
} else
{
// printf("sub distance got penetration\n");
if (m_cachedBC.m_degenerate)
{
m_cachedValidClosest = false;
} else
{
m_cachedValidClosest = true;
//degenerate case == false, penetration = true + zero
m_cachedV.setValue(0.f,0.f,0.f);
}
break;
}
m_cachedValidClosest = m_cachedBC.IsValid();
//closest point origin from tetrahedron
break;
}
default:
{
m_cachedValidClosest = false;
}
};
}
return m_cachedValidClosest;
}
bool VoronoiSimplexSolver::ClosestPtPointTriangle(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c,SubSimplexClosestResult& result)
{
result.m_usedVertices.reset();
// Check if P in vertex region outside A
SimdVector3 ab = b - a;
SimdVector3 ac = c - a;
SimdVector3 ap = p - a;
float d1 = ab.dot(ap);
float d2 = ac.dot(ap);
if (d1 <= 0.0f && d2 <= 0.0f)
{
result.m_closestPointOnSimplex = a;
result.m_usedVertices.usedVertexA = true;
result.SetBarycentricCoordinates(1,0,0);
return true;// a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
SimdVector3 bp = p - b;
float d3 = ab.dot(bp);
float d4 = ac.dot(bp);
if (d3 >= 0.0f && d4 <= d3)
{
result.m_closestPointOnSimplex = b;
result.m_usedVertices.usedVertexB = true;
result.SetBarycentricCoordinates(0,1,0);
return true; // b; // barycentric coordinates (0,1,0)
}
// Check if P in edge region of AB, if so return projection of P onto AB
float vc = d1*d4 - d3*d2;
if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
float v = d1 / (d1 - d3);
result.m_closestPointOnSimplex = a + v * ab;
result.m_usedVertices.usedVertexA = true;
result.m_usedVertices.usedVertexB = true;
result.SetBarycentricCoordinates(1-v,v,0);
return true;
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
SimdVector3 cp = p - c;
float d5 = ab.dot(cp);
float d6 = ac.dot(cp);
if (d6 >= 0.0f && d5 <= d6)
{
result.m_closestPointOnSimplex = c;
result.m_usedVertices.usedVertexC = true;
result.SetBarycentricCoordinates(0,0,1);
return true;//c; // barycentric coordinates (0,0,1)
}
// Check if P in edge region of AC, if so return projection of P onto AC
float vb = d5*d2 - d1*d6;
if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
float w = d2 / (d2 - d6);
result.m_closestPointOnSimplex = a + w * ac;
result.m_usedVertices.usedVertexA = true;
result.m_usedVertices.usedVertexC = true;
result.SetBarycentricCoordinates(1-w,0,w);
return true;
//return a + w * ac; // barycentric coordinates (1-w,0,w)
}
// Check if P in edge region of BC, if so return projection of P onto BC
float va = d3*d6 - d5*d4;
if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
result.m_closestPointOnSimplex = b + w * (c - b);
result.m_usedVertices.usedVertexB = true;
result.m_usedVertices.usedVertexC = true;
result.SetBarycentricCoordinates(0,1-w,w);
return true;
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
float denom = 1.0f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
result.m_closestPointOnSimplex = a + ab * v + ac * w;
result.m_usedVertices.usedVertexA = true;
result.m_usedVertices.usedVertexB = true;
result.m_usedVertices.usedVertexC = true;
result.SetBarycentricCoordinates(1-v-w,v,w);
return true;
// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w
}
