Bullet Collision Detection & Physics Library
boxBoxDistance.cpp
Go to the documentation of this file.
1 /*
2  Copyright (C) 2006, 2008 Sony Computer Entertainment Inc.
3  All rights reserved.
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 */
16 
17 
18 //#include "PfxContactBoxBox.h"
19 
20 #include <math.h>
21 #include "../PlatformDefinitions.h"
22 #include "boxBoxDistance.h"
23 
24 static inline float sqr( float a )
25 {
26  return (a * a);
27 }
28 
29 enum BoxSepAxisType
30 {
31  A_AXIS, B_AXIS, CROSS_AXIS
32 };
33 
34 //-------------------------------------------------------------------------------------------------
35 // voronoiTol: bevels Voronoi planes slightly which helps when features are parallel.
36 //-------------------------------------------------------------------------------------------------
37 
38 static const float voronoiTol = -1.0e-5f;
39 
40 //-------------------------------------------------------------------------------------------------
41 // separating axis tests: gaps along each axis are computed, and the axis with the maximum
42 // gap is stored. cross product axes are normalized.
43 //-------------------------------------------------------------------------------------------------
44 
45 #define AaxisTest( dim, letter, first ) \
46 { \
47  if ( first ) \
48  { \
49  maxGap = gap = gapsA.get##letter(); \
50  if ( gap > distanceThreshold ) return gap; \
51  axisType = A_AXIS; \
52  faceDimA = dim; \
53  axisA = identity.getCol##dim(); \
54  } \
55  else \
56  { \
57  gap = gapsA.get##letter(); \
58  if ( gap > distanceThreshold ) return gap; \
59  else if ( gap > maxGap ) \
60  { \
61  maxGap = gap; \
62  axisType = A_AXIS; \
63  faceDimA = dim; \
64  axisA = identity.getCol##dim(); \
65  } \
66  } \
67 }
68 
69 
70 #define BaxisTest( dim, letter ) \
71 { \
72  gap = gapsB.get##letter(); \
73  if ( gap > distanceThreshold ) return gap; \
74  else if ( gap > maxGap ) \
75  { \
76  maxGap = gap; \
77  axisType = B_AXIS; \
78  faceDimB = dim; \
79  axisB = identity.getCol##dim(); \
80  } \
81 }
82 
83 #define CrossAxisTest( dima, dimb, letterb ) \
84 { \
85  const float lsqr_tolerance = 1.0e-30f; \
86  float lsqr; \
87  \
88  lsqr = lsqrs.getCol##dima().get##letterb(); \
89  \
90  if ( lsqr > lsqr_tolerance ) \
91  { \
92  float l_recip = 1.0f / sqrtf( lsqr ); \
93  gap = float(gapsAxB.getCol##dima().get##letterb()) * l_recip; \
94  \
95  if ( gap > distanceThreshold ) \
96  { \
97  return gap; \
98  } \
99  \
100  if ( gap > maxGap ) \
101  { \
102  maxGap = gap; \
103  axisType = CROSS_AXIS; \
104  edgeDimA = dima; \
105  edgeDimB = dimb; \
106  axisA = cross(identity.getCol##dima(),matrixAB.getCol##dimb()) * l_recip; \
107  } \
108  } \
109 }
110 
111 //-------------------------------------------------------------------------------------------------
112 // tests whether a vertex of box B and a face of box A are the closest features
113 //-------------------------------------------------------------------------------------------------
114 
115 inline
116 float
117 VertexBFaceATest(
118  bool & inVoronoi,
119  float & t0,
120  float & t1,
121  const vmVector3 & hA,
122  PE_REF(vmVector3) faceOffsetAB,
123  PE_REF(vmVector3) faceOffsetBA,
124  const vmMatrix3 & matrixAB,
125  const vmMatrix3 & matrixBA,
126  PE_REF(vmVector3) signsB,
127  PE_REF(vmVector3) scalesB )
128 {
129  // compute a corner of box B in A's coordinate system
130 
131  vmVector3 corner =
132  vmVector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() );
133 
134  // compute the parameters of the point on A, closest to this corner
135 
136  t0 = corner[0];
137  t1 = corner[1];
138 
139  if ( t0 > hA[0] )
140  t0 = hA[0];
141  else if ( t0 < -hA[0] )
142  t0 = -hA[0];
143  if ( t1 > hA[1] )
144  t1 = hA[1];
145  else if ( t1 < -hA[1] )
146  t1 = -hA[1];
147 
148  // do the Voronoi test: already know the point on B is in the Voronoi region of the
149  // point on A, check the reverse.
150 
151  vmVector3 facePointB =
152  vmVector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) );
153 
154  inVoronoi = ( ( facePointB[0] >= voronoiTol * facePointB[2] ) &&
155  ( facePointB[1] >= voronoiTol * facePointB[0] ) &&
156  ( facePointB[2] >= voronoiTol * facePointB[1] ) );
157 
158  return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] ));
159 }
160 
161 #define VertexBFaceA_SetNewMin() \
162 { \
163  minDistSqr = distSqr; \
164  localPointA.setX(t0); \
165  localPointA.setY(t1); \
166  localPointB.setX( scalesB.getX() ); \
167  localPointB.setY( scalesB.getY() ); \
168  featureA = F; \
169  featureB = V; \
170 }
171 
172 void
173 VertexBFaceATests(
174  bool & done,
175  float & minDistSqr,
176  vmPoint3 & localPointA,
177  vmPoint3 & localPointB,
178  FeatureType & featureA,
179  FeatureType & featureB,
180  const vmVector3 & hA,
181  PE_REF(vmVector3) faceOffsetAB,
182  PE_REF(vmVector3) faceOffsetBA,
183  const vmMatrix3 & matrixAB,
184  const vmMatrix3 & matrixBA,
185  PE_REF(vmVector3) signsB,
186  PE_REF(vmVector3) scalesB,
187  bool first )
188 {
189 
190  float t0, t1;
191  float distSqr;
192 
193  distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
194  matrixAB, matrixBA, signsB, scalesB );
195 
196  if ( first ) {
197  VertexBFaceA_SetNewMin();
198  } else {
199  if ( distSqr < minDistSqr ) {
200  VertexBFaceA_SetNewMin();
201  }
202  }
203 
204  if ( done )
205  return;
206 
207  signsB.setX( -signsB.getX() );
208  scalesB.setX( -scalesB.getX() );
209 
210  distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
211  matrixAB, matrixBA, signsB, scalesB );
212 
213  if ( distSqr < minDistSqr ) {
214  VertexBFaceA_SetNewMin();
215  }
216 
217  if ( done )
218  return;
219 
220  signsB.setY( -signsB.getY() );
221  scalesB.setY( -scalesB.getY() );
222 
223  distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
224  matrixAB, matrixBA, signsB, scalesB );
225 
226  if ( distSqr < minDistSqr ) {
227  VertexBFaceA_SetNewMin();
228  }
229 
230  if ( done )
231  return;
232 
233  signsB.setX( -signsB.getX() );
234  scalesB.setX( -scalesB.getX() );
235 
236  distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,
237  matrixAB, matrixBA, signsB, scalesB );
238 
239  if ( distSqr < minDistSqr ) {
240  VertexBFaceA_SetNewMin();
241  }
242 }
243 
244 //-------------------------------------------------------------------------------------------------
245 // VertexAFaceBTest: tests whether a vertex of box A and a face of box B are the closest features
246 //-------------------------------------------------------------------------------------------------
247 
248 inline
249 float
250 VertexAFaceBTest(
251  bool & inVoronoi,
252  float & t0,
253  float & t1,
254  const vmVector3 & hB,
255  PE_REF(vmVector3) faceOffsetAB,
256  PE_REF(vmVector3) faceOffsetBA,
257  const vmMatrix3 & matrixAB,
258  const vmMatrix3 & matrixBA,
259  PE_REF(vmVector3) signsA,
260  PE_REF(vmVector3) scalesA )
261 {
262  vmVector3 corner =
263  vmVector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() );
264 
265  t0 = corner[0];
266  t1 = corner[1];
267 
268  if ( t0 > hB[0] )
269  t0 = hB[0];
270  else if ( t0 < -hB[0] )
271  t0 = -hB[0];
272  if ( t1 > hB[1] )
273  t1 = hB[1];
274  else if ( t1 < -hB[1] )
275  t1 = -hB[1];
276 
277  vmVector3 facePointA =
278  vmVector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) );
279 
280  inVoronoi = ( ( facePointA[0] >= voronoiTol * facePointA[2] ) &&
281  ( facePointA[1] >= voronoiTol * facePointA[0] ) &&
282  ( facePointA[2] >= voronoiTol * facePointA[1] ) );
283 
284  return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] ));
285 }
286 
287 #define VertexAFaceB_SetNewMin() \
288 { \
289  minDistSqr = distSqr; \
290  localPointB.setX(t0); \
291  localPointB.setY(t1); \
292  localPointA.setX( scalesA.getX() ); \
293  localPointA.setY( scalesA.getY() ); \
294  featureA = V; \
295  featureB = F; \
296 }
297 
298 void
299 VertexAFaceBTests(
300  bool & done,
301  float & minDistSqr,
302  vmPoint3 & localPointA,
303  vmPoint3 & localPointB,
304  FeatureType & featureA,
305  FeatureType & featureB,
306  const vmVector3 & hB,
307  PE_REF(vmVector3) faceOffsetAB,
308  PE_REF(vmVector3) faceOffsetBA,
309  const vmMatrix3 & matrixAB,
310  const vmMatrix3 & matrixBA,
311  PE_REF(vmVector3) signsA,
312  PE_REF(vmVector3) scalesA,
313  bool first )
314 {
315  float t0, t1;
316  float distSqr;
317 
318  distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
319  matrixAB, matrixBA, signsA, scalesA );
320 
321  if ( first ) {
322  VertexAFaceB_SetNewMin();
323  } else {
324  if ( distSqr < minDistSqr ) {
325  VertexAFaceB_SetNewMin();
326  }
327  }
328 
329  if ( done )
330  return;
331 
332  signsA.setX( -signsA.getX() );
333  scalesA.setX( -scalesA.getX() );
334 
335  distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
336  matrixAB, matrixBA, signsA, scalesA );
337 
338  if ( distSqr < minDistSqr ) {
339  VertexAFaceB_SetNewMin();
340  }
341 
342  if ( done )
343  return;
344 
345  signsA.setY( -signsA.getY() );
346  scalesA.setY( -scalesA.getY() );
347 
348  distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
349  matrixAB, matrixBA, signsA, scalesA );
350 
351  if ( distSqr < minDistSqr ) {
352  VertexAFaceB_SetNewMin();
353  }
354 
355  if ( done )
356  return;
357 
358  signsA.setX( -signsA.getX() );
359  scalesA.setX( -scalesA.getX() );
360 
361  distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,
362  matrixAB, matrixBA, signsA, scalesA );
363 
364  if ( distSqr < minDistSqr ) {
365  VertexAFaceB_SetNewMin();
366  }
367 }
368 
369 //-------------------------------------------------------------------------------------------------
370 // CustomEdgeEdgeTest:
371 //
372 // tests whether a pair of edges are the closest features
373 //
374 // note on the shorthand:
375 // 'a' & 'b' refer to the edges.
376 // 'c' is the dimension of the axis that points from the face center to the edge Center
377 // 'd' is the dimension of the edge Direction
378 // the dimension of the face normal is 2
379 //-------------------------------------------------------------------------------------------------
380 
381 #define CustomEdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter ) \
382 { \
383  vmVector3 edgeOffsetAB; \
384  vmVector3 edgeOffsetBA; \
385  \
386  edgeOffsetAB = faceOffsetAB + matrixAB.getCol##bc() * scalesB.get##bc_letter(); \
387  edgeOffsetAB.set##ac_letter( edgeOffsetAB.get##ac_letter() - scalesA.get##ac_letter() ); \
388  \
389  edgeOffsetBA = faceOffsetBA + matrixBA.getCol##ac() * scalesA.get##ac_letter(); \
390  edgeOffsetBA.set##bc_letter( edgeOffsetBA.get##bc_letter() - scalesB.get##bc_letter() ); \
391  \
392  float dirDot = matrixAB.getCol##bd().get##ad_letter(); \
393  float denom = 1.0f - dirDot*dirDot; \
394  float edgeOffsetAB_ad = edgeOffsetAB.get##ad_letter(); \
395  float edgeOffsetBA_bd = edgeOffsetBA.get##bd_letter(); \
396  \
397  if ( denom == 0.0f ) \
398  { \
399  tA = 0.0f; \
400  } \
401  else \
402  { \
403  tA = ( edgeOffsetAB_ad + edgeOffsetBA_bd * dirDot ) / denom; \
404  } \
405  \
406  if ( tA < -hA[ad] ) tA = -hA[ad]; \
407  else if ( tA > hA[ad] ) tA = hA[ad]; \
408  \
409  tB = tA * dirDot + edgeOffsetBA_bd; \
410  \
411  if ( tB < -hB[bd] ) \
412  { \
413  tB = -hB[bd]; \
414  tA = tB * dirDot + edgeOffsetAB_ad; \
415  \
416  if ( tA < -hA[ad] ) tA = -hA[ad]; \
417  else if ( tA > hA[ad] ) tA = hA[ad]; \
418  } \
419  else if ( tB > hB[bd] ) \
420  { \
421  tB = hB[bd]; \
422  tA = tB * dirDot + edgeOffsetAB_ad; \
423  \
424  if ( tA < -hA[ad] ) tA = -hA[ad]; \
425  else if ( tA > hA[ad] ) tA = hA[ad]; \
426  } \
427  \
428  vmVector3 edgeOffAB = vmVector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\
429  vmVector3 edgeOffBA = vmVector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\
430  \
431  inVoronoi = ( edgeOffAB[ac] >= voronoiTol * edgeOffAB[2] ) && \
432  ( edgeOffAB[2] >= voronoiTol * edgeOffAB[ac] ) && \
433  ( edgeOffBA[bc] >= voronoiTol * edgeOffBA[2] ) && \
434  ( edgeOffBA[2] >= voronoiTol * edgeOffBA[bc] ); \
435  \
436  edgeOffAB[ad] -= tA; \
437  edgeOffBA[bd] -= tB; \
438  \
439  return dot(edgeOffAB,edgeOffAB); \
440 }
441 
442 float
443 CustomEdgeEdgeTest_0101(
444  bool & inVoronoi,
445  float & tA,
446  float & tB,
447  const vmVector3 & hA,
448  const vmVector3 & hB,
449  PE_REF(vmVector3) faceOffsetAB,
450  PE_REF(vmVector3) faceOffsetBA,
451  const vmMatrix3 & matrixAB,
452  const vmMatrix3 & matrixBA,
453  PE_REF(vmVector3) signsA,
454  PE_REF(vmVector3) signsB,
455  PE_REF(vmVector3) scalesA,
456  PE_REF(vmVector3) scalesB )
457 {
458  CustomEdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y );
459 }
460 
461 float
462 CustomEdgeEdgeTest_0110(
463  bool & inVoronoi,
464  float & tA,
465  float & tB,
466  const vmVector3 & hA,
467  const vmVector3 & hB,
468  PE_REF(vmVector3) faceOffsetAB,
469  PE_REF(vmVector3) faceOffsetBA,
470  const vmMatrix3 & matrixAB,
471  const vmMatrix3 & matrixBA,
472  PE_REF(vmVector3) signsA,
473  PE_REF(vmVector3) signsB,
474  PE_REF(vmVector3) scalesA,
475  PE_REF(vmVector3) scalesB )
476 {
477  CustomEdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X );
478 }
479 
480 float
481 CustomEdgeEdgeTest_1001(
482  bool & inVoronoi,
483  float & tA,
484  float & tB,
485  const vmVector3 & hA,
486  const vmVector3 & hB,
487  PE_REF(vmVector3) faceOffsetAB,
488  PE_REF(vmVector3) faceOffsetBA,
489  const vmMatrix3 & matrixAB,
490  const vmMatrix3 & matrixBA,
491  PE_REF(vmVector3) signsA,
492  PE_REF(vmVector3) signsB,
493  PE_REF(vmVector3) scalesA,
494  PE_REF(vmVector3) scalesB )
495 {
496  CustomEdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y );
497 }
498 
499 float
500 CustomEdgeEdgeTest_1010(
501  bool & inVoronoi,
502  float & tA,
503  float & tB,
504  const vmVector3 & hA,
505  const vmVector3 & hB,
506  PE_REF(vmVector3) faceOffsetAB,
507  PE_REF(vmVector3) faceOffsetBA,
508  const vmMatrix3 & matrixAB,
509  const vmMatrix3 & matrixBA,
510  PE_REF(vmVector3) signsA,
511  PE_REF(vmVector3) signsB,
512  PE_REF(vmVector3) scalesA,
513  PE_REF(vmVector3) scalesB )
514 {
515  CustomEdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X );
516 }
517 
518 #define EdgeEdge_SetNewMin( ac_letter, ad_letter, bc_letter, bd_letter ) \
519 { \
520  minDistSqr = distSqr; \
521  localPointA.set##ac_letter(scalesA.get##ac_letter()); \
522  localPointA.set##ad_letter(tA); \
523  localPointB.set##bc_letter(scalesB.get##bc_letter()); \
524  localPointB.set##bd_letter(tB); \
525  otherFaceDimA = testOtherFaceDimA; \
526  otherFaceDimB = testOtherFaceDimB; \
527  featureA = E; \
528  featureB = E; \
529 }
530 
531 void
532 EdgeEdgeTests(
533  bool & done,
534  float & minDistSqr,
535  vmPoint3 & localPointA,
536  vmPoint3 & localPointB,
537  int & otherFaceDimA,
538  int & otherFaceDimB,
539  FeatureType & featureA,
540  FeatureType & featureB,
541  const vmVector3 & hA,
542  const vmVector3 & hB,
543  PE_REF(vmVector3) faceOffsetAB,
544  PE_REF(vmVector3) faceOffsetBA,
545  const vmMatrix3 & matrixAB,
546  const vmMatrix3 & matrixBA,
547  PE_REF(vmVector3) signsA,
548  PE_REF(vmVector3) signsB,
549  PE_REF(vmVector3) scalesA,
550  PE_REF(vmVector3) scalesB,
551  bool first )
552 {
553 
554  float distSqr;
555  float tA, tB;
556 
557  int testOtherFaceDimA, testOtherFaceDimB;
558 
559  testOtherFaceDimA = 0;
560  testOtherFaceDimB = 0;
561 
562  distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
563  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
564 
565  if ( first ) {
566  EdgeEdge_SetNewMin( X, Y, X, Y );
567  } else {
568  if ( distSqr < minDistSqr ) {
569  EdgeEdge_SetNewMin( X, Y, X, Y );
570  }
571  }
572 
573  if ( done )
574  return;
575 
576  signsA.setX( -signsA.getX() );
577  scalesA.setX( -scalesA.getX() );
578 
579  distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
580  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
581 
582  if ( distSqr < minDistSqr ) {
583  EdgeEdge_SetNewMin( X, Y, X, Y );
584  }
585 
586  if ( done )
587  return;
588 
589  signsB.setX( -signsB.getX() );
590  scalesB.setX( -scalesB.getX() );
591 
592  distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
593  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
594 
595  if ( distSqr < minDistSqr ) {
596  EdgeEdge_SetNewMin( X, Y, X, Y );
597  }
598 
599  if ( done )
600  return;
601 
602  signsA.setX( -signsA.getX() );
603  scalesA.setX( -scalesA.getX() );
604 
605  distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
606  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
607 
608  if ( distSqr < minDistSqr ) {
609  EdgeEdge_SetNewMin( X, Y, X, Y );
610  }
611 
612  if ( done )
613  return;
614 
615  testOtherFaceDimA = 1;
616  testOtherFaceDimB = 0;
617  signsB.setX( -signsB.getX() );
618  scalesB.setX( -scalesB.getX() );
619 
620  distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
621  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
622 
623  if ( distSqr < minDistSqr ) {
624  EdgeEdge_SetNewMin( Y, X, X, Y );
625  }
626 
627  if ( done )
628  return;
629 
630  signsA.setY( -signsA.getY() );
631  scalesA.setY( -scalesA.getY() );
632 
633  distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
634  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
635 
636  if ( distSqr < minDistSqr ) {
637  EdgeEdge_SetNewMin( Y, X, X, Y );
638  }
639 
640  if ( done )
641  return;
642 
643  signsB.setX( -signsB.getX() );
644  scalesB.setX( -scalesB.getX() );
645 
646  distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
647  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
648 
649  if ( distSqr < minDistSqr ) {
650  EdgeEdge_SetNewMin( Y, X, X, Y );
651  }
652 
653  if ( done )
654  return;
655 
656  signsA.setY( -signsA.getY() );
657  scalesA.setY( -scalesA.getY() );
658 
659  distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
660  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
661 
662  if ( distSqr < minDistSqr ) {
663  EdgeEdge_SetNewMin( Y, X, X, Y );
664  }
665 
666  if ( done )
667  return;
668 
669  testOtherFaceDimA = 0;
670  testOtherFaceDimB = 1;
671  signsB.setX( -signsB.getX() );
672  scalesB.setX( -scalesB.getX() );
673 
674  distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
675  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
676 
677  if ( distSqr < minDistSqr ) {
678  EdgeEdge_SetNewMin( X, Y, Y, X );
679  }
680 
681  if ( done )
682  return;
683 
684  signsA.setX( -signsA.getX() );
685  scalesA.setX( -scalesA.getX() );
686 
687  distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
688  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
689 
690  if ( distSqr < minDistSqr ) {
691  EdgeEdge_SetNewMin( X, Y, Y, X );
692  }
693 
694  if ( done )
695  return;
696 
697  signsB.setY( -signsB.getY() );
698  scalesB.setY( -scalesB.getY() );
699 
700  distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
701  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
702 
703  if ( distSqr < minDistSqr ) {
704  EdgeEdge_SetNewMin( X, Y, Y, X );
705  }
706 
707  if ( done )
708  return;
709 
710  signsA.setX( -signsA.getX() );
711  scalesA.setX( -scalesA.getX() );
712 
713  distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
714  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
715 
716  if ( distSqr < minDistSqr ) {
717  EdgeEdge_SetNewMin( X, Y, Y, X );
718  }
719 
720  if ( done )
721  return;
722 
723  testOtherFaceDimA = 1;
724  testOtherFaceDimB = 1;
725  signsB.setY( -signsB.getY() );
726  scalesB.setY( -scalesB.getY() );
727 
728  distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
729  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
730 
731  if ( distSqr < minDistSqr ) {
732  EdgeEdge_SetNewMin( Y, X, Y, X );
733  }
734 
735  if ( done )
736  return;
737 
738  signsA.setY( -signsA.getY() );
739  scalesA.setY( -scalesA.getY() );
740 
741  distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
742  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
743 
744  if ( distSqr < minDistSqr ) {
745  EdgeEdge_SetNewMin( Y, X, Y, X );
746  }
747 
748  if ( done )
749  return;
750 
751  signsB.setY( -signsB.getY() );
752  scalesB.setY( -scalesB.getY() );
753 
754  distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
755  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
756 
757  if ( distSqr < minDistSqr ) {
758  EdgeEdge_SetNewMin( Y, X, Y, X );
759  }
760 
761  if ( done )
762  return;
763 
764  signsA.setY( -signsA.getY() );
765  scalesA.setY( -scalesA.getY() );
766 
767  distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
768  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
769 
770  if ( distSqr < minDistSqr ) {
771  EdgeEdge_SetNewMin( Y, X, Y, X );
772  }
773 }
774 
775 
776 float
777 boxBoxDistance(vmVector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,
778  PE_REF(Box) boxA, const vmTransform3 & transformA, PE_REF(Box) boxB,
779  const vmTransform3 & transformB,
780  float distanceThreshold)
781 {
782  vmMatrix3 identity;
783  identity = vmMatrix3::identity();
784  vmVector3 ident[3];
785  ident[0] = identity.getCol0();
786  ident[1] = identity.getCol1();
787  ident[2] = identity.getCol2();
788 
789  // get relative transformations
790 
791  vmTransform3 transformAB, transformBA;
792  vmMatrix3 matrixAB, matrixBA;
793  vmVector3 offsetAB, offsetBA;
794 
795  transformAB = orthoInverse(transformA) * transformB;
796  transformBA = orthoInverse(transformAB);
797 
798  matrixAB = transformAB.getUpper3x3();
799  offsetAB = transformAB.getTranslation();
800  matrixBA = transformBA.getUpper3x3();
801  offsetBA = transformBA.getTranslation();
802 
803  vmMatrix3 absMatrixAB = absPerElem(matrixAB);
804  vmMatrix3 absMatrixBA = absPerElem(matrixBA);
805 
806  // find separating axis with largest gap between projections
807 
808  BoxSepAxisType axisType;
809  vmVector3 axisA(0.0f), axisB(0.0f);
810  float gap, maxGap;
811  int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0;
812 
813  // face axes
814 
815  vmVector3 gapsA = absPerElem(offsetAB) - boxA.mHalf - absMatrixAB * boxB.mHalf;
816 
817  AaxisTest(0,X,true);
818  AaxisTest(1,Y,false);
819  AaxisTest(2,Z,false);
820 
821  vmVector3 gapsB = absPerElem(offsetBA) - boxB.mHalf - absMatrixBA * boxA.mHalf;
822 
823  BaxisTest(0,X);
824  BaxisTest(1,Y);
825  BaxisTest(2,Z);
826 
827  // cross product axes
828 
829  // ŠOÏ‚ª‚O‚Ì‚Æ‚«‚Ì‘Îô
830  absMatrixAB += vmMatrix3(1.0e-5f);
831  absMatrixBA += vmMatrix3(1.0e-5f);
832 
833  vmMatrix3 lsqrs, projOffset, projAhalf, projBhalf;
834 
835  lsqrs.setCol0( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +
836  mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) );
837  lsqrs.setCol1( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +
838  mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) );
839  lsqrs.setCol2( mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) +
840  mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) );
841 
842  projOffset.setCol0(matrixBA.getCol1() * offsetAB.getZ() - matrixBA.getCol2() * offsetAB.getY());
843  projOffset.setCol1(matrixBA.getCol2() * offsetAB.getX() - matrixBA.getCol0() * offsetAB.getZ());
844  projOffset.setCol2(matrixBA.getCol0() * offsetAB.getY() - matrixBA.getCol1() * offsetAB.getX());
845 
846  projAhalf.setCol0(absMatrixBA.getCol1() * boxA.mHalf.getZ() + absMatrixBA.getCol2() * boxA.mHalf.getY());
847  projAhalf.setCol1(absMatrixBA.getCol2() * boxA.mHalf.getX() + absMatrixBA.getCol0() * boxA.mHalf.getZ());
848  projAhalf.setCol2(absMatrixBA.getCol0() * boxA.mHalf.getY() + absMatrixBA.getCol1() * boxA.mHalf.getX());
849 
850  projBhalf.setCol0(absMatrixAB.getCol1() * boxB.mHalf.getZ() + absMatrixAB.getCol2() * boxB.mHalf.getY());
851  projBhalf.setCol1(absMatrixAB.getCol2() * boxB.mHalf.getX() + absMatrixAB.getCol0() * boxB.mHalf.getZ());
852  projBhalf.setCol2(absMatrixAB.getCol0() * boxB.mHalf.getY() + absMatrixAB.getCol1() * boxB.mHalf.getX());
853 
854  vmMatrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf);
855 
856  CrossAxisTest(0,0,X);
857  CrossAxisTest(0,1,Y);
858  CrossAxisTest(0,2,Z);
859  CrossAxisTest(1,0,X);
860  CrossAxisTest(1,1,Y);
861  CrossAxisTest(1,2,Z);
862  CrossAxisTest(2,0,X);
863  CrossAxisTest(2,1,Y);
864  CrossAxisTest(2,2,Z);
865 
866  // need to pick the face on each box whose normal best matches the separating axis.
867  // will transform vectors to be in the coordinate system of this face to simplify things later.
868  // for this, a permutation matrix can be used, which the next section computes.
869 
870  int dimA[3], dimB[3];
871 
872  if ( axisType == A_AXIS ) {
873  if ( dot(axisA,offsetAB) < 0.0f )
874  axisA = -axisA;
875  axisB = matrixBA * -axisA;
876 
877  vmVector3 absAxisB = vmVector3(absPerElem(axisB));
878 
879  if ( ( absAxisB[0] > absAxisB[1] ) && ( absAxisB[0] > absAxisB[2] ) )
880  faceDimB = 0;
881  else if ( absAxisB[1] > absAxisB[2] )
882  faceDimB = 1;
883  else
884  faceDimB = 2;
885  } else if ( axisType == B_AXIS ) {
886  if ( dot(axisB,offsetBA) < 0.0f )
887  axisB = -axisB;
888  axisA = matrixAB * -axisB;
889 
890  vmVector3 absAxisA = vmVector3(absPerElem(axisA));
891 
892  if ( ( absAxisA[0] > absAxisA[1] ) && ( absAxisA[0] > absAxisA[2] ) )
893  faceDimA = 0;
894  else if ( absAxisA[1] > absAxisA[2] )
895  faceDimA = 1;
896  else
897  faceDimA = 2;
898  }
899 
900  if ( axisType == CROSS_AXIS ) {
901  if ( dot(axisA,offsetAB) < 0.0f )
902  axisA = -axisA;
903  axisB = matrixBA * -axisA;
904 
905  vmVector3 absAxisA = vmVector3(absPerElem(axisA));
906  vmVector3 absAxisB = vmVector3(absPerElem(axisB));
907 
908  dimA[1] = edgeDimA;
909  dimB[1] = edgeDimB;
910 
911  if ( edgeDimA == 0 ) {
912  if ( absAxisA[1] > absAxisA[2] ) {
913  dimA[0] = 2;
914  dimA[2] = 1;
915  } else {
916  dimA[0] = 1;
917  dimA[2] = 2;
918  }
919  } else if ( edgeDimA == 1 ) {
920  if ( absAxisA[2] > absAxisA[0] ) {
921  dimA[0] = 0;
922  dimA[2] = 2;
923  } else {
924  dimA[0] = 2;
925  dimA[2] = 0;
926  }
927  } else {
928  if ( absAxisA[0] > absAxisA[1] ) {
929  dimA[0] = 1;
930  dimA[2] = 0;
931  } else {
932  dimA[0] = 0;
933  dimA[2] = 1;
934  }
935  }
936 
937  if ( edgeDimB == 0 ) {
938  if ( absAxisB[1] > absAxisB[2] ) {
939  dimB[0] = 2;
940  dimB[2] = 1;
941  } else {
942  dimB[0] = 1;
943  dimB[2] = 2;
944  }
945  } else if ( edgeDimB == 1 ) {
946  if ( absAxisB[2] > absAxisB[0] ) {
947  dimB[0] = 0;
948  dimB[2] = 2;
949  } else {
950  dimB[0] = 2;
951  dimB[2] = 0;
952  }
953  } else {
954  if ( absAxisB[0] > absAxisB[1] ) {
955  dimB[0] = 1;
956  dimB[2] = 0;
957  } else {
958  dimB[0] = 0;
959  dimB[2] = 1;
960  }
961  }
962  } else {
963  dimA[2] = faceDimA;
964  dimA[0] = (faceDimA+1)%3;
965  dimA[1] = (faceDimA+2)%3;
966  dimB[2] = faceDimB;
967  dimB[0] = (faceDimB+1)%3;
968  dimB[1] = (faceDimB+2)%3;
969  }
970 
971  vmMatrix3 aperm_col, bperm_col;
972 
973  aperm_col.setCol0(ident[dimA[0]]);
974  aperm_col.setCol1(ident[dimA[1]]);
975  aperm_col.setCol2(ident[dimA[2]]);
976 
977  bperm_col.setCol0(ident[dimB[0]]);
978  bperm_col.setCol1(ident[dimB[1]]);
979  bperm_col.setCol2(ident[dimB[2]]);
980 
981  vmMatrix3 aperm_row, bperm_row;
982 
983  aperm_row = transpose(aperm_col);
984  bperm_row = transpose(bperm_col);
985 
986  // permute all box parameters to be in the face coordinate systems
987 
988  vmMatrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col;
989  vmMatrix3 matrixBA_perm = transpose(matrixAB_perm);
990 
991  vmVector3 offsetAB_perm, offsetBA_perm;
992 
993  offsetAB_perm = aperm_row * offsetAB;
994  offsetBA_perm = bperm_row * offsetBA;
995 
996  vmVector3 halfA_perm, halfB_perm;
997 
998  halfA_perm = aperm_row * boxA.mHalf;
999  halfB_perm = bperm_row * boxB.mHalf;
1000 
1001  // compute the vector between the centers of each face, in each face's coordinate frame
1002 
1003  vmVector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm;
1004 
1005  signsA_perm = copySignPerElem(vmVector3(1.0f),aperm_row * axisA);
1006  signsB_perm = copySignPerElem(vmVector3(1.0f),bperm_row * axisB);
1007  scalesA_perm = mulPerElem( signsA_perm, halfA_perm );
1008  scalesB_perm = mulPerElem( signsB_perm, halfB_perm );
1009 
1010  faceOffsetAB_perm = offsetAB_perm + matrixAB_perm.getCol2() * scalesB_perm.getZ();
1011  faceOffsetAB_perm.setZ( faceOffsetAB_perm.getZ() - scalesA_perm.getZ() );
1012 
1013  faceOffsetBA_perm = offsetBA_perm + matrixBA_perm.getCol2() * scalesA_perm.getZ();
1014  faceOffsetBA_perm.setZ( faceOffsetBA_perm.getZ() - scalesB_perm.getZ() );
1015 
1016  if ( maxGap < 0.0f ) {
1017  // if boxes overlap, this will separate the faces for finding points of penetration.
1018 
1019  faceOffsetAB_perm -= aperm_row * axisA * maxGap * 1.01f;
1020  faceOffsetBA_perm -= bperm_row * axisB * maxGap * 1.01f;
1021  }
1022 
1023  // for each vertex/face or edge/edge pair of the two faces, find the closest points.
1024  //
1025  // these points each have an associated box feature (vertex, edge, or face). if each
1026  // point is in the external Voronoi region of the other's feature, they are the
1027  // closest points of the boxes, and the algorithm can exit.
1028  //
1029  // the feature pairs are arranged so that in the general case, the first test will
1030  // succeed. degenerate cases (parallel faces) may require up to all tests in the
1031  // worst case.
1032  //
1033  // if for some reason no case passes the Voronoi test, the features with the minimum
1034  // distance are returned.
1035 
1036  vmPoint3 localPointA_perm, localPointB_perm;
1037  float minDistSqr;
1038  bool done;
1039 
1040  vmVector3 hA_perm( halfA_perm ), hB_perm( halfB_perm );
1041 
1042  localPointA_perm.setZ( scalesA_perm.getZ() );
1043  localPointB_perm.setZ( scalesB_perm.getZ() );
1044  scalesA_perm.setZ(0.0f);
1045  scalesB_perm.setZ(0.0f);
1046 
1047  int otherFaceDimA, otherFaceDimB;
1048  FeatureType featureA, featureB;
1049 
1050  if ( axisType == CROSS_AXIS ) {
1051  EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,
1052  otherFaceDimA, otherFaceDimB, featureA, featureB,
1053  hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
1054  matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,
1055  scalesA_perm, scalesB_perm, true );
1056 
1057  if ( !done ) {
1058  VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,
1059  featureA, featureB,
1060  hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,
1061  matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false );
1062 
1063  if ( !done ) {
1064  VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,
1065  featureA, featureB,
1066  hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
1067  matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false );
1068  }
1069  }
1070  } else if ( axisType == B_AXIS ) {
1071  VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,
1072  featureA, featureB,
1073  hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
1074  matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, true );
1075 
1076  if ( !done ) {
1077  VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,
1078  featureA, featureB,
1079  hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,
1080  matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false );
1081 
1082  if ( !done ) {
1083  EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,
1084  otherFaceDimA, otherFaceDimB, featureA, featureB,
1085  hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
1086  matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,
1087  scalesA_perm, scalesB_perm, false );
1088  }
1089  }
1090  } else {
1091  VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,
1092  featureA, featureB,
1093  hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,
1094  matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, true );
1095 
1096  if ( !done ) {
1097  VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,
1098  featureA, featureB,
1099  hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
1100  matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false );
1101 
1102  if ( !done ) {
1103  EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,
1104  otherFaceDimA, otherFaceDimB, featureA, featureB,
1105  hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,
1106  matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,
1107  scalesA_perm, scalesB_perm, false );
1108  }
1109  }
1110  }
1111 
1112  // convert local points from face-local to box-local coordinate system
1113 
1114 
1115  boxPointA.localPoint = vmPoint3( aperm_col * vmVector3( localPointA_perm )) ;
1116  boxPointB.localPoint = vmPoint3( bperm_col * vmVector3( localPointB_perm )) ;
1117 
1118 #if 0
1119  // find which features of the boxes are involved.
1120  // the only feature pairs which occur in this function are VF, FV, and EE, even though the
1121  // closest points might actually lie on sub-features, as in a VF contact might be used for
1122  // what's actually a VV contact. this means some feature pairs could possibly seem distinct
1123  // from others, although their contact positions are the same. don't know yet whether this
1124  // matters.
1125 
1126  int sA[3], sB[3];
1127 
1128  sA[0] = boxPointA.localPoint.getX() > 0.0f;
1129  sA[1] = boxPointA.localPoint.getY() > 0.0f;
1130  sA[2] = boxPointA.localPoint.getZ() > 0.0f;
1131 
1132  sB[0] = boxPointB.localPoint.getX() > 0.0f;
1133  sB[1] = boxPointB.localPoint.getY() > 0.0f;
1134  sB[2] = boxPointB.localPoint.getZ() > 0.0f;
1135 
1136  if ( featureA == F ) {
1137  boxPointA.setFaceFeature( dimA[2], sA[dimA[2]] );
1138  } else if ( featureA == E ) {
1139  boxPointA.setEdgeFeature( dimA[2], sA[dimA[2]], dimA[otherFaceDimA], sA[dimA[otherFaceDimA]] );
1140  } else {
1141  boxPointA.setVertexFeature( sA[0], sA[1], sA[2] );
1142  }
1143 
1144  if ( featureB == F ) {
1145  boxPointB.setFaceFeature( dimB[2], sB[dimB[2]] );
1146  } else if ( featureB == E ) {
1147  boxPointB.setEdgeFeature( dimB[2], sB[dimB[2]], dimB[otherFaceDimB], sB[dimB[otherFaceDimB]] );
1148  } else {
1149  boxPointB.setVertexFeature( sB[0], sB[1], sB[2] );
1150  }
1151 #endif
1152 
1153  normal = transformA * axisA;
1154 
1155  if ( maxGap < 0.0f ) {
1156  return (maxGap);
1157  } else {
1158  return (sqrtf( minDistSqr ));
1159  }
1160 }
Vectormath::Aos::Transform3::getTranslation
const Vector3 getTranslation() const
Definition: neon/mat_aos.h:1368
EdgeEdgeTests
void EdgeEdgeTests(bool &done, float &minDistSqr, vmPoint3 &localPointA, vmPoint3 &localPointB, int &otherFaceDimA, int &otherFaceDimB, FeatureType &featureA, FeatureType &featureB, const vmVector3 &hA, const vmVector3 &hB, PE_REF(vmVector3) faceOffsetAB, PE_REF(vmVector3) faceOffsetBA, const vmMatrix3 &matrixAB, const vmMatrix3 &matrixBA, PE_REF(vmVector3) signsA, PE_REF(vmVector3) signsB, PE_REF(vmVector3) scalesA, PE_REF(vmVector3) scalesB, bool first)
Definition: boxBoxDistance.cpp:532
BoxPoint::localPoint
vmPoint3 localPoint
Definition: Box.h:98
CustomEdgeEdgeTest_1010
float CustomEdgeEdgeTest_1010(bool &inVoronoi, float &tA, float &tB, const vmVector3 &hA, const vmVector3 &hB, PE_REF(vmVector3) faceOffsetAB, PE_REF(vmVector3) faceOffsetBA, const vmMatrix3 &matrixAB, const vmMatrix3 &matrixBA, PE_REF(vmVector3) signsA, PE_REF(vmVector3) signsB, PE_REF(vmVector3) scalesA, PE_REF(vmVector3) scalesB)
Definition: boxBoxDistance.cpp:500
vmPoint3
Vectormath::Aos::Point3 vmPoint3
Definition: vmInclude.h:27
BoxPoint::setFaceFeature
void setFaceFeature(int dim, int plus)
Definition: Box.h:134
VertexBFaceATest
float VertexBFaceATest(bool &inVoronoi, float &t0, float &t1, const vmVector3 &hA, PE_REF(vmVector3) faceOffsetAB, PE_REF(vmVector3) faceOffsetBA, const vmMatrix3 &matrixAB, const vmMatrix3 &matrixBA, PE_REF(vmVector3) signsB, PE_REF(vmVector3) scalesB)
Definition: boxBoxDistance.cpp:117
CustomEdgeEdgeTest_0101
float CustomEdgeEdgeTest_0101(bool &inVoronoi, float &tA, float &tB, const vmVector3 &hA, const vmVector3 &hB, PE_REF(vmVector3) faceOffsetAB, PE_REF(vmVector3) faceOffsetBA, const vmMatrix3 &matrixAB, const vmMatrix3 &matrixBA, PE_REF(vmVector3) signsA, PE_REF(vmVector3) signsB, PE_REF(vmVector3) scalesA, PE_REF(vmVector3) scalesB)
Definition: boxBoxDistance.cpp:443
Vectormath::Aos::Matrix3::setCol0
Matrix3 & setCol0(const Vector3 &col0)
Definition: neon/mat_aos.h:76
AaxisTest
#define AaxisTest(dim, letter, first)
Definition: boxBoxDistance.cpp:45
Vectormath::Aos::Matrix3::setCol2
Matrix3 & setCol2(const Vector3 &col2)
Definition: neon/mat_aos.h:88
EdgeEdge_SetNewMin
#define EdgeEdge_SetNewMin(ac_letter, ad_letter, bc_letter, bd_letter)
Definition: boxBoxDistance.cpp:518
CustomEdgeEdgeTest_0110
float CustomEdgeEdgeTest_0110(bool &inVoronoi, float &tA, float &tB, const vmVector3 &hA, const vmVector3 &hB, PE_REF(vmVector3) faceOffsetAB, PE_REF(vmVector3) faceOffsetBA, const vmMatrix3 &matrixAB, const vmMatrix3 &matrixBA, PE_REF(vmVector3) signsA, PE_REF(vmVector3) signsB, PE_REF(vmVector3) scalesA, PE_REF(vmVector3) scalesB)
Definition: boxBoxDistance.cpp:462
FeatureType
FeatureType
Definition: Box.h:33
Vectormath::Aos::Vector3::setY
Vector3 & setY(float y)
Definition: neon/vec_aos.h:207
vmMatrix3
Vectormath::Aos::Matrix3 vmMatrix3
Definition: vmInclude.h:25
F
Definition: Box.h:33
BoxPoint::setVertexFeature
void setVertexFeature(int plusX, int plusY, int plusZ)
Definition: Box.h:113
Vectormath::Aos::Matrix3::identity
static const Matrix3 identity()
Definition: neon/mat_aos.h:295
voronoiTol
static const float voronoiTol
Definition: boxBoxDistance.cpp:38
Vectormath::Aos::Vector3::setZ
Vector3 & setZ(float z)
Definition: neon/vec_aos.h:218
BoxPoint::setEdgeFeature
void setEdgeFeature(int dim0, int plus0, int dim1, int plus1)
Definition: Box.h:121
Box::mHalf
vmVector3 mHalf
Definition: Box.h:42
Vectormath::Aos::Vector3::setX
Vector3 & setX(float x)
Definition: neon/vec_aos.h:196
Vectormath::Aos::copySignPerElem
const Vector3 copySignPerElem(const Vector3 &vec0, const Vector3 &vec1)
Definition: neon/vec_aos.h:387
Vectormath::Aos::Matrix3::getCol0
const Vector3 getCol0() const
Definition: neon/mat_aos.h:122
PE_REF
#define PE_REF(a)
Definition: Box.h:22
VertexAFaceBTest
float VertexAFaceBTest(bool &inVoronoi, float &t0, float &t1, const vmVector3 &hB, PE_REF(vmVector3) faceOffsetAB, PE_REF(vmVector3) faceOffsetBA, const vmMatrix3 &matrixAB, const vmMatrix3 &matrixBA, PE_REF(vmVector3) signsA, PE_REF(vmVector3) scalesA)
Definition: boxBoxDistance.cpp:250
Vectormath::Aos::Transform3::getUpper3x3
const Matrix3 getUpper3x3() const
Definition: neon/mat_aos.h:1357
BoxSepAxisType
BoxSepAxisType
Definition: boxBoxDistance.cpp:29
Vectormath::Aos::Matrix3::setCol1
Matrix3 & setCol1(const Vector3 &col1)
Definition: neon/mat_aos.h:82
Vectormath::Aos::orthoInverse
const Matrix4 orthoInverse(const Matrix4 &mat)
Definition: neon/mat_aos.h:676
Box
The Box is an internal class used by the boxBoxDistance calculation.
Definition: Box.h:39
Vectormath::Aos::distSqr
float distSqr(const Point3 &pnt0, const Point3 &pnt1)
Definition: neon/vec_aos.h:1390
E
Definition: Box.h:33
Vectormath::Aos::Point3::setZ
Point3 & setZ(float z)
Definition: neon/vec_aos.h:1185
boxBoxDistance
float boxBoxDistance(vmVector3 &normal, BoxPoint &boxPointA, BoxPoint &boxPointB, PE_REF(Box) boxA, const vmTransform3 &transformA, PE_REF(Box) boxB, const vmTransform3 &transformB, float distanceThreshold)
Definition: boxBoxDistance.cpp:777
Vectormath::Aos::transpose
const Matrix3 transpose(const Matrix3 &mat)
Definition: neon/mat_aos.h:165
vmVector3
Vectormath::Aos::Vector3 vmVector3
Definition: vmInclude.h:23
sqr
static float sqr(float a)
Definition: boxBoxDistance.cpp:24
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:827
CrossAxisTest
#define CrossAxisTest(dima, dimb, letterb)
Definition: boxBoxDistance.cpp:83
VertexAFaceBTests
void VertexAFaceBTests(bool &done, float &minDistSqr, vmPoint3 &localPointA, vmPoint3 &localPointB, FeatureType &featureA, FeatureType &featureB, const vmVector3 &hB, PE_REF(vmVector3) faceOffsetAB, PE_REF(vmVector3) faceOffsetBA, const vmMatrix3 &matrixAB, const vmMatrix3 &matrixBA, PE_REF(vmVector3) signsA, PE_REF(vmVector3) scalesA, bool first)
Definition: boxBoxDistance.cpp:299
CustomEdgeEdgeTest
#define CustomEdgeEdgeTest(ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter)
Definition: boxBoxDistance.cpp:381
VertexAFaceB_SetNewMin
#define VertexAFaceB_SetNewMin()
Definition: boxBoxDistance.cpp:287
CustomEdgeEdgeTest_1001
float CustomEdgeEdgeTest_1001(bool &inVoronoi, float &tA, float &tB, const vmVector3 &hA, const vmVector3 &hB, PE_REF(vmVector3) faceOffsetAB, PE_REF(vmVector3) faceOffsetBA, const vmMatrix3 &matrixAB, const vmMatrix3 &matrixBA, PE_REF(vmVector3) signsA, PE_REF(vmVector3) signsB, PE_REF(vmVector3) scalesA, PE_REF(vmVector3) scalesB)
Definition: boxBoxDistance.cpp:481
BoxPoint
The BoxPoint class is an internally used class to contain feature information for boxBoxDistance calc...
Definition: Box.h:93
Vectormath::Aos::mulPerElem
const Matrix3 mulPerElem(const Matrix3 &mat0, const Matrix3 &mat1)
Definition: neon/mat_aos.h:286
VertexBFaceA_SetNewMin
#define VertexBFaceA_SetNewMin()
Definition: boxBoxDistance.cpp:161
Vectormath::Aos::Matrix3::getCol2
const Vector3 getCol2() const
Definition: neon/mat_aos.h:132
Vectormath::Aos::absPerElem
const Matrix3 absPerElem(const Matrix3 &mat)
Definition: neon/mat_aos.h:233
VertexBFaceATests
void VertexBFaceATests(bool &done, float &minDistSqr, vmPoint3 &localPointA, vmPoint3 &localPointB, FeatureType &featureA, FeatureType &featureB, const vmVector3 &hA, PE_REF(vmVector3) faceOffsetAB, PE_REF(vmVector3) faceOffsetBA, const vmMatrix3 &matrixAB, const vmMatrix3 &matrixBA, PE_REF(vmVector3) signsB, PE_REF(vmVector3) scalesB, bool first)
Definition: boxBoxDistance.cpp:173
BaxisTest
#define BaxisTest(dim, letter)
Definition: boxBoxDistance.cpp:70
Vectormath::Aos::Matrix3::getCol1
const Vector3 getCol1() const
Definition: neon/mat_aos.h:127
Generated on Wed Feb 5 2014 21:36:50 for Bullet Collision Detection & Physics Library by   doxygen 1.8.6