All pastes #574693 Raw Edit

Someone

public text v1 · immutable
·published 2007-06-18 17:48 UTC
rendered paste body

extern "C"
double nearestpoints(float *p1, float *p2, float *p3, float *q1, float *q2, float *q3, float *pa, float *pb, int real, float *normal)
{
	double distance = 0;
	
	btTriangleShape trishapeA(btVector3(p1[0], p1[1], p1[2]), btVector3(p2[0], p2[1], p2[2]), btVector3(p3[0], p3[1], p3[2]));
	trishapeA.setMargin(0.0001f);
	

	btTriangleShape trishapeB(btVector3(q1[0], q1[1], q1[2]), btVector3(q2[0], q2[1], q2[2]), btVector3(q3[0], q3[1], q3[2]));
	trishapeB.setMargin(0.0001f);
	
	// btVoronoiSimplexSolver sGjkSimplexSolver;
	// btGjkEpaPenetrationDepthSolver penSolverPtr;	
	
	// btGjkEpaPenetrationDepthSolver	*gEpaPenetrationDepthSolver;
	// btMinkowskiPenetrationDepthSolver	gPenetrationDepthSolver;
	
	static btSimplexSolverInterface sGjkSimplexSolver;
	sGjkSimplexSolver.reset();
	
	static btGjkEpaPenetrationDepthSolver Solver0;
	static btMinkowskiPenetrationDepthSolver Solver1;
	
	btConvexPenetrationDepthSolver* Solver = NULL;
	
	Solver = &Solver0;
	
	
	btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
	
	convexConvex.m_catchDegeneracies = 1;
	
	// btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
	
	btPointCollector gjkOutput;
	btGjkPairDetector::ClosestPointInput input;
	
	btStackAlloc gStackAlloc(1024*1024*2);
 
	input.m_stackAlloc = &gStackAlloc;
	
	btTransform tr;
	tr.setIdentity();
	
	input.m_transformA = tr;
	input.m_transformB = tr;
	
	convexConvex.getClosestPoints(input, gjkOutput, 0);
	
	
	if (gjkOutput.m_hasResult)
	{
		
		VECCOPY(pa, gjkOutput.m_pointInWorld);
		VECCOPY(pb, gjkOutput.m_pointInWorld);
		VECADDFAC(pb, pb, gjkOutput.m_normalOnBInWorld, gjkOutput.m_distance);
		
		VECCOPY(normal, gjkOutput.m_normalOnBInWorld);
		
		return gjkOutput.m_distance;
	}
	else
		return -1.0f;	
}