예제 #1
0
파일: AccCorner.cpp 프로젝트: ahmidou/aphid
Vector3F AccCorner::computeNormal() const
{
	if(isOnBoundary())
		return *_centerNormal * (2.f / 3.f) +  _edgeNormals[0] * (1.f / 6.f) +  _edgeNormals[valence() - 1] * (1.f / 6.f);
		
	float e = 4.f;
	float c = 1.f;
	float sum = 0.f;
	Vector3F res;
	res.setZero();
	Vector3F q;
	
	for(int i = 0; i < valence(); i++) {
		q = _edgeNormals[i];
		res += q * e;
		sum += e;
		q = _cornerNormals[i];
		res += q * c;
		sum += c;
	}
	
	sum += valence() * valence();
	res += *_centerNormal * valence() * valence();
	return res / sum;
}
예제 #2
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;
}