Sign in to follow this  
Irlan

GJK-SAT + EPA Problem

Recommended Posts

Hi. I have GJK-SAT working in the last few weeks and I wanted to implement EPA but I'm having some problems with it.
 
I've found a good explanation here on how to deal with the polytope re-construction each iteration, and later I've found here and I'm using it as a helper.
 
One problem with my implementation is that still doesn't give me the correct contact point sometimes using the barycentrics coordinates when passing the pre-defined threshold, and another problem is that I get stucked in the EPA even clamping the iterations by some amount.
 
Sometimes the algorithm returns the contact point. Here's a picture:
 
 epa-contact-point1.jpg

 

Now when from another location:

 

epa-wrong-contact-point1.jpg

 

I've checked if the GJK-SAT and is always returning a tetrahedron at the end of the algorithm if there is a intersection.

Here's the EPA code:

#include "CEpa.h"

void CEpa::BlowUpSimplex( CGjk::SIMPLEX& _sSimplex ) const {
	/*
	if ( _sSimplex.ssSimplexType == CGjk::SIMPLEX::ST_POINT ) {

	}
	else if ( _sSimplex.ssSimplexType == CGjk::SIMPLEX::ST_LINE ) {
	
	}
	else if ( _sSimplex.ssSimplexType == CGjk::SIMPLEX::ST_TRIANGLE ) {
	}
	*/
}

void CEpa::ExtractContactInfo( CGjk& _gGjk ) {
	//BlowUpSimplex(_gGjk.m_sSimplex);

	//Create the initial polytope (normal is computed in the constructor).
	m_epPolytope.vFaces.push_back( EPA_POLYTOPE_FACE(_gGjk.m_sSimplex.vA, _gGjk.m_sSimplex.vB, _gGjk.m_sSimplex.vC));
	m_epPolytope.vFaces.push_back( EPA_POLYTOPE_FACE(_gGjk.m_sSimplex.vA, _gGjk.m_sSimplex.vC, _gGjk.m_sSimplex.vD));
	m_epPolytope.vFaces.push_back( EPA_POLYTOPE_FACE(_gGjk.m_sSimplex.vA, _gGjk.m_sSimplex.vD, _gGjk.m_sSimplex.vB));
	m_epPolytope.vFaces.push_back( EPA_POLYTOPE_FACE(_gGjk.m_sSimplex.vB, _gGjk.m_sSimplex.vD, _gGjk.m_sSimplex.vC));

	//Clamp to 20 iterations.
	for (unsigned int I = 0UL; I < EPA_ITERATIONS; ++I) {
		//Get the closest face to the origin.
		unsigned int ui32ClosestFaceIndex = 0UL;
		float fClosestFaceDistanceToOrigin = m_epPolytope.vFaces[0UL].vNormal.Dot(m_epPolytope.vFaces[0UL].vA.vMinkSup); //Could be A/B/C.
		for ( unsigned int J = 1UL; J < m_epPolytope.vFaces.size(); ++J ) {
			const EPA_POLYTOPE_FACE& epfFace = m_epPolytope.vFaces[J];
			float fFaceDistanceToOrigin = epfFace.vNormal.Dot( epfFace.vA.vMinkSup ); //Could be A/B/C.
			if (fFaceDistanceToOrigin < fClosestFaceDistanceToOrigin) {
				ui32ClosestFaceIndex = J;
				fClosestFaceDistanceToOrigin = fFaceDistanceToOrigin;
			}
		}
		
		//The new simplex in the direction of the closest face.
		const EPA_POLYTOPE_FACE& epfClosestFace = m_epPolytope.vFaces[ui32ClosestFaceIndex];
		CGjk::SIMPLEX_VERTEX svNewSimplexVertex;
		_gGjk.GetSupportPoint(epfClosestFace.vNormal, m_psiLeft, m_psiRight, svNewSimplexVertex);

		//EPA_LIMIT is 0.0001f
		float fNewSimplexVertexDistanceToOrigin = epfClosestFace.vNormal.Dot(svNewSimplexVertex.vMinkSup);
		if (fNewSimplexVertexDistanceToOrigin - fClosestFaceDistanceToOrigin < EPA_LIMIT) {
			//Extract contact information.
			
			float fBaryU, fBaryV, fBaryW;
			CClosest::Barycentric( 
				epfClosestFace.vA.vMinkSup,
				epfClosestFace.vB.vMinkSup,
				epfClosestFace.vC.vMinkSup,
				epfClosestFace.vNormal * fClosestFaceDistanceToOrigin,
				fBaryU,
				fBaryV,
				fBaryW);

			//epfClosestFace.vB.vSupA is already in world space.
			m_vContactPos = (epfClosestFace.vA.vSupA * fBaryU) + (epfClosestFace.vB.vSupA * fBaryV) + (epfClosestFace.vC.vSupA * fBaryW);
			m_vContactNormal = -epfClosestFace.vNormal;
			m_fPenDepth = fClosestFaceDistanceToOrigin;

			break; 
		}

		/*
		* Add the new vertex.
		*/
		
		for ( std::vector<EPA_POLYTOPE_FACE>::iterator J = m_epPolytope.vFaces.begin(); J != m_epPolytope.vFaces.end(); ) {
			const EPA_POLYTOPE_FACE& epfFace = *J;

			//Check which polytope faces can be seen by the new vertex. Re-build keeping the face wind order if necessary.
			bool bFaceCanBeSeenByNewVertex = epfFace.vNormal.Dot(svNewSimplexVertex.vMinkSup - epfFace.vA.vMinkSup) > ML_ZERO;
			if ( bFaceCanBeSeenByNewVertex ) {
				UpdateEdge(epfFace.vA, epfFace.vB);
				UpdateEdge(epfFace.vB, epfFace.vC);
				UpdateEdge(epfFace.vC, epfFace.vA);

				J = m_epPolytope.vFaces.erase(J); //Remove face.
			}
			else {
				++J;
			}
		}

		for ( unsigned int J = 0UL; J < m_epPolytope.vFaceEdges.size(); ++J ) {
			//Add the new temporary edges to create a face.
			const EPA_POLYTOPE_FACE_EDGE& epeEdge = m_epPolytope.vFaceEdges[J];
			m_epPolytope.vFaces.push_back(EPA_POLYTOPE_FACE(svNewSimplexVertex, epeEdge.vA, epeEdge.vB));
		}

		//Clear the temporary edge array (or list).
		m_epPolytope.vFaceEdges.swap(std::vector<EPA_POLYTOPE_FACE_EDGE>());
	}
}

