Exemplo n.º 1
0
float32 DistanceGeneric(b2Vec2* x1, b2Vec2* x2,
				   const T1* shape1, const b2XForm& xf1,
				   const T2* shape2, const b2XForm& xf2)
{
	b2Vec2 p1s[3], p2s[3];
	b2Vec2 points[3];
	int32 pointCount = 0;

	*x1 = shape1->GetFirstVertex(xf1);
	*x2 = shape2->GetFirstVertex(xf2);

	float32 vSqr = 0.0f;
	const int32 maxIterations = 20;
	for (int32 iter = 0; iter < maxIterations; ++iter)
	{
		b2Vec2 v = *x2 - *x1;
		b2Vec2 w1 = shape1->Support(xf1, v);
		b2Vec2 w2 = shape2->Support(xf2, -v);

		vSqr = b2Dot(v, v);
		b2Vec2 w = w2 - w1;
		float32 vw = b2Dot(v, w);
		if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points
		{
			if (pointCount == 0)
			{
				*x1 = w1;
				*x2 = w2;
			}
			g_GJK_Iterations = iter;
			return b2Sqrt(vSqr);
		}

		switch (pointCount)
		{
		case 0:
			p1s[0] = w1;
			p2s[0] = w2;
			points[0] = w;
			*x1 = p1s[0];
			*x2 = p2s[0];
			++pointCount;
			break;

		case 1:
			p1s[1] = w1;
			p2s[1] = w2;
			points[1] = w;
			pointCount = ProcessTwo(x1, x2, p1s, p2s, points);
			break;

		case 2:
			p1s[2] = w1;
			p2s[2] = w2;
			points[2] = w;
			pointCount = ProcessThree(x1, x2, p1s, p2s, points);
			break;
		}

		// If we have three points, then the origin is in the corresponding triangle.
		if (pointCount == 3)
		{
			g_GJK_Iterations = iter;
			return 0.0f;
		}

		float32 maxSqr = -B2_FLT_MAX;
		for (int32 i = 0; i < pointCount; ++i)
		{
			maxSqr = b2Max(maxSqr, b2Dot(points[i], points[i]));
		}

#ifdef TARGET_FLOAT32_IS_FIXED
		if (pointCount == 3 || vSqr <= 5.0*B2_FLT_EPSILON * maxSqr)
#else
		if (vSqr <= 100.0f * B2_FLT_EPSILON * maxSqr)
#endif
		{
			g_GJK_Iterations = iter;
			v = *x2 - *x1;
			vSqr = b2Dot(v, v);
			return b2Sqrt(vSqr);
		}
	}

	g_GJK_Iterations = maxIterations;
	return b2Sqrt(vSqr);
}
Exemplo n.º 2
0
float32 b2Distance(b2Vec2* p1Out, b2Vec2* p2Out, const b2Shape* shape1, const b2Shape* shape2)
{
	b2Vec2 p1s[3], p2s[3];
	b2Vec2 points[3];
	int32 pointCount = 0;

	*p1Out = shape1->m_position;
	*p2Out = shape2->m_position;

	float32 vSqr = 0.0f;
	const int32 maxIterations = 20;
	for (int32 iter = 0; iter < maxIterations; ++iter)
	{
		b2Vec2 v = *p2Out - *p1Out;
		b2Vec2 w1 = shape1->Support(v);
		b2Vec2 w2 = shape2->Support(-v);

		vSqr = b2Dot(v, v);
		b2Vec2 w = w2 - w1;
		float32 vw = b2Dot(v, w);
		if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points
		{
			if (pointCount == 0)
			{
				*p1Out = w1;
				*p2Out = w2;
			}
			g_GJK_Iterations = iter;
			return sqrtf(vSqr);
		}

		switch (pointCount)
		{
		case 0:
			p1s[0] = w1;
			p2s[0] = w2;
			points[0] = w;
			*p1Out = p1s[0];
			*p2Out = p2s[0];
			++pointCount;
			break;
			
		case 1:
			p1s[1] = w1;
			p2s[1] = w2;
			points[1] = w;
			pointCount = ProcessTwo(p1Out, p2Out, p1s, p2s, points);
			break;

		case 2:
			p1s[2] = w1;
			p2s[2] = w2;
			points[2] = w;
			pointCount = ProcessThree(p1Out, p2Out, p1s, p2s, points);
			break;
		}

		// If we have three points, then the origin is in the corresponding triangle.
		if (pointCount == 3)
		{
			g_GJK_Iterations = iter;
			return 0.0f;
		}

		float32 maxSqr = -FLT_MAX;
		for (int32 i = 0; i < pointCount; ++i)
		{
			maxSqr = b2Max(maxSqr, b2Dot(points[i], points[i]));
		}

		if (pointCount == 3 || vSqr <= 100.0f * FLT_EPSILON * maxSqr)
		{
			g_GJK_Iterations = iter;
			return sqrtf(vSqr);
		}
	}

	g_GJK_Iterations = maxIterations;
	return sqrtf(vSqr);
}