← Back to branch summary

~diresu/blender/blender-command-port

« back to all changes in this revision

Viewing changes to extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp

  • Committer: theeth
  • Date: 2008-10-14 16:52:04 UTC
  • Revision ID: vcs-imports@canonical.com-20081014165204-r32w2gm6s0osvdhn
copy back trunk

Show diffs side-by-side

added added

removed removed

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