void CEpa::UpdateEdge(const CGjk::SIMPLEX_VERTEX& _vA, const CGjk::SIMPLEX_VERTEX& _vB) {
	for (std::vector<EPA_POLYTOPE_FACE_EDGE>::iterator I = m_epPolytope.vFaceEdges.begin(); I != m_epPolytope.vFaceEdges.end(); ++I) {
		const EPA_POLYTOPE_FACE_EDGE& epeFaceEdge = *I;
		if ( epeFaceEdge.vA == _vB && epeFaceEdge.vB == _vA )  {
			//Is a opossite edge. Remove it and don't add a new edge.
			m_epPolytope.vFaceEdges.erase(I);
			return;
		}
	}
	EPA_POLYTOPE_FACE_EDGE epeAddLater;
	epeAddLater.vA = _vA;
	epeAddLater.vB = _vB;
	m_epPolytope.vFaceEdges.push_back(epeAddLater);
}

Can someone that hás implemented correctly check if something is suspicious?

 

Can it be a problem with GJK?

 

Thanks.

 

[UPDATE] 

 

Just checked GJK and everything is fine but still doesn't get correct the contact point and I'm stucked in the algorithm during the polytope expansion. Here is the GJK:

#include "CGjk.h"

CGjk::CGjk() {
        //Member function pointers to the simplexes update functions.
	m_psfpSimplexUpdateFuncs[SIMPLEX::ST_EDGE] = &CGjk::SimplexEdgeUpdate;
	m_psfpSimplexUpdateFuncs[SIMPLEX::ST_FACE] = &CGjk::SimplexFaceUpdate;
	m_psfpSimplexUpdateFuncs[SIMPLEX::ST_TETRAHEDRON] = &CGjk::SimplexTetrahedronUpdate;
}

