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 
44  if (m_ownManifold)
45  {
46  if (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>
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 
84 struct ClipVertex
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 
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
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 }
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:280
#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)
#define btAssert(x)
Definition: btScalar.h:113
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)
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
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:156
const btTransform & getWorldTransform() const
btCollisionObject can be used to manage collision detection objects.
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
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 btCollisionShape * getCollisionShape() const
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
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:34
const btVector3 & getCentroid() const
Definition: btBox2dShape.h:182
#define b2MulT(a, b)
int getVertexCount() const
Definition: btBox2dShape.h:146
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:845
#define b2Dot(a, b)
const btVector3 * getNormals() const
Definition: btBox2dShape.h:161
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:278
const btCollisionObject * getCollisionObject() const