Blender  V3.3
btBoxBoxDetector.cpp
Go to the documentation of this file.
1 /*
2  * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
3  * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
4  * All rights reserved. Email: russ@q12.org Web: www.q12.org
5  Bullet Continuous Collision Detection and Physics Library
6  Bullet is Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
7 
8 This software is provided 'as-is', without any express or implied warranty.
9 In no event will the authors be held liable for any damages arising from the use of this software.
10 Permission is granted to anyone to use this software for any purpose,
11 including commercial applications, and to alter it and redistribute it freely,
12 subject to the following restrictions:
13 
14 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.
15 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
16 3. This notice may not be removed or altered from any source distribution.
17 */
18 
20 
21 #include "btBoxBoxDetector.h"
23 
24 #include <float.h>
25 #include <string.h>
26 
28  : m_box1(box1),
29  m_box2(box2)
30 {
31 }
32 
33 // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
34 // generate contact points. this returns 0 if there is no contact otherwise
35 // it returns the number of contacts generated.
36 // `normal' returns the contact normal.
37 // `depth' returns the maximum penetration depth along that normal.
38 // `return_code' returns a number indicating the type of contact that was
39 // detected:
40 // 1,2,3 = box 2 intersects with a face of box 1
41 // 4,5,6 = box 1 intersects with a face of box 2
42 // 7..15 = edge-edge contact
43 // `maxc' is the maximum number of contacts allowed to be generated, i.e.
44 // the size of the `contact' array.
45 // `contact' and `skip' are the contact array information provided to the
46 // collision functions. this function only fills in the position and depth
47 // fields.
48 struct dContactGeom;
49 #define dDOTpq(a, b, p, q) ((a)[0] * (b)[0] + (a)[p] * (b)[q] + (a)[2 * (p)] * (b)[2 * (q)])
50 #define dInfinity FLT_MAX
51 
52 /*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
53 PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
54 PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
55 PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
56 */
57 static btScalar dDOT(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 1); }
58 static btScalar dDOT44(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 4); }
59 static btScalar dDOT41(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 1); }
60 static btScalar dDOT14(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 4); }
61 #define dMULTIPLYOP1_331(A, op, B, C) \
62  { \
63  (A)[0] op dDOT41((B), (C)); \
64  (A)[1] op dDOT41((B + 1), (C)); \
65  (A)[2] op dDOT41((B + 2), (C)); \
66  }
67 
68 #define dMULTIPLYOP0_331(A, op, B, C) \
69  { \
70  (A)[0] op dDOT((B), (C)); \
71  (A)[1] op dDOT((B + 4), (C)); \
72  (A)[2] op dDOT((B + 8), (C)); \
73  }
74 
75 #define dMULTIPLY1_331(A, B, C) dMULTIPLYOP1_331(A, =, B, C)
76 #define dMULTIPLY0_331(A, B, C) dMULTIPLYOP0_331(A, =, B, C)
77 
78 typedef btScalar dMatrix3[4 * 3];
79 
80 void dLineClosestApproach(const btVector3& pa, const btVector3& ua,
81  const btVector3& pb, const btVector3& ub,
82  btScalar* alpha, btScalar* beta);
83 void dLineClosestApproach(const btVector3& pa, const btVector3& ua,
84  const btVector3& pb, const btVector3& ub,
85  btScalar* alpha, btScalar* beta)
86 {
87  btVector3 p;
88  p[0] = pb[0] - pa[0];
89  p[1] = pb[1] - pa[1];
90  p[2] = pb[2] - pa[2];
91  btScalar uaub = dDOT(ua, ub);
92  btScalar q1 = dDOT(ua, p);
93  btScalar q2 = -dDOT(ub, p);
94  btScalar d = 1 - uaub * uaub;
95  if (d <= btScalar(0.0001f))
96  {
97  // @@@ this needs to be made more robust
98  *alpha = 0;
99  *beta = 0;
100  }
101  else
102  {
103  d = 1.f / d;
104  *alpha = (q1 + uaub * q2) * d;
105  *beta = (uaub * q1 + q2) * d;
106  }
107 }
108 
109 // find all the intersection points between the 2D rectangle with vertices
110 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
111 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
112 //
113 // the intersection points are returned as x,y pairs in the 'ret' array.
114 // the number of intersection points is returned by the function (this will
115 // be in the range 0 to 8).
116 
117 static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
118 {
119  // q (and r) contain nq (and nr) coordinate points for the current (and
120  // chopped) polygons
121  int nq = 4, nr = 0;
122  btScalar buffer[16];
123  btScalar* q = p;
124  btScalar* r = ret;
125  for (int dir = 0; dir <= 1; dir++)
126  {
127  // direction notation: xy[0] = x axis, xy[1] = y axis
128  for (int sign = -1; sign <= 1; sign += 2)
129  {
130  // chop q along the line xy[dir] = sign*h[dir]
131  btScalar* pq = q;
132  btScalar* pr = r;
133  nr = 0;
134  for (int i = nq; i > 0; i--)
135  {
136  // go through all points in q and all lines between adjacent points
137  if (sign * pq[dir] < h[dir])
138  {
139  // this point is inside the chopping line
140  pr[0] = pq[0];
141  pr[1] = pq[1];
142  pr += 2;
143  nr++;
144  if (nr & 8)
145  {
146  q = r;
147  goto done;
148  }
149  }
150  btScalar* nextq = (i > 1) ? pq + 2 : q;
151  if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir]))
152  {
153  // this line crosses the chopping line
154  pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) /
155  (nextq[dir] - pq[dir]) * (sign * h[dir] - pq[dir]);
156  pr[dir] = sign * h[dir];
157  pr += 2;
158  nr++;
159  if (nr & 8)
160  {
161  q = r;
162  goto done;
163  }
164  }
165  pq += 2;
166  }
167  q = r;
168  r = (q == ret) ? buffer : ret;
169  nq = nr;
170  }
171  }
172 done:
173  if (q != ret) memcpy(ret, q, nr * 2 * sizeof(btScalar));
174  return nr;
175 }
176 
177 #define M__PI 3.14159265f
178 
179 // given n points in the plane (array p, of size 2*n), generate m points that
180 // best represent the whole set. the definition of 'best' here is not
181 // predetermined - the idea is to select points that give good box-box
182 // collision detection behavior. the chosen point indexes are returned in the
183 // array iret (of size m). 'i0' is always the first entry in the array.
184 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
185 // in the range [0..n-1].
186 
187 void cullPoints2(int n, btScalar p[], int m, int i0, int iret[]);
188 void cullPoints2(int n, btScalar p[], int m, int i0, int iret[])
189 {
190  // compute the centroid of the polygon in cx,cy
191  int i, j;
192  btScalar a, cx, cy, q;
193  if (n == 1)
194  {
195  cx = p[0];
196  cy = p[1];
197  }
198  else if (n == 2)
199  {
200  cx = btScalar(0.5) * (p[0] + p[2]);
201  cy = btScalar(0.5) * (p[1] + p[3]);
202  }
203  else
204  {
205  a = 0;
206  cx = 0;
207  cy = 0;
208  for (i = 0; i < (n - 1); i++)
209  {
210  q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1];
211  a += q;
212  cx += q * (p[i * 2] + p[i * 2 + 2]);
213  cy += q * (p[i * 2 + 1] + p[i * 2 + 3]);
214  }
215  q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1];
216  if (btFabs(a + q) > SIMD_EPSILON)
217  {
218  a = 1.f / (btScalar(3.0) * (a + q));
219  }
220  else
221  {
222  a = BT_LARGE_FLOAT;
223  }
224  cx = a * (cx + q * (p[n * 2 - 2] + p[0]));
225  cy = a * (cy + q * (p[n * 2 - 1] + p[1]));
226  }
227 
228  // compute the angle of each point w.r.t. the centroid
229  btScalar A[8];
230  for (i = 0; i < n; i++) A[i] = btAtan2(p[i * 2 + 1] - cy, p[i * 2] - cx);
231 
232  // search for points that have angles closest to A[i0] + i*(2*pi/m).
233  int avail[8];
234  for (i = 0; i < n; i++) avail[i] = 1;
235  avail[i0] = 0;
236  iret[0] = i0;
237  iret++;
238  for (j = 1; j < m; j++)
239  {
240  a = btScalar(j) * (2 * M__PI / m) + A[i0];
241  if (a > M__PI) a -= 2 * M__PI;
242  btScalar maxdiff = 1e9, diff;
243 
244  *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
245 
246  for (i = 0; i < n; i++)
247  {
248  if (avail[i])
249  {
250  diff = btFabs(A[i] - a);
251  if (diff > M__PI) diff = 2 * M__PI - diff;
252  if (diff < maxdiff)
253  {
254  maxdiff = diff;
255  *iret = i;
256  }
257  }
258  }
259 #if defined(DEBUG) || defined(_DEBUG)
260  btAssert(*iret != i0); // ensure iret got set
261 #endif
262  avail[*iret] = 0;
263  iret++;
264  }
265 }
266 
267 int dBoxBox2(const btVector3& p1, const dMatrix3 R1,
268  const btVector3& side1, const btVector3& p2,
269  const dMatrix3 R2, const btVector3& side2,
270  btVector3& normal, btScalar* depth, int* return_code,
271  int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output);
272 int dBoxBox2(const btVector3& p1, const dMatrix3 R1,
273  const btVector3& side1, const btVector3& p2,
274  const dMatrix3 R2, const btVector3& side2,
275  btVector3& normal, btScalar* depth, int* return_code,
276  int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output)
277 {
278  const btScalar fudge_factor = btScalar(1.05);
279  btVector3 p, pp, normalC(0.f, 0.f, 0.f);
280  const btScalar* normalR = 0;
281  btScalar A[3], B[3], R11, R12, R13, R21, R22, R23, R31, R32, R33,
282  Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33, s, s2, l;
283  int i, j, invert_normal, code;
284 
285  // get vector from centers of box 1 to box 2, relative to box 1
286  p = p2 - p1;
287  dMULTIPLY1_331(pp, R1, p); // get pp = p relative to body 1
288 
289  // get side lengths / 2
290  A[0] = side1[0] * btScalar(0.5);
291  A[1] = side1[1] * btScalar(0.5);
292  A[2] = side1[2] * btScalar(0.5);
293  B[0] = side2[0] * btScalar(0.5);
294  B[1] = side2[1] * btScalar(0.5);
295  B[2] = side2[2] * btScalar(0.5);
296 
297  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
298  R11 = dDOT44(R1 + 0, R2 + 0);
299  R12 = dDOT44(R1 + 0, R2 + 1);
300  R13 = dDOT44(R1 + 0, R2 + 2);
301  R21 = dDOT44(R1 + 1, R2 + 0);
302  R22 = dDOT44(R1 + 1, R2 + 1);
303  R23 = dDOT44(R1 + 1, R2 + 2);
304  R31 = dDOT44(R1 + 2, R2 + 0);
305  R32 = dDOT44(R1 + 2, R2 + 1);
306  R33 = dDOT44(R1 + 2, R2 + 2);
307 
308  Q11 = btFabs(R11);
309  Q12 = btFabs(R12);
310  Q13 = btFabs(R13);
311  Q21 = btFabs(R21);
312  Q22 = btFabs(R22);
313  Q23 = btFabs(R23);
314  Q31 = btFabs(R31);
315  Q32 = btFabs(R32);
316  Q33 = btFabs(R33);
317 
318  // for all 15 possible separating axes:
319  // * see if the axis separates the boxes. if so, return 0.
320  // * find the depth of the penetration along the separating axis (s2)
321  // * if this is the largest depth so far, record it.
322  // the normal vector will be set to the separating axis with the smallest
323  // depth. note: normalR is set to point to a column of R1 or R2 if that is
324  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
325  // set to a vector relative to body 1. invert_normal is 1 if the sign of
326  // the normal should be flipped.
327 
328 #define TST(expr1, expr2, norm, cc) \
329  s2 = btFabs(expr1) - (expr2); \
330  if (s2 > 0) return 0; \
331  if (s2 > s) \
332  { \
333  s = s2; \
334  normalR = norm; \
335  invert_normal = ((expr1) < 0); \
336  code = (cc); \
337  }
338 
339  s = -dInfinity;
340  invert_normal = 0;
341  code = 0;
342 
343  // separating axis = u1,u2,u3
344  TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1 + 0, 1);
345  TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1 + 1, 2);
346  TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1 + 2, 3);
347 
348  // separating axis = v1,v2,v3
349  TST(dDOT41(R2 + 0, p), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2 + 0, 4);
350  TST(dDOT41(R2 + 1, p), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2 + 1, 5);
351  TST(dDOT41(R2 + 2, p), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2 + 2, 6);
352 
353  // note: cross product axes need to be scaled when s is computed.
354  // normal (n1,n2,n3) is relative to box 1.
355 #undef TST
356 #define TST(expr1, expr2, n1, n2, n3, cc) \
357  s2 = btFabs(expr1) - (expr2); \
358  if (s2 > SIMD_EPSILON) return 0; \
359  l = btSqrt((n1) * (n1) + (n2) * (n2) + (n3) * (n3)); \
360  if (l > SIMD_EPSILON) \
361  { \
362  s2 /= l; \
363  if (s2 * fudge_factor > s) \
364  { \
365  s = s2; \
366  normalR = 0; \
367  normalC[0] = (n1) / l; \
368  normalC[1] = (n2) / l; \
369  normalC[2] = (n3) / l; \
370  invert_normal = ((expr1) < 0); \
371  code = (cc); \
372  } \
373  }
374 
375  btScalar fudge2(1.0e-5f);
376 
377  Q11 += fudge2;
378  Q12 += fudge2;
379  Q13 += fudge2;
380 
381  Q21 += fudge2;
382  Q22 += fudge2;
383  Q23 += fudge2;
384 
385  Q31 += fudge2;
386  Q32 += fudge2;
387  Q33 += fudge2;
388 
389  // separating axis = u1 x (v1,v2,v3)
390  TST(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, 7);
391  TST(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, 8);
392  TST(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, 9);
393 
394  // separating axis = u2 x (v1,v2,v3)
395  TST(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, 10);
396  TST(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, 11);
397  TST(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, 12);
398 
399  // separating axis = u3 x (v1,v2,v3)
400  TST(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, 13);
401  TST(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, 14);
402  TST(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, 15);
403 
404 #undef TST
405 
406  if (!code) return 0;
407 
408  // if we get to this point, the boxes interpenetrate. compute the normal
409  // in global coordinates.
410  if (normalR)
411  {
412  normal[0] = normalR[0];
413  normal[1] = normalR[4];
414  normal[2] = normalR[8];
415  }
416  else
417  {
418  dMULTIPLY0_331(normal, R1, normalC);
419  }
420  if (invert_normal)
421  {
422  normal[0] = -normal[0];
423  normal[1] = -normal[1];
424  normal[2] = -normal[2];
425  }
426  *depth = -s;
427 
428  // compute contact point(s)
429 
430  if (code > 6)
431  {
432  // an edge from box 1 touches an edge from box 2.
433  // find a point pa on the intersecting edge of box 1
434  btVector3 pa;
435  btScalar sign;
436  for (i = 0; i < 3; i++) pa[i] = p1[i];
437  for (j = 0; j < 3; j++)
438  {
439  sign = (dDOT14(normal, R1 + j) > 0) ? btScalar(1.0) : btScalar(-1.0);
440  for (i = 0; i < 3; i++) pa[i] += sign * A[j] * R1[i * 4 + j];
441  }
442 
443  // find a point pb on the intersecting edge of box 2
444  btVector3 pb;
445  for (i = 0; i < 3; i++) pb[i] = p2[i];
446  for (j = 0; j < 3; j++)
447  {
448  sign = (dDOT14(normal, R2 + j) > 0) ? btScalar(-1.0) : btScalar(1.0);
449  for (i = 0; i < 3; i++) pb[i] += sign * B[j] * R2[i * 4 + j];
450  }
451 
452  btScalar alpha, beta;
453  btVector3 ua, ub;
454  for (i = 0; i < 3; i++) ua[i] = R1[((code)-7) / 3 + i * 4];
455  for (i = 0; i < 3; i++) ub[i] = R2[((code)-7) % 3 + i * 4];
456 
457  dLineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
458  for (i = 0; i < 3; i++) pa[i] += ua[i] * alpha;
459  for (i = 0; i < 3; i++) pb[i] += ub[i] * beta;
460 
461  {
462  //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
463  //contact[0].depth = *depth;
464  btVector3 pointInWorld;
465 
466 #ifdef USE_CENTER_POINT
467  for (i = 0; i < 3; i++)
468  pointInWorld[i] = (pa[i] + pb[i]) * btScalar(0.5);
469  output.addContactPoint(-normal, pointInWorld, -*depth);
470 #else
471  output.addContactPoint(-normal, pb, -*depth);
472 
473 #endif //
474  *return_code = code;
475  }
476  return 1;
477  }
478 
479  // okay, we have a face-something intersection (because the separating
480  // axis is perpendicular to a face). define face 'a' to be the reference
481  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
482  // the incident face (the closest face of the other box).
483 
484  const btScalar *Ra, *Rb, *pa, *pb, *Sa, *Sb;
485  if (code <= 3)
486  {
487  Ra = R1;
488  Rb = R2;
489  pa = p1;
490  pb = p2;
491  Sa = A;
492  Sb = B;
493  }
494  else
495  {
496  Ra = R2;
497  Rb = R1;
498  pa = p2;
499  pb = p1;
500  Sa = B;
501  Sb = A;
502  }
503 
504  // nr = normal vector of reference face dotted with axes of incident box.
505  // anr = absolute values of nr.
506  btVector3 normal2, nr, anr;
507  if (code <= 3)
508  {
509  normal2[0] = normal[0];
510  normal2[1] = normal[1];
511  normal2[2] = normal[2];
512  }
513  else
514  {
515  normal2[0] = -normal[0];
516  normal2[1] = -normal[1];
517  normal2[2] = -normal[2];
518  }
519  dMULTIPLY1_331(nr, Rb, normal2);
520  anr[0] = btFabs(nr[0]);
521  anr[1] = btFabs(nr[1]);
522  anr[2] = btFabs(nr[2]);
523 
524  // find the largest compontent of anr: this corresponds to the normal
525  // for the indident face. the other axis numbers of the indicent face
526  // are stored in a1,a2.
527  int lanr, a1, a2;
528  if (anr[1] > anr[0])
529  {
530  if (anr[1] > anr[2])
531  {
532  a1 = 0;
533  lanr = 1;
534  a2 = 2;
535  }
536  else
537  {
538  a1 = 0;
539  a2 = 1;
540  lanr = 2;
541  }
542  }
543  else
544  {
545  if (anr[0] > anr[2])
546  {
547  lanr = 0;
548  a1 = 1;
549  a2 = 2;
550  }
551  else
552  {
553  a1 = 0;
554  a2 = 1;
555  lanr = 2;
556  }
557  }
558 
559  // compute center point of incident face, in reference-face coordinates
561  if (nr[lanr] < 0)
562  {
563  for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
564  }
565  else
566  {
567  for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
568  }
569 
570  // find the normal and non-normal axis numbers of the reference box
571  int codeN, code1, code2;
572  if (code <= 3)
573  codeN = code - 1;
574  else
575  codeN = code - 4;
576  if (codeN == 0)
577  {
578  code1 = 1;
579  code2 = 2;
580  }
581  else if (codeN == 1)
582  {
583  code1 = 0;
584  code2 = 2;
585  }
586  else
587  {
588  code1 = 0;
589  code2 = 1;
590  }
591 
592  // find the four corners of the incident face, in reference-face coordinates
593  btScalar quad[8]; // 2D coordinate of incident face (x,y pairs)
594  btScalar c1, c2, m11, m12, m21, m22;
595  c1 = dDOT14(center, Ra + code1);
596  c2 = dDOT14(center, Ra + code2);
597  // optimize this? - we have already computed this data above, but it is not
598  // stored in an easy-to-index format. for now it's quicker just to recompute
599  // the four dot products.
600  m11 = dDOT44(Ra + code1, Rb + a1);
601  m12 = dDOT44(Ra + code1, Rb + a2);
602  m21 = dDOT44(Ra + code2, Rb + a1);
603  m22 = dDOT44(Ra + code2, Rb + a2);
604  {
605  btScalar k1 = m11 * Sb[a1];
606  btScalar k2 = m21 * Sb[a1];
607  btScalar k3 = m12 * Sb[a2];
608  btScalar k4 = m22 * Sb[a2];
609  quad[0] = c1 - k1 - k3;
610  quad[1] = c2 - k2 - k4;
611  quad[2] = c1 - k1 + k3;
612  quad[3] = c2 - k2 + k4;
613  quad[4] = c1 + k1 + k3;
614  quad[5] = c2 + k2 + k4;
615  quad[6] = c1 + k1 - k3;
616  quad[7] = c2 + k2 - k4;
617  }
618 
619  // find the size of the reference face
620  btScalar rect[2];
621  rect[0] = Sa[code1];
622  rect[1] = Sa[code2];
623 
624  // intersect the incident and reference faces
625  btScalar ret[16];
626  int n = intersectRectQuad2(rect, quad, ret);
627  if (n < 1) return 0; // this should never happen
628 
629  // convert the intersection points into reference-face coordinates,
630  // and compute the contact position and depth for each point. only keep
631  // those points that have a positive (penetrating) depth. delete points in
632  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
633  btScalar point[3 * 8]; // penetrating contact points
634  btScalar dep[8]; // depths for those points
635  btScalar det1 = 1.f / (m11 * m22 - m12 * m21);
636  m11 *= det1;
637  m12 *= det1;
638  m21 *= det1;
639  m22 *= det1;
640  int cnum = 0; // number of penetrating contact points found
641  for (j = 0; j < n; j++)
642  {
643  btScalar k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
644  btScalar k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
645  for (i = 0; i < 3; i++) point[cnum * 3 + i] =
646  center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
647  dep[cnum] = Sa[codeN] - dDOT(normal2, point + cnum * 3);
648  if (dep[cnum] >= 0)
649  {
650  ret[cnum * 2] = ret[j * 2];
651  ret[cnum * 2 + 1] = ret[j * 2 + 1];
652  cnum++;
653  }
654  }
655  if (cnum < 1) return 0; // this should never happen
656 
657  // we can't generate more contacts than we actually have
658  if (maxc > cnum) maxc = cnum;
659  if (maxc < 1) maxc = 1;
660 
661  if (cnum <= maxc)
662  {
663  if (code < 4)
664  {
665  // we have less contacts than we need, so we use them all
666  for (j = 0; j < cnum; j++)
667  {
668  btVector3 pointInWorld;
669  for (i = 0; i < 3; i++)
670  pointInWorld[i] = point[j * 3 + i] + pa[i];
671  output.addContactPoint(-normal, pointInWorld, -dep[j]);
672  }
673  }
674  else
675  {
676  // we have less contacts than we need, so we use them all
677  for (j = 0; j < cnum; j++)
678  {
679  btVector3 pointInWorld;
680  for (i = 0; i < 3; i++)
681  pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j];
682  //pointInWorld[i] = point[j*3+i] + pa[i];
683  output.addContactPoint(-normal, pointInWorld, -dep[j]);
684  }
685  }
686  }
687  else
688  {
689  // we have more contacts than are wanted, some of them must be culled.
690  // find the deepest point, it is always the first contact.
691  int i1 = 0;
692  btScalar maxdepth = dep[0];
693  for (i = 1; i < cnum; i++)
694  {
695  if (dep[i] > maxdepth)
696  {
697  maxdepth = dep[i];
698  i1 = i;
699  }
700  }
701 
702  int iret[8];
703  cullPoints2(cnum, ret, maxc, i1, iret);
704 
705  for (j = 0; j < maxc; j++)
706  {
707  // dContactGeom *con = CONTACT(contact,skip*j);
708  // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
709  // con->depth = dep[iret[j]];
710 
711  btVector3 posInWorld;
712  for (i = 0; i < 3; i++)
713  posInWorld[i] = point[iret[j] * 3 + i] + pa[i];
714  if (code < 4)
715  {
716  output.addContactPoint(-normal, posInWorld, -dep[iret[j]]);
717  }
718  else
719  {
720  output.addContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]);
721  }
722  }
723  cnum = maxc;
724  }
725 
726  *return_code = code;
727  return cnum;
728 }
729 
730 void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* /*debugDraw*/, bool /*swapResults*/)
731 {
732  const btTransform& transformA = input.m_transformA;
733  const btTransform& transformB = input.m_transformB;
734 
735  int skip = 0;
736  dContactGeom* contact = 0;
737 
738  dMatrix3 R1;
739  dMatrix3 R2;
740 
741  for (int j = 0; j < 3; j++)
742  {
743  R1[0 + 4 * j] = transformA.getBasis()[j].x();
744  R2[0 + 4 * j] = transformB.getBasis()[j].x();
745 
746  R1[1 + 4 * j] = transformA.getBasis()[j].y();
747  R2[1 + 4 * j] = transformB.getBasis()[j].y();
748 
749  R1[2 + 4 * j] = transformA.getBasis()[j].z();
750  R2[2 + 4 * j] = transformB.getBasis()[j].z();
751  }
752 
754  btScalar depth;
755  int return_code;
756  int maxc = 4;
757 
758  dBoxBox2(transformA.getOrigin(),
759  R1,
760  2.f * m_box1->getHalfExtentsWithMargin(),
761  transformB.getOrigin(),
762  R2,
763  2.f * m_box2->getHalfExtentsWithMargin(),
764  normal, &depth, &return_code,
765  maxc, contact, skip,
766  output);
767 }
NSNotificationCenter * center
_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 GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble GLdouble r _GL_VOID_RET _GL_VOID GLfloat GLfloat r _GL_VOID_RET _GL_VOID GLint GLint r _GL_VOID_RET _GL_VOID GLshort GLshort r _GL_VOID_RET _GL_VOID GLdouble GLdouble r
_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
in reality light always falls off quadratically Particle Retrieve the data of the particle that spawned the object for example to give variation to multiple instances of an object Point Retrieve information about points in a point cloud Retrieve the edges of an object as it appears to Cycles topology will always appear triangulated Convert a blackbody temperature to an RGB value Normal Generate a perturbed normal from an RGB normal map image Typically used for faking highly detailed surfaces Generate an OSL shader from a file or text data block Image Sample an image file as a texture Sky Generate a procedural sky texture Noise Generate fractal Perlin noise Wave Generate procedural bands or rings with noise Voronoi Generate Worley noise based on the distance to random points Typically used to generate textures such as or biological cells Brick Generate a procedural texture producing bricks Texture Retrieve multiple types of texture coordinates nTypically used as inputs for texture nodes Vector Convert a point
ATTR_WARN_UNUSED_RESULT const BMLoop * l
#define A
static btScalar dDOT14(const btScalar *a, const btScalar *b)
#define dMULTIPLY0_331(A, B, C)
void dLineClosestApproach(const btVector3 &pa, const btVector3 &ua, const btVector3 &pb, const btVector3 &ub, btScalar *alpha, btScalar *beta)
int dBoxBox2(const btVector3 &p1, const dMatrix3 R1, const btVector3 &side1, const btVector3 &p2, const dMatrix3 R2, const btVector3 &side2, btVector3 &normal, btScalar *depth, int *return_code, int maxc, dContactGeom *, int, btDiscreteCollisionDetectorInterface::Result &output)
btScalar dMatrix3[4 *3]
static btScalar dDOT(const btScalar *a, const btScalar *b)
#define M__PI
#define dMULTIPLY1_331(A, B, C)
static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
#define dDOTpq(a, b, p, q)
void cullPoints2(int n, btScalar p[], int m, int i0, int iret[])
#define dInfinity
#define TST(expr1, expr2, norm, cc)
static btScalar dDOT41(const btScalar *a, const btScalar *b)
static btScalar dDOT44(const btScalar *a, const btScalar *b)
btBoxShape(const btVector3 &boxHalfExtents)
Definition: btBoxShape.cpp:17
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
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
#define SIMD_EPSILON
Definition: btScalar.h:543
#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
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
GPUBatch * quad
IconTextureDrawCall normal
ccl_global float * buffer
ccl_global KernelShaderEvalInput ccl_global float * output
ccl_global KernelShaderEvalInput * input
#define B
static unsigned a[3]
Definition: RandGen.cpp:78
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)
double sign(double arg)
Definition: utility.h:250
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
return ret
const btBoxShape * m_box1
const btBoxShape * m_box2
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
btBoxBoxDetector(const btBoxShape *box1, const btBoxShape *box2)
ODE box-box collision detection is adapted to work with Bullet.
ccl_device_inline float beta(float x, float y)
Definition: util/math.h:775