Bullet Collision Detection & Physics Library: btBox2dBox2dCollisionAlgorithm.cpp Source File

Bullet Collision Detection & Physics Library

btBox2dBox2dCollisionAlgorithm.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 * The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages 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 freely,
9 subject to the following restrictions:
10
11 1. 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
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
29 btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap)
30 : btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap),
31 m_ownManifold(false),
32 m_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
41 btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm()
42 {
43
44 if (m_ownManifold)
45 {
46 if (m_manifoldPtr)
47 m_dispatcher->releaseManifold(m_manifoldPtr);
48 }
49
50 }
51
52
53 void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
54
55 //#include <stdio.h>
56 void 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
77 btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
78 {
79 //not yet
80 return 1.f;
81 }
82
83
84 struct 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
98 int b2_maxManifoldPoints =2;
99
100 static 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.
135 static 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.
164 static 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
255 static 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
305 void 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 }
ClipVertex::v
btVector3 v
Definition: btBox2dBox2dCollisionAlgorithm.cpp:86
btDispatcher::releaseManifold
virtual void releaseManifold(btPersistentManifold *manifold)=0
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:55
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:281
btCrossS
#define btCrossS(a, s)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:96
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:650
btBox2dBox2dCollisionAlgorithm::m_manifoldPtr
btPersistentManifold * m_manifoldPtr
Definition: btBox2dBox2dCollisionAlgorithm.h:30
btBox2dBox2dCollisionAlgorithm::processCollision
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:56
btBox2dShape.h
btBoxShape.h
btManifoldResult::setPersistentManifold
void setPersistentManifold(btPersistentManifold *manifoldPtr)
Definition: btManifoldResult.h:72
btBox2dShape::getCentroid
const btVector3 & getCentroid() const
Definition: btBox2dShape.h:182
btAssert
#define btAssert(x)
Definition: btScalar.h:114
btActivatingCollisionAlgorithm
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
Definition: btActivatingCollisionAlgorithm.h:22
ClipSegmentToLine
static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], const btVector3 &normal, btScalar offset)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:100
btManifoldResult::refreshContactPoints
void refreshContactPoints()
Definition: btManifoldResult.h:101
btBoxBoxDetector.h
btManifoldResult
btManifoldResult is a helper class to manage contact results.
Definition: btManifoldResult.h:39
btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:77
btCollisionObjectWrapper::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObjectWrapper.h:40
btCollisionAlgorithm::m_dispatcher
btDispatcher * m_dispatcher
Definition: btCollisionAlgorithm.h:60
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:307
btBox2dShape::getVertexCount
int getVertexCount() const
Definition: btBox2dShape.h:146
b2CollidePolygons
void b2CollidePolygons(btManifoldResult *manifold, const btBox2dShape *polyA, const btTransform &xfA, const btBox2dShape *polyB, const btTransform &xfB)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:305
btManifoldResult::addContactPoint
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
Definition: btManifoldResult.cpp:104
FindMaxSeparation
static btScalar FindMaxSeparation(int *edgeIndex, const btBox2dShape *poly1, const btTransform &xf1, const btBox2dShape *poly2, const btTransform &xf2)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:164
btBox2dShape::getVertices
const btVector3 * getVertices() const
Definition: btBox2dShape.h:156
btCollisionObjectWrapper::getCollisionObject
const btCollisionObject * getCollisionObject() const
Definition: btCollisionObjectWrapper.h:39
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:49
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
FindIncidentEdge
static void FindIncidentEdge(ClipVertex c[2], const btBox2dShape *poly1, const btTransform &xf1, int edge1, const btBox2dShape *poly2, const btTransform &xf2)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:255
btDispatcher::getNewManifold
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
btCollisionObjectWrapper.h
btBox2dShape::getNormals
const btVector3 * getNormals() const
Definition: btBox2dShape.h:161
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
EdgeSeparation
static btScalar EdgeSeparation(const btBox2dShape *poly1, const btTransform &xf1, int edge1, const btBox2dShape *poly2, const btTransform &xf2)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:135
btBox2dBox2dCollisionAlgorithm.h
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
ClipVertex
Definition: btBox2dBox2dCollisionAlgorithm.cpp:84
btCollisionDispatcher.h
b2MulT
#define b2MulT(a, b)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:94
btCollisionObjectWrapper::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btCollisionObjectWrapper.h:38
btCollisionObject.h
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:848
btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm
virtual ~btBox2dBox2dCollisionAlgorithm()
Definition: btBox2dBox2dCollisionAlgorithm.cpp:41
b2Dot
#define b2Dot(a, b)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:92
b2_maxManifoldPoints
int b2_maxManifoldPoints
Definition: btBox2dBox2dCollisionAlgorithm.cpp:98
btDispatcherInfo
Definition: btDispatcher.h:30
btDispatcher::needsCollision
virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1)=0
b2Mul
#define b2Mul(a, b)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:93
btCollisionAlgorithmConstructionInfo
Definition: btCollisionAlgorithm.h:32
ClipVertex::id
int id
Definition: btBox2dBox2dCollisionAlgorithm.cpp:87
btCollisionObjectWrapper
Definition: btCollisionObjectWrapper.h:17
btBox2dShape
The btBox2dShape is a box primitive around the origin, its sides axis aligned with length specified b...
Definition: btBox2dShape.h:26
btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm
btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci)
Definition: btBox2dBox2dCollisionAlgorithm.h:33
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btBox2dBox2dCollisionAlgorithm::m_ownManifold
bool m_ownManifold
Definition: btBox2dBox2dCollisionAlgorithm.h:29