CGjk::~CGjk() {
}

void CGjk::GetSupportPoint(const CVector3& _vSearchDir, const CShapeInstance* _psiLeft, const CShapeInstance* _psiRight, SIMPLEX_VERTEX& _vRet) const {
	CVector3 vSupA;
	_vRet.ui32SupA = static_cast<const CPolygon*>(_psiLeft->ShapeInstanceShape())->GetFarthestVertex(_vSearchDir, vSupA);
	CVector3 vSupB;
	_vRet.ui32SupB = static_cast<const CPolygon*>(_psiRight->ShapeInstanceShape())->GetFarthestVertex(-_vSearchDir, vSupB);
	_vRet.vSupA = _psiLeft->GetTransform() * vSupA;
	_vRet.vSupB = _psiRight->GetTransform() * vSupB;
	_vRet.vMinkSup = _vRet.vSupA - _vRet.vSupB;
}

bool CGjk::Intersect(const CShapeInstance* _psiLeft, const CShapeInstance* _psiRight) {
	m_vSearchDir = CVector3( ML_ONE, ML_ONE, ML_ONE );

	//vA it's our first supporting point.
	GetSupportPoint(m_vSearchDir, _psiLeft, _psiRight, m_sSimplex.vA);
	m_sSimplex.ssSimplexType = SIMPLEX::ST_POINT;

	m_vSearchDir = -m_vSearchDir;

	for ( unsigned int I = 0UL; I < GJK_ITERATIONS; ++I ) {
		SIMPLEX_VERTEX vNewSimplexVertex;
		GetSupportPoint(m_vSearchDir, _psiLeft, _psiRight, vNewSimplexVertex);
		
		if (vNewSimplexVertex.vMinkSup.Dot(m_vSearchDir) <= ML_ZERO) { return false; }

		//Add new point to create a new simplex.
		if (m_sSimplex.ssSimplexType == SIMPLEX::ST_POINT) {
			m_sSimplex.vB = m_sSimplex.vA;
			m_sSimplex.vA = vNewSimplexVertex;
			m_sSimplex.ssSimplexType = SIMPLEX::ST_EDGE;
		}
		else if (m_sSimplex.ssSimplexType == SIMPLEX::ST_EDGE) {
			m_sSimplex.vC = m_sSimplex.vB;
			m_sSimplex.vB = m_sSimplex.vA;
			m_sSimplex.vA = vNewSimplexVertex;
			m_sSimplex.ssSimplexType = SIMPLEX::ST_FACE;
		}
		else if (m_sSimplex.ssSimplexType == SIMPLEX::ST_FACE) {
			m_sSimplex.vD = m_sSimplex.vC;
			m_sSimplex.vC = m_sSimplex.vB;
			m_sSimplex.vB = m_sSimplex.vA;
			m_sSimplex.vA = vNewSimplexVertex;
			m_sSimplex.ssSimplexType = SIMPLEX::ST_TETRAHEDRON;
		}

		//Check if the simplex contains the origin.
		//Update simplex and direction.
		if ( UpdateSimplex() ) { return true; }
	}

	return false;
}

bool CGjk::UpdateSimplex() {
	return (this->*m_psfpSimplexUpdateFuncs[m_sSimplex.ssSimplexType])();
}

bool CGjk::SimplexEdgeUpdate() {
	CVector3 vAo = -m_sSimplex.vA.vMinkSup;
	CVector3 vAb = m_sSimplex.vB.vMinkSup - m_sSimplex.vA.vMinkSup;
	m_vSearchDir = vAb.Cross(vAo).Cross(vAb);
	return false;
}

