1/*
2Bullet Continuous Collision Detection and Physics Library
3* The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16///btBox2dBox2dCollisionAlgorithm, with modified b2CollidePolygons routines from the Box2D library.
17///The modifications include: switching from b2Vec to btVector3, redefinition of b2Dot, b2Cross
18
19#include "btBox2dBox2dCollisionAlgorithm.h"
20#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
21#include "BulletCollision/CollisionShapes/btBoxShape.h"
22#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
23#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"
24#include "BulletCollision/CollisionShapes/btBox2dShape.h"
25#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
26
27#define USE_PERSISTENT_CONTACTS 1
28
29btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap)
30: btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap),
31m_ownManifold(false),
32m_manifoldPtr(mf)
33{
34	if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject()))
35	{
36		m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject());
37		m_ownManifold = true;
38	}
39}
40
41btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
42{
43
44	if (m_ownManifold)
45	{
46		if (m_manifoldPtr)
47			m_dispatcher->releaseManifold(m_manifoldPtr);
48	}
49
50}
51
52
53void b2CollidePolygons(btManifoldResult* manifold,  const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
54
55//#include <stdio.h>
56void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
57{
58	if (!m_manifoldPtr)
59		return;
60
61
62	const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape();
63	const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape();
64
65	resultOut->setPersistentManifold(m_manifoldPtr);
66
67	b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform());
68
69	//  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
70	if (m_ownManifold)
71	{
72		resultOut->refreshContactPoints();
73	}
74
75}
76
77btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
78{
79	//not yet
80	return 1.f;
81}
82
83
84struct ClipVertex
85{
86	btVector3 v;
87	int id;
88	//b2ContactID id;
89	//b2ContactID id;
90};
91
92#define b2Dot(a,b) (a).dot(b)
93#define b2Mul(a,b) (a)*(b)
94#define b2MulT(a,b) (a).transpose()*(b)
95#define b2Cross(a,b) (a).cross(b)
96#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f)
97
98int b2_maxManifoldPoints =2;
99
100static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
101					  const btVector3& normal, btScalar offset)
102{
103	// Start with no output points
104	int numOut = 0;
105
106	// Calculate the distance of end points to the line
107	btScalar distance0 = b2Dot(normal, vIn[0].v) - offset;
108	btScalar distance1 = b2Dot(normal, vIn[1].v) - offset;
109
110	// If the points are behind the plane
111	if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
112	if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
113
114	// If the points are on different sides of the plane
115	if (distance0 * distance1 < 0.0f)
116	{
117		// Find intersection point of edge and plane
118		btScalar interp = distance0 / (distance0 - distance1);
119		vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
120		if (distance0 > 0.0f)
121		{
122			vOut[numOut].id = vIn[0].id;
123		}
124		else
125		{
126			vOut[numOut].id = vIn[1].id;
127		}
128		++numOut;
129	}
130
131	return numOut;
132}
133
134// Find the separation between poly1 and poly2 for a give edge normal on poly1.
135static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1,
136							  const btBox2dShape* poly2, const btTransform& xf2)
137{
138	const btVector3* vertices1 = poly1->getVertices();
139	const btVector3* normals1 = poly1->getNormals();
140
141	int count2 = poly2->getVertexCount();
142	const btVector3* vertices2 = poly2->getVertices();
143
144	btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
145
146	// Convert normal from poly1's frame into poly2's frame.
147	btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]);
148	btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World);
149
150	// Find support vertex on poly2 for -normal.
151	int index = 0;
152	btScalar minDot = BT_LARGE_FLOAT;
153
154    if( count2 > 0 )
155        index = (int) normal1.minDot( vertices2, count2, minDot);
156
157	btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
158	btVector3 v2 = b2Mul(xf2, vertices2[index]);
159	btScalar separation = b2Dot(v2 - v1, normal1World);
160	return separation;
161}
162
163// Find the max separation between poly1 and poly2 using edge normals from poly1.
164static btScalar FindMaxSeparation(int* edgeIndex,
165								 const btBox2dShape* poly1, const btTransform& xf1,
166								 const btBox2dShape* poly2, const btTransform& xf2)
167{
168	int count1 = poly1->getVertexCount();
169	const btVector3* normals1 = poly1->getNormals();
170
171	// Vector pointing from the centroid of poly1 to the centroid of poly2.
172	btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid());
173	btVector3 dLocal1 = b2MulT(xf1.getBasis(), d);
174
175	// Find edge normal on poly1 that has the largest projection onto d.
176	int edge = 0;
177    btScalar maxDot;
178    if( count1 > 0 )
179        edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
180
181	// Get the separation for the edge normal.
182	btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
183	if (s > 0.0f)
184	{
185		return s;
186	}
187
188	// Check the separation for the previous edge normal.
189	int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
190	btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
191	if (sPrev > 0.0f)
192	{
193		return sPrev;
194	}
195
196	// Check the separation for the next edge normal.
197	int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
198	btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
199	if (sNext > 0.0f)
200	{
201		return sNext;
202	}
203
204	// Find the best edge and the search direction.
205	int bestEdge;
206	btScalar bestSeparation;
207	int increment;
208	if (sPrev > s && sPrev > sNext)
209	{
210		increment = -1;
211		bestEdge = prevEdge;
212		bestSeparation = sPrev;
213	}
214	else if (sNext > s)
215	{
216		increment = 1;
217		bestEdge = nextEdge;
218		bestSeparation = sNext;
219	}
220	else
221	{
222		*edgeIndex = edge;
223		return s;
224	}
225
226	// Perform a local search for the best edge normal.
227	for ( ; ; )
228	{
229		if (increment == -1)
230			edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
231		else
232			edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
233
234		s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
235		if (s > 0.0f)
236		{
237			return s;
238		}
239
240		if (s > bestSeparation)
241		{
242			bestEdge = edge;
243			bestSeparation = s;
244		}
245		else
246		{
247			break;
248		}
249	}
250
251	*edgeIndex = bestEdge;
252	return bestSeparation;
253}
254
255static void FindIncidentEdge(ClipVertex c[2],
256							 const btBox2dShape* poly1, const btTransform& xf1, int edge1,
257							 const btBox2dShape* poly2, const btTransform& xf2)
258{
259	const btVector3* normals1 = poly1->getNormals();
260
261	int count2 = poly2->getVertexCount();
262	const btVector3* vertices2 = poly2->getVertices();
263	const btVector3* normals2 = poly2->getNormals();
264
265	btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
266
267	// Get the normal of the reference edge in poly2's frame.
268	btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1]));
269
270	// Find the incident edge on poly2.
271	int index = 0;
272	btScalar minDot = BT_LARGE_FLOAT;
273	for (int i = 0; i < count2; ++i)
274	{
275		btScalar dot = b2Dot(normal1, normals2[i]);
276		if (dot < minDot)
277		{
278			minDot = dot;
279			index = i;
280		}
281	}
282
283	// Build the clip vertices for the incident edge.
284	int i1 = index;
285	int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
286
287	c[0].v = b2Mul(xf2, vertices2[i1]);
288//	c[0].id.features.referenceEdge = (unsigned char)edge1;
289//	c[0].id.features.incidentEdge = (unsigned char)i1;
290//	c[0].id.features.incidentVertex = 0;
291
292	c[1].v = b2Mul(xf2, vertices2[i2]);
293//	c[1].id.features.referenceEdge = (unsigned char)edge1;
294//	c[1].id.features.incidentEdge = (unsigned char)i2;
295//	c[1].id.features.incidentVertex = 1;
296}
297
298// Find edge normal of max separation on A - return if separating axis is found
299// Find edge normal of max separation on B - return if separation axis is found
300// Choose reference edge as min(minA, minB)
301// Find incident edge
302// Clip
303
304// The normal points from 1 to 2
305void b2CollidePolygons(btManifoldResult* manifold,
306					  const btBox2dShape* polyA, const btTransform& xfA,
307					  const btBox2dShape* polyB, const btTransform& xfB)
308{
309
310	int edgeA = 0;
311	btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
312	if (separationA > 0.0f)
313		return;
314
315	int edgeB = 0;
316	btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
317	if (separationB > 0.0f)
318		return;
319
320	const btBox2dShape* poly1;	// reference poly
321	const btBox2dShape* poly2;	// incident poly
322	btTransform xf1, xf2;
323	int edge1;		// reference edge
324	unsigned char flip;
325	const btScalar k_relativeTol = 0.98f;
326	const btScalar k_absoluteTol = 0.001f;
327
328	// TODO_ERIN use "radius" of poly for absolute tolerance.
329	if (separationB > k_relativeTol * separationA + k_absoluteTol)
330	{
331		poly1 = polyB;
332		poly2 = polyA;
333		xf1 = xfB;
334		xf2 = xfA;
335		edge1 = edgeB;
336		flip = 1;
337	}
338	else
339	{
340		poly1 = polyA;
341		poly2 = polyB;
342		xf1 = xfA;
343		xf2 = xfB;
344		edge1 = edgeA;
345		flip = 0;
346	}
347
348	ClipVertex incidentEdge[2];
349	FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
350
351	int count1 = poly1->getVertexCount();
352	const btVector3* vertices1 = poly1->getVertices();
353
354	btVector3 v11 = vertices1[edge1];
355	btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
356
357	//btVector3 dv = v12 - v11;
358	btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11);
359	sideNormal.normalize();
360	btVector3 frontNormal = btCrossS(sideNormal, 1.0f);
361
362
363	v11 = b2Mul(xf1, v11);
364	v12 = b2Mul(xf1, v12);
365
366	btScalar frontOffset = b2Dot(frontNormal, v11);
367	btScalar sideOffset1 = -b2Dot(sideNormal, v11);
368	btScalar sideOffset2 = b2Dot(sideNormal, v12);
369
370	// Clip incident edge against extruded edge1 side edges.
371	ClipVertex clipPoints1[2];
372	clipPoints1[0].v.setValue(0,0,0);
373	clipPoints1[1].v.setValue(0,0,0);
374
375	ClipVertex clipPoints2[2];
376	clipPoints2[0].v.setValue(0,0,0);
377	clipPoints2[1].v.setValue(0,0,0);
378
379
380	int np;
381
382	// Clip to box side 1
383	np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
384
385	if (np < 2)
386		return;
387
388	// Clip to negative box side 1
389	np = ClipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, sideOffset2);
390
391	if (np < 2)
392	{
393		return;
394	}
395
396	// Now clipPoints2 contains the clipped points.
397	btVector3 manifoldNormal = flip ? -frontNormal : frontNormal;
398
399	int pointCount = 0;
400	for (int i = 0; i < b2_maxManifoldPoints; ++i)
401	{
402		btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
403
404		if (separation <= 0.0f)
405		{
406
407			//b2ManifoldPoint* cp = manifold->points + pointCount;
408			//btScalar separation = separation;
409			//cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
410			//cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
411
412			manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
413
414//			cp->id = clipPoints2[i].id;
415//			cp->id.features.flip = flip;
416			++pointCount;
417		}
418	}
419
420//	manifold->pointCount = pointCount;}
421}
422