C++ PointSet::supportPoint方法代码示例

void GjkContactSolver::penetration(const PointSet & A, const PointSet & B, ClosestTestContext * result)
{
	resetSimplex(result->W);
	const Vector3F r = result->rayDirection;
	const Vector3F startP = Vector3F::Zero - result->rayDirection * 99.f;
	Vector3F hitP = startP;
	// from origin to startP
	Vector3F v = hitP;
	Vector3F w, p, pa, pb, localA, localB;
	float lamda = 0.f;
	float vdotw, vdotr;

	int k = 0;
	for(; k < 39; k++) {
		vdotr = v.dot(r);
	
		// SA-B(v)
		pa = A.supportPoint(v, result->transformA, localA, result->margin);
		pb = B.supportPoint(v.reversed(), result->transformB, localB, result->margin);
		p = pa - pb;// + v.normal() * MARGIN_DISTANCE;
		w = hitP - p;
		vdotw = v.dot(w); 
		
		if(vdotw > 0.f) {
			if(vdotr >= 0.f)
				break;
			lamda -= vdotw / vdotr;
			hitP = startP + r * lamda;
		}
				
		addToSimplex(result->W, p, localB);
	
		result->hasResult = 0;
		result->distance = 1e9;
		result->referencePoint = hitP;
	
		closestOnSimplex(result);
	
		v = hitP - result->closestPoint;
		
		interpolatePointB(result);
	
		if(v.length2() < TINY_VALUE) break;
		
		result->separateAxis = v;
	
		smallestSimplex(result);
	}
	
	result->distance = hitP.length();
	result->separateAxis.normalize();
}

void GjkContactSolver::separateDistance(const PointSet & A, const PointSet & B, ClosestTestContext * result)
{
	resetSimplex(result->W);
	float v2;
	Vector3F w, pa, pb;
	Vector3F v = result->transformA.transform(A.X[0]) - result->referencePoint;
	if(v.length2() < TINY_VALUE) v = result->transformA.transform(A.X[1]) - result->referencePoint;
	Vector3F localA, localB;
	
	for(int i=0; i < 99; i++) {
		// SA-B(-v)
	    pa = A.supportPoint(v.reversed(), result->transformA, localA, result->margin);
		pb = B.supportPoint(v, result->transformB, localB, result->margin);
		w = pa - pb;
	    
		// terminate when v is close enough to v(A - B).
	    // http://www.bulletphysics.com/ftp/pub/test/physics/papers/jgt04raycast.pdf
		v2 = v.length2();
	    if(v2 - w.dot(v) < 0.0001f * v2) {
	        // std::cout<<" v is close to w "<<v2 - w.dot(v)<<"\n";
			break;
	    }
	    
	    addToSimplex(result->W, w, localB);
		
#ifdef DBG_GJK_DRAW
		glPushMatrix();
		m_dbgDrawer->useSpace(result->transformB);
		glColor3f(0.f, .5f, 0.f);
		m_dbgDrawer->arrow(Vector3F::Zero, localB);
		glPopMatrix();
		
		glPushMatrix();
		m_dbgDrawer->useSpace(result->transformA);
		glColor3f(.5f, 0.f, 0.f);
		m_dbgDrawer->arrow(Vector3F::Zero, localA);
		glPopMatrix();
		
		glPushMatrix();
		m_dbgDrawer->useSpace(result->transformA);
		glColor3f(.5f, .5f, 0.f);
		m_dbgDrawer->arrow(Vector3F::Zero, w + result->transformB.transform(localB));
		glPopMatrix();
#endif
 
	    if(isPointInsideSimplex(result->W, result->referencePoint)) {
	        // std::cout<<" Minkowski difference contains the reference point\n";
			result->hasResult = 1;
			return;
	    }
	    
	    result->hasResult = 0;
		result->distance = 1e9;
		closestOnSimplex(result);
	    v = result->closestPoint - result->referencePoint;
		result->separateAxis = v;
#ifdef DBG_GJK_DRAW
		glColor3f(.1f, 3.f, 0.f);
		m_dbgDrawer->arrow(Vector3F::Zero, v);
#endif
		interpolatePointB(result);
		// in world space
		smallestSimplex(result);
	}
	result->hasResult = 0;
}

void GjkContactSolver::rayCast(const PointSet & A, const PointSet & B, ClosestTestContext * result)
{
	separateDistance(A, B, result);
	if(result->hasResult) return;
	
	resetSimplex(result->W);
	
	const Vector3F r = result->rayDirection;
	float lamda = 0.f;
	// ray started at origin
	const Vector3F startP = Vector3F::Zero;
	Vector3F hitP = startP;
	Vector3F hitN; hitN.setZero();
	Vector3F v = hitP - result->closestPoint;
	Vector3F w, p, pa, pb, localA, localB;
	
	float vdotw, vdotr;
	int k = 0;
	for(; k < 32; k++) {
	    vdotr = v.dot(r);
	    
	    // SA-B(v)
		pa = A.supportPoint(v, result->transformA, localA, result->margin);
		pb = B.supportPoint(v.reversed(), result->transformB, localB, result->margin);
	    p = pa - pb;
	    
		w = hitP - p;
	    vdotw = v.dot(w); 
	    
	    if(vdotw > 0.f) {
			// std::cout<<" v.w > 0\n";
			if(vdotr >= 0.f) {
				// std::cout<<" v.r >= 0 missed\n";
				result->hasResult = 0;
				return;
			}
			lamda -= vdotw / vdotr;
			hitP = startP + r * lamda;
			hitN = v;
		}
		
	    addToSimplex(result->W, p, localB);
	    
	    result->hasResult = 0;
	    result->distance = 1e9;
	    result->referencePoint = hitP;
	    
	    closestOnSimplex(result);
	    
	    v = hitP - result->closestPoint;
		
		interpolatePointB(result);
		
		if(v.length2() < TINY_VALUE) break;
		
		smallestSimplex(result);
	}
	
	if(k==32) std::cout<<"    max iterations reached!\n";
	// std::cout<<" k"<<k<<" ||v|| "<<v.length()<<"\n";
	result->hasResult = 1;
	result->separateAxis = hitN.normal();
	result->distance = lamda;
}