bool CGjk::SimplexFaceUpdate() {
	CVector3 vAo = -m_sSimplex.vA.vMinkSup;
	CVector3 vAb = m_sSimplex.vB.vMinkSup - m_sSimplex.vA.vMinkSup;
	CVector3 vAc = m_sSimplex.vC.vMinkSup - m_sSimplex.vA.vMinkSup;
	CVector3 vFaceNormal = vAb.Cross(vAc);

	if ( vAb.Cross(vFaceNormal).Dot(vAo) > ML_ZERO ) {
		m_sSimplex.ssSimplexType = SIMPLEX::ST_EDGE;
		//A and B makes the edge.
		m_vSearchDir = vAb.Cross(vAo).Cross(vAb); //Ab to Ao.
		return false;
	}
	
	if ( vFaceNormal.Cross(vAc).Dot(vAo) > ML_ZERO ) {
		//A and B makes the edge.
		m_sSimplex.vB = m_sSimplex.vC;
		m_sSimplex.ssSimplexType = SIMPLEX::ST_EDGE;
		m_vSearchDir = vAc.Cross(vAo).Cross(vAc); //Ac to Ao.
		return false;
	}

	if ( vFaceNormal.Dot(vAo) > ML_ZERO ) {
		//Above face.
		m_vSearchDir = vFaceNormal;
		return false;
	}
	
	//Below face.
	CMathLib::Swap(m_sSimplex.vB, m_sSimplex.vC);
	m_vSearchDir = -vFaceNormal;
	return false;
}

bool CGjk::SimplexTetrahedronUpdate() {
	CVector3 vAo = -m_sSimplex.vA.vMinkSup;

	CVector3 vAb = m_sSimplex.vB.vMinkSup - m_sSimplex.vA.vMinkSup;
	CVector3 vAc = m_sSimplex.vC.vMinkSup - m_sSimplex.vA.vMinkSup;
	if ( vAb.Cross(vAc).Dot(vAo) > ML_ZERO ) {
		return UpdateSimplexTetrahedronFace(vAo);
	}

	CVector3 vAd = m_sSimplex.vD.vMinkSup - m_sSimplex.vA.vMinkSup;
	if ( vAc.Cross(vAd).Dot(vAo) > ML_ZERO ) {
		m_sSimplex.vB = m_sSimplex.vC;
		m_sSimplex.vC = m_sSimplex.vD;
		return UpdateSimplexTetrahedronFace(vAo);
	}
	
	if ( vAd.Cross(vAb).Dot(vAo) > ML_ZERO ) {
		SIMPLEX_VERTEX vOldB = m_sSimplex.vB;
		m_sSimplex.vB = m_sSimplex.vD;
		m_sSimplex.vC = vOldB;
		return UpdateSimplexTetrahedronFace(vAo);
	}

	return true;
}

bool CGjk::UpdateSimplexTetrahedronFace(const CVector3& _vAo) {
	CVector3 vAb = m_sSimplex.vB.vMinkSup - m_sSimplex.vA.vMinkSup;
	CVector3 vAc = m_sSimplex.vC.vMinkSup - m_sSimplex.vA.vMinkSup;
	CVector3 vFaceNormal = vAb.Cross(vAc);

	if (vAb.Cross(vFaceNormal).Dot(_vAo) > ML_ZERO) {
		//Test if origin it is in the voronoi region of the AB edge.
		m_sSimplex.ssSimplexType = SIMPLEX::ST_EDGE;
		//Get the perpendicular direction of the edge towards the origin.
		m_vSearchDir = vAb.Cross(_vAo).Cross(vAb);
		return false;
	}
	
	if ( vFaceNormal.Cross(vAc).Dot(_vAo) > ML_ZERO ) {
		//Test if origin it is in the voronoi region of the AC edge.
		//Change to edge.
		m_sSimplex.vB = m_sSimplex.vC;
		m_sSimplex.ssSimplexType = SIMPLEX::ST_EDGE;
		//Get the perpendicular direction of the edge towards the origin.
		m_vSearchDir = vAc.Cross(_vAo).Cross(vAc);
		return false;
	}

	m_sSimplex.ssSimplexType = SIMPLEX::ST_FACE;
	m_vSearchDir = vFaceNormal;
	
	return false;
}

[UPDATE]

 

Solved. I was not cleaning the vector of faces at the end of the cycle. The algorithm is working prefectly.
 
Thank those who have seen.
Edited by Irlan

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

Sign in to follow this