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 if (m_ownManifold)
44 {
45 if (m_manifoldPtr)
46 m_dispatcher->releaseManifold(m_manifoldPtr);
47 }
48 }
49
50 void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB);
51
52 //#include <stdio.h>
53 void btBox2dBox2dCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
54 {
55 if (!m_manifoldPtr)
56 return;
57
58 const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape();
59 const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape();
60
61 resultOut->setPersistentManifold(m_manifoldPtr);
62
63 b2CollidePolygons(resultOut, box0, body0Wrap->getWorldTransform(), box1, body1Wrap->getWorldTransform());
64
65 // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
66 if (m_ownManifold)
67 {
68 resultOut->refreshContactPoints();
69 }
70 }
71
72 btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/)
73 {
74 //not yet
75 return 1.f;
76 }
77
78 struct ClipVertex
79 {
80 btVector3 v;
81 int id;
82 //b2ContactID id;
83 //b2ContactID id;
84 };
85
86 #define b2Dot(a, b) (a).dot(b)
87 #define b2Mul(a, b) (a) * (b)
88 #define b2MulT(a, b) (a).transpose() * (b)
89 #define b2Cross(a, b) (a).cross(b)
90 #define btCrossS(a, s) btVector3(s* a.getY(), -s* a.getX(), 0.f)
91
92 int b2_maxManifoldPoints = 2;
93
94 static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
95 const btVector3& normal, btScalar offset)
96 {
97 // Start with no output points
98 int numOut = 0;
99
100 // Calculate the distance of end points to the line
101 btScalar distance0 = b2Dot(normal, vIn[0].v) - offset;
102 btScalar distance1 = b2Dot(normal, vIn[1].v) - offset;
103
104 // If the points are behind the plane
105 if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
106 if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
107
108 // If the points are on different sides of the plane
109 if (distance0 * distance1 < 0.0f)
110 {
111 // Find intersection point of edge and plane
112 btScalar interp = distance0 / (distance0 - distance1);
113 vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
114 if (distance0 > 0.0f)
115 {
116 vOut[numOut].id = vIn[0].id;
117 }
118 else
119 {
120 vOut[numOut].id = vIn[1].id;
121 }
122 ++numOut;
123 }
124
125 return numOut;
126 }
127
128 // Find the separation between poly1 and poly2 for a give edge normal on poly1.
129 static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1,
130 const btBox2dShape* poly2, const btTransform& xf2)
131 {
132 const btVector3* vertices1 = poly1->getVertices();
133 const btVector3* normals1 = poly1->getNormals();
134
135 int count2 = poly2->getVertexCount();
136 const btVector3* vertices2 = poly2->getVertices();
137
138 btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
139
140 // Convert normal from poly1's frame into poly2's frame.
141 btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]);
142 btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World);
143
144 // Find support vertex on poly2 for -normal.
145 int index = 0;
146 btScalar minDot = BT_LARGE_FLOAT;
147
148 if (count2 > 0)
149 index = (int)normal1.minDot(vertices2, count2, minDot);
150
151 btVector3 v1 = b2Mul(xf1, vertices1[edge1]);
152 btVector3 v2 = b2Mul(xf2, vertices2[index]);
153 btScalar separation = b2Dot(v2 - v1, normal1World);
154 return separation;
155 }
156
157 // Find the max separation between poly1 and poly2 using edge normals from poly1.
158 static btScalar FindMaxSeparation(int* edgeIndex,
159 const btBox2dShape* poly1, const btTransform& xf1,
160 const btBox2dShape* poly2, const btTransform& xf2)
161 {
162 int count1 = poly1->getVertexCount();
163 const btVector3* normals1 = poly1->getNormals();
164
165 // Vector pointing from the centroid of poly1 to the centroid of poly2.
166 btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid());
167 btVector3 dLocal1 = b2MulT(xf1.getBasis(), d);
168
169 // Find edge normal on poly1 that has the largest projection onto d.
170 int edge = 0;
171 btScalar maxDot;
172 if (count1 > 0)
173 edge = (int)dLocal1.maxDot(normals1, count1, maxDot);
174
175 // Get the separation for the edge normal.
176 btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
177 if (s > 0.0f)
178 {
179 return s;
180 }
181
182 // Check the separation for the previous edge normal.
183 int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
184 btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
185 if (sPrev > 0.0f)
186 {
187 return sPrev;
188 }
189
190 // Check the separation for the next edge normal.
191 int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
192 btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
193 if (sNext > 0.0f)
194 {
195 return sNext;
196 }
197
198 // Find the best edge and the search direction.
199 int bestEdge;
200 btScalar bestSeparation;
201 int increment;
202 if (sPrev > s && sPrev > sNext)
203 {
204 increment = -1;
205 bestEdge = prevEdge;
206 bestSeparation = sPrev;
207 }
208 else if (sNext > s)
209 {
210 increment = 1;
211 bestEdge = nextEdge;
212 bestSeparation = sNext;
213 }
214 else
215 {
216 *edgeIndex = edge;
217 return s;
218 }
219
220 // Perform a local search for the best edge normal.
221 for (;;)
222 {
223 if (increment == -1)
224 edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
225 else
226 edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
227
228 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
229 if (s > 0.0f)
230 {
231 return s;
232 }
233
234 if (s > bestSeparation)
235 {
236 bestEdge = edge;
237 bestSeparation = s;
238 }
239 else
240 {
241 break;
242 }
243 }
244
245 *edgeIndex = bestEdge;
246 return bestSeparation;
247 }
248
249 static void FindIncidentEdge(ClipVertex c[2],
250 const btBox2dShape* poly1, const btTransform& xf1, int edge1,
251 const btBox2dShape* poly2, const btTransform& xf2)
252 {
253 const btVector3* normals1 = poly1->getNormals();
254
255 int count2 = poly2->getVertexCount();
256 const btVector3* vertices2 = poly2->getVertices();
257 const btVector3* normals2 = poly2->getNormals();
258
259 btAssert(0 <= edge1 && edge1 < poly1->getVertexCount());
260
261 // Get the normal of the reference edge in poly2's frame.
262 btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1]));
263
264 // Find the incident edge on poly2.
265 int index = 0;
266 btScalar minDot = BT_LARGE_FLOAT;
267 for (int i = 0; i < count2; ++i)
268 {
269 btScalar dot = b2Dot(normal1, normals2[i]);
270 if (dot < minDot)
271 {
272 minDot = dot;
273 index = i;
274 }
275 }
276
277 // Build the clip vertices for the incident edge.
278 int i1 = index;
279 int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
280
281 c[0].v = b2Mul(xf2, vertices2[i1]);
282 // c[0].id.features.referenceEdge = (unsigned char)edge1;
283 // c[0].id.features.incidentEdge = (unsigned char)i1;
284 // c[0].id.features.incidentVertex = 0;
285
286 c[1].v = b2Mul(xf2, vertices2[i2]);
287 // c[1].id.features.referenceEdge = (unsigned char)edge1;
288 // c[1].id.features.incidentEdge = (unsigned char)i2;
289 // c[1].id.features.incidentVertex = 1;
290 }
291
292 // Find edge normal of max separation on A - return if separating axis is found
293 // Find edge normal of max separation on B - return if separation axis is found
294 // Choose reference edge as min(minA, minB)
295 // Find incident edge
296 // Clip
297
298 // The normal points from 1 to 2
299 void b2CollidePolygons(btManifoldResult* manifold,
300 const btBox2dShape* polyA, const btTransform& xfA,
301 const btBox2dShape* polyB, const btTransform& xfB)
302 {
303 int edgeA = 0;
304 btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
305 if (separationA > 0.0f)
306 return;
307
308 int edgeB = 0;
309 btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
310 if (separationB > 0.0f)
311 return;
312
313 const btBox2dShape* poly1; // reference poly
314 const btBox2dShape* poly2; // incident poly
315 btTransform xf1, xf2;
316 int edge1; // reference edge
317 unsigned char flip;
318 const btScalar k_relativeTol = 0.98f;
319 const btScalar k_absoluteTol = 0.001f;
320
321 // TODO_ERIN use "radius" of poly for absolute tolerance.
322 if (separationB > k_relativeTol * separationA + k_absoluteTol)
323 {
324 poly1 = polyB;
325 poly2 = polyA;
326 xf1 = xfB;
327 xf2 = xfA;
328 edge1 = edgeB;
329 flip = 1;
330 }
331 else
332 {
333 poly1 = polyA;
334 poly2 = polyB;
335 xf1 = xfA;
336 xf2 = xfB;
337 edge1 = edgeA;
338 flip = 0;
339 }
340
341 ClipVertex incidentEdge[2];
342 FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
343
344 int count1 = poly1->getVertexCount();
345 const btVector3* vertices1 = poly1->getVertices();
346
347 btVector3 v11 = vertices1[edge1];
348 btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
349
350 //btVector3 dv = v12 - v11;
351 btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11);
352 sideNormal.normalize();
353 btVector3 frontNormal = btCrossS(sideNormal, 1.0f);
354
355 v11 = b2Mul(xf1, v11);
356 v12 = b2Mul(xf1, v12);
357
358 btScalar frontOffset = b2Dot(frontNormal, v11);
359 btScalar sideOffset1 = -b2Dot(sideNormal, v11);
360 btScalar sideOffset2 = b2Dot(sideNormal, v12);
361
362 // Clip incident edge against extruded edge1 side edges.
363 ClipVertex clipPoints1[2];
364 clipPoints1[0].v.setValue(0, 0, 0);
365 clipPoints1[1].v.setValue(0, 0, 0);
366
367 ClipVertex clipPoints2[2];
368 clipPoints2[0].v.setValue(0, 0, 0);
369 clipPoints2[1].v.setValue(0, 0, 0);
370
371 int np;
372
373 // Clip to box side 1
374 np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
375
376 if (np < 2)
377 return;
378
379 // Clip to negative box side 1
380 np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
381
382 if (np < 2)
383 {
384 return;
385 }
386
387 // Now clipPoints2 contains the clipped points.
388 btVector3 manifoldNormal = flip ? -frontNormal : frontNormal;
389
390 int pointCount = 0;
391 for (int i = 0; i < b2_maxManifoldPoints; ++i)
392 {
393 btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
394
395 if (separation <= 0.0f)
396 {
397 //b2ManifoldPoint* cp = manifold->points + pointCount;
398 //btScalar separation = separation;
399 //cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
400 //cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
401
402 manifold->addContactPoint(-manifoldNormal, clipPoints2[i].v, separation);
403
404 // cp->id = clipPoints2[i].id;
405 // cp->id.features.flip = flip;
406 ++pointCount;
407 }
408 }
409
410 // manifold->pointCount = pointCount;}
411 }
ClipVertex::v
btVector3 v
Definition: btBox2dBox2dCollisionAlgorithm.cpp:80
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:63
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:296
btCrossS
#define btCrossS(a, s)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:90
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
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:53
btBox2dShape.h
btBoxShape.h
btManifoldResult::setPersistentManifold
void setPersistentManifold(btPersistentManifold *manifoldPtr)
Definition: btManifoldResult.h:77
btBox2dShape::getCentroid
const btVector3 & getCentroid() const
Definition: btBox2dShape.h:164
btAssert
#define btAssert(x)
Definition: btScalar.h:133
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:94
btManifoldResult::refreshContactPoints
void refreshContactPoints()
Definition: btManifoldResult.h:105
btBoxBoxDetector.h
btManifoldResult
btManifoldResult is a helper class to manage contact results.
Definition: btManifoldResult.h:47
btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:72
btCollisionObjectWrapper::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObjectWrapper.h:40
btCollisionAlgorithm::m_dispatcher
btDispatcher * m_dispatcher
Definition: btCollisionAlgorithm.h:56
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btBox2dShape::getVertexCount
int getVertexCount() const
Definition: btBox2dShape.h:135
b2CollidePolygons
void b2CollidePolygons(btManifoldResult *manifold, const btBox2dShape *polyA, const btTransform &xfA, const btBox2dShape *polyB, const btTransform &xfB)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:299
btManifoldResult::addContactPoint
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
Definition: btManifoldResult.cpp:105
FindMaxSeparation
static btScalar FindMaxSeparation(int *edgeIndex, const btBox2dShape *poly1, const btTransform &xf1, const btBox2dShape *poly2, const btTransform &xf2)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:158
btBox2dShape::getVertices
const btVector3 * getVertices() const
Definition: btBox2dShape.h:145
btCollisionObjectWrapper::getCollisionObject
const btCollisionObject * getCollisionObject() const
Definition: btCollisionObjectWrapper.h:39
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:48
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
FindIncidentEdge
static void FindIncidentEdge(ClipVertex c[2], const btBox2dShape *poly1, const btTransform &xf1, int edge1, const btBox2dShape *poly2, const btTransform &xf2)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:249
btDispatcher::getNewManifold
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
btCollisionObjectWrapper.h
btBox2dShape::getNormals
const btVector3 * getNormals() const
Definition: btBox2dShape.h:150
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
EdgeSeparation
static btScalar EdgeSeparation(const btBox2dShape *poly1, const btTransform &xf1, int edge1, const btBox2dShape *poly2, const btTransform &xf2)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:129
btBox2dBox2dCollisionAlgorithm.h
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
ClipVertex
Definition: btBox2dBox2dCollisionAlgorithm.cpp:78
btCollisionDispatcher.h
b2MulT
#define b2MulT(a, b)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:88
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:888
btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm
virtual ~btBox2dBox2dCollisionAlgorithm()
Definition: btBox2dBox2dCollisionAlgorithm.cpp:41
b2Dot
#define b2Dot(a, b)
Definition: btBox2dBox2dCollisionAlgorithm.cpp:86
b2_maxManifoldPoints
int b2_maxManifoldPoints
Definition: btBox2dBox2dCollisionAlgorithm.cpp:92
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:87
btCollisionAlgorithmConstructionInfo
Definition: btCollisionAlgorithm.h:32
ClipVertex::id
int id
Definition: btBox2dBox2dCollisionAlgorithm.cpp:81
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:294
btBox2dBox2dCollisionAlgorithm::m_ownManifold
bool m_ownManifold
Definition: btBox2dBox2dCollisionAlgorithm.h:29