1/*
2* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
3*
4* This software is provided 'as-is', without any express or implied
5* warranty.  In no event will the authors be held liable for any damages
6* arising from the use of this software.
7* Permission is granted to anyone to use this software for any purpose,
8* including commercial applications, and to alter it and redistribute it
9* freely, subject to the following restrictions:
10* 1. The origin of this software must not be misrepresented; you must not
11* claim that you wrote the original software. If you use this software
12* in a product, an acknowledgment in the product documentation would be
13* appreciated but is not required.
14* 2. Altered source versions must be plainly marked as such, and must not be
15* misrepresented as being the original software.
16* 3. This notice may not be removed or altered from any source distribution.
17*/
18
19#include <Box2D/Collision/b2Collision.h>
20#include <Box2D/Collision/b2Distance.h>
21
22void b2WorldManifold::Initialize(const b2Manifold* manifold,
23						  const b2Transform& xfA, float32 radiusA,
24						  const b2Transform& xfB, float32 radiusB)
25{
26	if (manifold->pointCount == 0)
27	{
28		return;
29	}
30
31	switch (manifold->type)
32	{
33	case b2Manifold::e_circles:
34		{
35			normal.Set(1.0f, 0.0f);
36			b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
37			b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
38			if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
39			{
40				normal = pointB - pointA;
41				normal.Normalize();
42			}
43
44			b2Vec2 cA = pointA + radiusA * normal;
45			b2Vec2 cB = pointB - radiusB * normal;
46			points[0] = 0.5f * (cA + cB);
47			separations[0] = b2Dot(cB - cA, normal);
48		}
49		break;
50
51	case b2Manifold::e_faceA:
52		{
53			normal = b2Mul(xfA.q, manifold->localNormal);
54			b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
55
56			for (int32 i = 0; i < manifold->pointCount; ++i)
57			{
58				b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
59				b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
60				b2Vec2 cB = clipPoint - radiusB * normal;
61				points[i] = 0.5f * (cA + cB);
62				separations[i] = b2Dot(cB - cA, normal);
63			}
64		}
65		break;
66
67	case b2Manifold::e_faceB:
68		{
69			normal = b2Mul(xfB.q, manifold->localNormal);
70			b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
71
72			for (int32 i = 0; i < manifold->pointCount; ++i)
73			{
74				b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
75				b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
76				b2Vec2 cA = clipPoint - radiusA * normal;
77				points[i] = 0.5f * (cA + cB);
78				separations[i] = b2Dot(cA - cB, normal);
79			}
80
81			// Ensure normal points from A to B.
82			normal = -normal;
83		}
84		break;
85	}
86}
87
88void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
89					  const b2Manifold* manifold1, const b2Manifold* manifold2)
90{
91	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
92	{
93		state1[i] = b2_nullState;
94		state2[i] = b2_nullState;
95	}
96
97	// Detect persists and removes.
98	for (int32 i = 0; i < manifold1->pointCount; ++i)
99	{
100		b2ContactID id = manifold1->points[i].id;
101
102		state1[i] = b2_removeState;
103
104		for (int32 j = 0; j < manifold2->pointCount; ++j)
105		{
106			if (manifold2->points[j].id.key == id.key)
107			{
108				state1[i] = b2_persistState;
109				break;
110			}
111		}
112	}
113
114	// Detect persists and adds.
115	for (int32 i = 0; i < manifold2->pointCount; ++i)
116	{
117		b2ContactID id = manifold2->points[i].id;
118
119		state2[i] = b2_addState;
120
121		for (int32 j = 0; j < manifold1->pointCount; ++j)
122		{
123			if (manifold1->points[j].id.key == id.key)
124			{
125				state2[i] = b2_persistState;
126				break;
127			}
128		}
129	}
130}
131
132// From Real-time Collision Detection, p179.
133bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
134{
135	float32 tmin = -b2_maxFloat;
136	float32 tmax = b2_maxFloat;
137
138	b2Vec2 p = input.p1;
139	b2Vec2 d = input.p2 - input.p1;
140	b2Vec2 absD = b2Abs(d);
141
142	b2Vec2 normal;
143
144	for (int32 i = 0; i < 2; ++i)
145	{
146		if (absD(i) < b2_epsilon)
147		{
148			// Parallel.
149			if (p(i) < lowerBound(i) || upperBound(i) < p(i))
150			{
151				return false;
152			}
153		}
154		else
155		{
156			float32 inv_d = 1.0f / d(i);
157			float32 t1 = (lowerBound(i) - p(i)) * inv_d;
158			float32 t2 = (upperBound(i) - p(i)) * inv_d;
159
160			// Sign of the normal vector.
161			float32 s = -1.0f;
162
163			if (t1 > t2)
164			{
165				b2Swap(t1, t2);
166				s = 1.0f;
167			}
168
169			// Push the min up
170			if (t1 > tmin)
171			{
172				normal.SetZero();
173				normal(i) = s;
174				tmin = t1;
175			}
176
177			// Pull the max down
178			tmax = b2Min(tmax, t2);
179
180			if (tmin > tmax)
181			{
182				return false;
183			}
184		}
185	}
186
187	// Does the ray start inside the box?
188	// Does the ray intersect beyond the max fraction?
189	if (tmin < 0.0f || input.maxFraction < tmin)
190	{
191		return false;
192	}
193
194	// Intersection.
195	output->fraction = tmin;
196	output->normal = normal;
197	return true;
198}
199
200// Sutherland-Hodgman clipping.
201int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
202						const b2Vec2& normal, float32 offset, int32 vertexIndexA)
203{
204	// Start with no output points
205	int32 numOut = 0;
206
207	// Calculate the distance of end points to the line
208	float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
209	float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
210
211	// If the points are behind the plane
212	if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
213	if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
214
215	// If the points are on different sides of the plane
216	if (distance0 * distance1 < 0.0f)
217	{
218		// Find intersection point of edge and plane
219		float32 interp = distance0 / (distance0 - distance1);
220		vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
221
222		// VertexA is hitting edgeB.
223		vOut[numOut].id.cf.indexA = static_cast<uint8>(vertexIndexA);
224		vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
225		vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;
226		vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;
227		++numOut;
228	}
229
230	return numOut;
231}
232
233bool b2TestOverlap(	const b2Shape* shapeA, int32 indexA,
234					const b2Shape* shapeB, int32 indexB,
235					const b2Transform& xfA, const b2Transform& xfB)
236{
237	b2DistanceInput input;
238	input.proxyA.Set(shapeA, indexA);
239	input.proxyB.Set(shapeB, indexB);
240	input.transformA = xfA;
241	input.transformB = xfB;
242	input.useRadii = true;
243
244	b2SimplexCache cache;
245	cache.count = 0;
246
247	b2DistanceOutput output;
248
249	b2Distance(&output, &cache, &input);
250
251	return output.distance < 10.0f * b2_epsilon;
252}
253