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