Blender  V3.3
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 {
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 
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>
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 
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],
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;
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;
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 
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;
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 }
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint i1
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble v1
ATTR_WARN_UNUSED_RESULT const BMVert * v2
ATTR_WARN_UNUSED_RESULT const BMVert * v
#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)
btBox2dShape(const btVector3 &boxHalfExtents)
a btBox2dShape is a flat 2D box in the X-Y plane (Z extents are zero)
Definition: btBox2dShape.h:85
int getVertexCount() const
Definition: btBox2dShape.h:135
btCollisionObject
btPersistentManifold * m_manifoldPtr
btPersistentManifold()
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
#define btAssert(x)
Definition: btScalar.h:295
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
SIMD_FORCE_INLINE 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:1033
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE 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:998
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)
virtual void releaseManifold(btPersistentManifold *manifold)=0
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1)=0
btManifoldResult is a helper class to manage contact results.
void setPersistentManifold(btPersistentManifold *manifoldPtr)
SIMD_FORCE_INLINE void refreshContactPoints()
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
IconTextureDrawCall normal
ccl_gpu_kernel_postfix ccl_global float int int int int float bool int offset
ccl_device_inline float2 interp(const float2 &a, const float2 &b, float t)
Definition: math_float2.h:232
static unsigned c
Definition: RandGen.cpp:83
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
SIMD_FORCE_INLINE const btCollisionShape * getCollisionShape() const
SIMD_FORCE_INLINE const btTransform & getWorldTransform() const
SIMD_FORCE_INLINE const btCollisionObject * getCollisionObject() const