Bullet Collision Detection & Physics Library
btBox2dBox2dCollisionAlgorithm.cpp
Go to the documentation of this file.
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
18
26
27#define USE_PERSISTENT_CONTACTS 1
28
30: btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap),
31m_ownManifold(false),
32m_manifoldPtr(mf)
33{
35 {
37 m_ownManifold = true;
38 }
39}
40
42{
43
44 if (m_ownManifold)
45 {
46 if (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>
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
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
78{
79 //not yet
80 return 1.f;
81}
82
83
85{
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
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
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
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}
#define b2Mul(a, b)
static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], const btVector3 &normal, btScalar offset)
static btScalar EdgeSeparation(const btBox2dShape *poly1, const btTransform &xf1, int edge1, const btBox2dShape *poly2, const btTransform &xf2)
static void FindIncidentEdge(ClipVertex c[2], const btBox2dShape *poly1, const btTransform &xf1, int edge1, const btBox2dShape *poly2, const btTransform &xf2)
#define b2Dot(a, b)
#define btCrossS(a, s)
static btScalar FindMaxSeparation(int *edgeIndex, const btBox2dShape *poly1, const btTransform &xf1, const btBox2dShape *poly2, const btTransform &xf2)
void b2CollidePolygons(btManifoldResult *manifold, const btBox2dShape *polyA, const btTransform &xfA, const btBox2dShape *polyB, const btTransform &xfB)
#define b2MulT(a, b)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
#define btAssert(x)
Definition: btScalar.h:131
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci)
The btBox2dShape is a box primitive around the origin, its sides axis aligned with length specified b...
Definition: btBox2dShape.h:27
const btVector3 * getVertices() const
Definition: btBox2dShape.h:157
int getVertexCount() const
Definition: btBox2dShape.h:147
const btVector3 & getCentroid() const
Definition: btBox2dShape.h:183
const btVector3 * getNormals() const
Definition: btBox2dShape.h:162
btCollisionObject can be used to manage collision detection objects.
virtual void releaseManifold(btPersistentManifold *manifold)=0
virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1)=0
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
btManifoldResult is a helper class to manage contact results.
void setPersistentManifold(btPersistentManifold *manifoldPtr)
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
long minDot(const btVector3 *array, long array_count, btScalar &dotOut) const
returns index of minimum dot product between this and vectors in array[]
Definition: btVector3.h:1048
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
long maxDot(const btVector3 *array, long array_count, btScalar &dotOut) const
returns index of maximum dot product between this and vectors in array[]
Definition: btVector3.h:1013
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
const btCollisionShape * getCollisionShape() const
const btCollisionObject * getCollisionObject() const
const btTransform & getWorldTransform() const