1 /*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23 /*
24
25 standard ODE geometry primitives: public API and pairwise collision functions.
26
27 the rule is that only the low level primitive collision functions should set
28 dContactGeom::g1 and dContactGeom::g2.
29
30 */
31
32 #include <ode/common.h>
33 #include <ode/collision.h>
34 #include <ode/matrix.h>
35 #include <ode/rotation.h>
36 #include <ode/odemath.h>
37 #include "collision_kernel.h"
38 #include "collision_std.h"
39 #include "collision_util.h"
40
41 #ifdef _MSC_VER
42 #pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
43 #endif
44
45 //****************************************************************************
46 // box public API
47
dxBox(dSpaceID space,dReal lx,dReal ly,dReal lz)48 dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1)
49 {
50 dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
51 type = dBoxClass;
52 side[0] = lx;
53 side[1] = ly;
54 side[2] = lz;
55 updateZeroSizedFlag(!lx || !ly || !lz);
56 }
57
58
computeAABB()59 void dxBox::computeAABB()
60 {
61 const dMatrix3& R = final_posr->R;
62 const dVector3& pos = final_posr->pos;
63
64 dReal xrange = REAL(0.5) * (dFabs (R[0] * side[0]) +
65 dFabs (R[1] * side[1]) + dFabs (R[2] * side[2]));
66 dReal yrange = REAL(0.5) * (dFabs (R[4] * side[0]) +
67 dFabs (R[5] * side[1]) + dFabs (R[6] * side[2]));
68 dReal zrange = REAL(0.5) * (dFabs (R[8] * side[0]) +
69 dFabs (R[9] * side[1]) + dFabs (R[10] * side[2]));
70 aabb[0] = pos[0] - xrange;
71 aabb[1] = pos[0] + xrange;
72 aabb[2] = pos[1] - yrange;
73 aabb[3] = pos[1] + yrange;
74 aabb[4] = pos[2] - zrange;
75 aabb[5] = pos[2] + zrange;
76 }
77
78
dCreateBox(dSpaceID space,dReal lx,dReal ly,dReal lz)79 dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz)
80 {
81 return new dxBox (space,lx,ly,lz);
82 }
83
84
dGeomBoxSetLengths(dGeomID g,dReal lx,dReal ly,dReal lz)85 void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz)
86 {
87 dUASSERT (g && g->type == dBoxClass,"argument not a box");
88 dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
89 dxBox *b = (dxBox*) g;
90 b->side[0] = lx;
91 b->side[1] = ly;
92 b->side[2] = lz;
93 b->updateZeroSizedFlag(!lx || !ly || !lz);
94 dGeomMoved (g);
95 }
96
97
dGeomBoxGetLengths(dGeomID g,dVector3 result)98 void dGeomBoxGetLengths (dGeomID g, dVector3 result)
99 {
100 dUASSERT (g && g->type == dBoxClass,"argument not a box");
101 dxBox *b = (dxBox*) g;
102 result[0] = b->side[0];
103 result[1] = b->side[1];
104 result[2] = b->side[2];
105 }
106
107
dGeomBoxPointDepth(dGeomID g,dReal x,dReal y,dReal z)108 dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z)
109 {
110 dUASSERT (g && g->type == dBoxClass,"argument not a box");
111 g->recomputePosr();
112 dxBox *b = (dxBox*) g;
113
114 // Set p = (x,y,z) relative to box center
115 //
116 // This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2)
117
118 dVector3 p,q;
119
120 p[0] = x - b->final_posr->pos[0];
121 p[1] = y - b->final_posr->pos[1];
122 p[2] = z - b->final_posr->pos[2];
123
124 // Rotate p into box's coordinate frame, so we can
125 // treat the OBB as an AABB
126
127 dMULTIPLY1_331 (q,b->final_posr->R,p);
128
129 // Record distance from point to each successive box side, and see
130 // if the point is inside all six sides
131
132 dReal dist[6];
133 int i;
134
135 bool inside = true;
136
137 for (i=0; i < 3; i++) {
138 dReal side = b->side[i] * REAL(0.5);
139
140 dist[i ] = side - q[i];
141 dist[i+3] = side + q[i];
142
143 if ((dist[i] < 0) || (dist[i+3] < 0)) {
144 inside = false;
145 }
146 }
147
148 // If point is inside the box, the depth is the smallest positive distance
149 // to any side
150
151 if (inside) {
152 dReal smallest_dist = (dReal) (unsigned) -1;
153
154 for (i=0; i < 6; i++) {
155 if (dist[i] < smallest_dist) smallest_dist = dist[i];
156 }
157
158 return smallest_dist;
159 }
160
161 // Otherwise, if point is outside the box, the depth is the largest
162 // distance to any side. This is an approximation to the 'proper'
163 // solution (the proper solution may be larger in some cases).
164
165 dReal largest_dist = 0;
166
167 for (i=0; i < 6; i++) {
168 if (dist[i] > largest_dist) largest_dist = dist[i];
169 }
170
171 return -largest_dist;
172 }
173
174 //****************************************************************************
175 // box-box collision utility
176
177
178 // find all the intersection points between the 2D rectangle with vertices
179 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
180 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
181 //
182 // the intersection points are returned as x,y pairs in the 'ret' array.
183 // the number of intersection points is returned by the function (this will
184 // be in the range 0 to 8).
185
intersectRectQuad(dReal h[2],dReal p[8],dReal ret[16])186 static int intersectRectQuad (dReal h[2], dReal p[8], dReal ret[16])
187 {
188 // q (and r) contain nq (and nr) coordinate points for the current (and
189 // chopped) polygons
190 int nq=4,nr;
191 dReal buffer[16];
192 dReal *q = p;
193 dReal *r = ret;
194 for (int dir=0; dir <= 1; dir++) {
195 // direction notation: xy[0] = x axis, xy[1] = y axis
196 for (int sign=-1; sign <= 1; sign += 2) {
197 // chop q along the line xy[dir] = sign*h[dir]
198 dReal *pq = q;
199 dReal *pr = r;
200 nr = 0;
201 for (int i=nq; i > 0; i--) {
202 // go through all points in q and all lines between adjacent points
203 if (sign*pq[dir] < h[dir]) {
204 // this point is inside the chopping line
205 pr[0] = pq[0];
206 pr[1] = pq[1];
207 pr += 2;
208 nr++;
209 if (nr & 8) {
210 q = r;
211 goto done;
212 }
213 }
214 dReal *nextq = (i > 1) ? pq+2 : q;
215 if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
216 // this line crosses the chopping line
217 pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
218 (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
219 pr[dir] = sign*h[dir];
220 pr += 2;
221 nr++;
222 if (nr & 8) {
223 q = r;
224 goto done;
225 }
226 }
227 pq += 2;
228 }
229 q = r;
230 r = (q==ret) ? buffer : ret;
231 nq = nr;
232 }
233 }
234 done:
235 if (q != ret) memcpy (ret,q,nr*2*sizeof(dReal));
236 return nr;
237 }
238
239
240 // given n points in the plane (array p, of size 2*n), generate m points that
241 // best represent the whole set. the definition of 'best' here is not
242 // predetermined - the idea is to select points that give good box-box
243 // collision detection behavior. the chosen point indexes are returned in the
244 // array iret (of size m). 'i0' is always the first entry in the array.
245 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
246 // in the range [0..n-1].
247
cullPoints(int n,dReal p[],int m,int i0,int iret[])248 void cullPoints (int n, dReal p[], int m, int i0, int iret[])
249 {
250 // compute the centroid of the polygon in cx,cy
251 int i,j;
252 dReal a,cx,cy,q;
253 if (n==1) {
254 cx = p[0];
255 cy = p[1];
256 }
257 else if (n==2) {
258 cx = REAL(0.5)*(p[0] + p[2]);
259 cy = REAL(0.5)*(p[1] + p[3]);
260 }
261 else {
262 a = 0;
263 cx = 0;
264 cy = 0;
265 for (i=0; i<(n-1); i++) {
266 q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
267 a += q;
268 cx += q*(p[i*2]+p[i*2+2]);
269 cy += q*(p[i*2+1]+p[i*2+3]);
270 }
271 q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
272 a = dRecip(REAL(3.0)*(a+q));
273 cx = a*(cx + q*(p[n*2-2]+p[0]));
274 cy = a*(cy + q*(p[n*2-1]+p[1]));
275 }
276
277 // compute the angle of each point w.r.t. the centroid
278 dReal A[8];
279 for (i=0; i<n; i++) A[i] = dAtan2(p[i*2+1]-cy,p[i*2]-cx);
280
281 // search for points that have angles closest to A[i0] + i*(2*pi/m).
282 int avail[8];
283 for (i=0; i<n; i++) avail[i] = 1;
284 avail[i0] = 0;
285 iret[0] = i0;
286 iret++;
287 for (j=1; j<m; j++) {
288 a = (dReal)(dReal(j)*(2*M_PI/m) + A[i0]);
289 if (a > M_PI) a -= (dReal)(2*M_PI);
290 dReal maxdiff=1e9,diff;
291 #ifndef dNODEBUG
292 *iret = i0; // iret is not allowed to keep this value
293 #endif
294 for (i=0; i<n; i++) {
295 if (avail[i]) {
296 diff = dFabs (A[i]-a);
297 if (diff > M_PI) diff = (dReal) (2*M_PI - diff);
298 if (diff < maxdiff) {
299 maxdiff = diff;
300 *iret = i;
301 }
302 }
303 }
304 #ifndef dNODEBUG
305 dIASSERT (*iret != i0); // ensure iret got set
306 #endif
307 avail[*iret] = 0;
308 iret++;
309 }
310 }
311
312
313 // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
314 // generate contact points. this returns 0 if there is no contact otherwise
315 // it returns the number of contacts generated.
316 // `normal' returns the contact normal.
317 // `depth' returns the maximum penetration depth along that normal.
318 // `return_code' returns a number indicating the type of contact that was
319 // detected:
320 // 1,2,3 = box 2 intersects with a face of box 1
321 // 4,5,6 = box 1 intersects with a face of box 2
322 // 7..15 = edge-edge contact
323 // `maxc' is the maximum number of contacts allowed to be generated, i.e.
324 // the size of the `contact' array.
325 // `contact' and `skip' are the contact array information provided to the
326 // collision functions. this function only fills in the position and depth
327 // fields.
328
329
dBoxBox(const dVector3 p1,const dMatrix3 R1,const dVector3 side1,const dVector3 p2,const dMatrix3 R2,const dVector3 side2,dVector3 normal,dReal * depth,int * return_code,int flags,dContactGeom * contact,int skip)330 int dBoxBox (const dVector3 p1, const dMatrix3 R1,
331 const dVector3 side1, const dVector3 p2,
332 const dMatrix3 R2, const dVector3 side2,
333 dVector3 normal, dReal *depth, int *return_code,
334 int flags, dContactGeom *contact, int skip)
335 {
336 const dReal fudge_factor = REAL(1.05);
337 dVector3 p,pp,normalC={0,0,0};
338 const dReal *normalR = 0;
339 dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
340 Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val;
341 int i,j,invert_normal,code;
342
343 // get vector from centers of box 1 to box 2, relative to box 1
344 p[0] = p2[0] - p1[0];
345 p[1] = p2[1] - p1[1];
346 p[2] = p2[2] - p1[2];
347 dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
348
349 // get side lengths / 2
350 A[0] = side1[0]*REAL(0.5);
351 A[1] = side1[1]*REAL(0.5);
352 A[2] = side1[2]*REAL(0.5);
353 B[0] = side2[0]*REAL(0.5);
354 B[1] = side2[1]*REAL(0.5);
355 B[2] = side2[2]*REAL(0.5);
356
357 // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
358 R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
359 R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
360 R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
361
362 Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
363 Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
364 Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
365
366 // for all 15 possible separating axes:
367 // * see if the axis separates the boxes. if so, return 0.
368 // * find the depth of the penetration along the separating axis (s2)
369 // * if this is the largest depth so far, record it.
370 // the normal vector will be set to the separating axis with the smallest
371 // depth. note: normalR is set to point to a column of R1 or R2 if that is
372 // the smallest depth normal so far. otherwise normalR is 0 and normalC is
373 // set to a vector relative to body 1. invert_normal is 1 if the sign of
374 // the normal should be flipped.
375
376 do {
377 #define TST(expr1,expr2,norm,cc) \
378 expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
379 s2 = dFabs(expr1_val) - (expr2); \
380 if (s2 > 0) return 0; \
381 if (s2 > s) { \
382 s = s2; \
383 normalR = norm; \
384 invert_normal = ((expr1_val) < 0); \
385 code = (cc); \
386 if (flags & CONTACTS_UNIMPORTANT) break; \
387 }
388
389 s = -dInfinity;
390 invert_normal = 0;
391 code = 0;
392
393 // separating axis = u1,u2,u3
394 TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
395 TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
396 TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
397
398 // separating axis = v1,v2,v3
399 TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
400 TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
401 TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
402
403 // note: cross product axes need to be scaled when s is computed.
404 // normal (n1,n2,n3) is relative to box 1.
405 #undef TST
406 #define TST(expr1,expr2,n1,n2,n3,cc) \
407 expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
408 s2 = dFabs(expr1_val) - (expr2); \
409 if (s2 > 0) return 0; \
410 l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
411 if (l > 0) { \
412 s2 /= l; \
413 if (s2*fudge_factor > s) { \
414 s = s2; \
415 normalR = 0; \
416 normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
417 invert_normal = ((expr1_val) < 0); \
418 code = (cc); \
419 if (flags & CONTACTS_UNIMPORTANT) break; \
420 } \
421 }
422
423 // We only need to check 3 edges per box
424 // since parallel edges are equivalent.
425
426 // separating axis = u1 x (v1,v2,v3)
427 TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
428 TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
429 TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
430
431 // separating axis = u2 x (v1,v2,v3)
432 TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
433 TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
434 TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
435
436 // separating axis = u3 x (v1,v2,v3)
437 TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
438 TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
439 TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
440 #undef TST
441 } while (0);
442
443 if (!code) return 0;
444
445 // if we get to this point, the boxes interpenetrate. compute the normal
446 // in global coordinates.
447 if (normalR) {
448 normal[0] = normalR[0];
449 normal[1] = normalR[4];
450 normal[2] = normalR[8];
451 }
452 else {
453 dMULTIPLY0_331 (normal,R1,normalC);
454 }
455 if (invert_normal) {
456 normal[0] = -normal[0];
457 normal[1] = -normal[1];
458 normal[2] = -normal[2];
459 }
460 *depth = -s;
461
462 // compute contact point(s)
463
464 if (code > 6) {
465 // An edge from box 1 touches an edge from box 2.
466 // find a point pa on the intersecting edge of box 1
467 dVector3 pa;
468 dReal sign;
469 // Copy p1 into pa
470 for (i=0; i<3; i++) pa[i] = p1[i]; // why no memcpy?
471 // Get world position of p2 into pa
472 for (j=0; j<3; j++) {
473 sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
474 for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
475 }
476
477 // find a point pb on the intersecting edge of box 2
478 dVector3 pb;
479 // Copy p2 into pb
480 for (i=0; i<3; i++) pb[i] = p2[i]; // why no memcpy?
481 // Get world position of p2 into pb
482 for (j=0; j<3; j++) {
483 sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
484 for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
485 }
486
487 dReal alpha,beta;
488 dVector3 ua,ub;
489 // Get direction of first edge
490 for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
491 // Get direction of second edge
492 for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
493 // Get closest points between edges (one at each)
494 dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
495 for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
496 for (i=0; i<3; i++) pb[i] += ub[i]*beta;
497 // Set the contact point as halfway between the 2 closest points
498 for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
499 contact[0].depth = *depth;
500 *return_code = code;
501 return 1;
502 }
503
504 // okay, we have a face-something intersection (because the separating
505 // axis is perpendicular to a face). define face 'a' to be the reference
506 // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
507 // the incident face (the closest face of the other box).
508 // Note: Unmodified parameter values are being used here
509 const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
510 if (code <= 3) { // One of the faces of box 1 is the reference face
511 Ra = R1; // Rotation of 'a'
512 Rb = R2; // Rotation of 'b'
513 pa = p1; // Center (location) of 'a'
514 pb = p2; // Center (location) of 'b'
515 Sa = A; // Side Lenght of 'a'
516 Sb = B; // Side Lenght of 'b'
517 }
518 else { // One of the faces of box 2 is the reference face
519 Ra = R2; // Rotation of 'a'
520 Rb = R1; // Rotation of 'b'
521 pa = p2; // Center (location) of 'a'
522 pb = p1; // Center (location) of 'b'
523 Sa = B; // Side Lenght of 'a'
524 Sb = A; // Side Lenght of 'b'
525 }
526
527 // nr = normal vector of reference face dotted with axes of incident box.
528 // anr = absolute values of nr.
529 /*
530 The normal is flipped if necessary so it always points outward from box 'a',
531 box 'b' is thus always the incident box
532 */
533 dVector3 normal2,nr,anr;
534 if (code <= 3) {
535 normal2[0] = normal[0];
536 normal2[1] = normal[1];
537 normal2[2] = normal[2];
538 }
539 else {
540 normal2[0] = -normal[0];
541 normal2[1] = -normal[1];
542 normal2[2] = -normal[2];
543 }
544 // Rotate normal2 in incident box opposite direction
545 dMULTIPLY1_331 (nr,Rb,normal2);
546 anr[0] = dFabs (nr[0]);
547 anr[1] = dFabs (nr[1]);
548 anr[2] = dFabs (nr[2]);
549
550 // find the largest compontent of anr: this corresponds to the normal
551 // for the incident face. the other axis numbers of the incident face
552 // are stored in a1,a2.
553 int lanr,a1,a2;
554 if (anr[1] > anr[0]) {
555 if (anr[1] > anr[2]) {
556 a1 = 0;
557 lanr = 1;
558 a2 = 2;
559 }
560 else {
561 a1 = 0;
562 a2 = 1;
563 lanr = 2;
564 }
565 }
566 else {
567 if (anr[0] > anr[2]) {
568 lanr = 0;
569 a1 = 1;
570 a2 = 2;
571 }
572 else {
573 a1 = 0;
574 a2 = 1;
575 lanr = 2;
576 }
577 }
578
579 // compute center point of incident face, in reference-face coordinates
580 dVector3 center;
581 if (nr[lanr] < 0) {
582 for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
583 }
584 else {
585 for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
586 }
587
588 // find the normal and non-normal axis numbers of the reference box
589 int codeN,code1,code2;
590 if (code <= 3) codeN = code-1; else codeN = code-4;
591 if (codeN==0) {
592 code1 = 1;
593 code2 = 2;
594 }
595 else if (codeN==1) {
596 code1 = 0;
597 code2 = 2;
598 }
599 else {
600 code1 = 0;
601 code2 = 1;
602 }
603
604 // find the four corners of the incident face, in reference-face coordinates
605 dReal quad[8]; // 2D coordinate of incident face (x,y pairs)
606 dReal c1,c2,m11,m12,m21,m22;
607 c1 = dDOT14 (center,Ra+code1);
608 c2 = dDOT14 (center,Ra+code2);
609 // optimize this? - we have already computed this data above, but it is not
610 // stored in an easy-to-index format. for now it's quicker just to recompute
611 // the four dot products.
612 m11 = dDOT44 (Ra+code1,Rb+a1);
613 m12 = dDOT44 (Ra+code1,Rb+a2);
614 m21 = dDOT44 (Ra+code2,Rb+a1);
615 m22 = dDOT44 (Ra+code2,Rb+a2);
616 {
617 dReal k1 = m11*Sb[a1];
618 dReal k2 = m21*Sb[a1];
619 dReal k3 = m12*Sb[a2];
620 dReal k4 = m22*Sb[a2];
621 quad[0] = c1 - k1 - k3;
622 quad[1] = c2 - k2 - k4;
623 quad[2] = c1 - k1 + k3;
624 quad[3] = c2 - k2 + k4;
625 quad[4] = c1 + k1 + k3;
626 quad[5] = c2 + k2 + k4;
627 quad[6] = c1 + k1 - k3;
628 quad[7] = c2 + k2 - k4;
629 }
630
631 // find the size of the reference face
632 dReal rect[2];
633 rect[0] = Sa[code1];
634 rect[1] = Sa[code2];
635
636 // intersect the incident and reference faces
637 dReal ret[16];
638 int n = intersectRectQuad (rect,quad,ret);
639 if (n < 1) return 0; // this should never happen
640
641 // convert the intersection points into reference-face coordinates,
642 // and compute the contact position and depth for each point. only keep
643 // those points that have a positive (penetrating) depth. delete points in
644 // the 'ret' array as necessary so that 'point' and 'ret' correspond.
645 dReal point[3*8]; // penetrating contact points
646 dReal dep[8]; // depths for those points
647 dReal det1 = dRecip(m11*m22 - m12*m21);
648 m11 *= det1;
649 m12 *= det1;
650 m21 *= det1;
651 m22 *= det1;
652 int cnum = 0; // number of penetrating contact points found
653 for (j=0; j < n; j++) {
654 dReal k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
655 dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
656 for (i=0; i<3; i++) point[cnum*3+i] =
657 center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
658 dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
659 if (dep[cnum] >= 0) {
660 ret[cnum*2] = ret[j*2];
661 ret[cnum*2+1] = ret[j*2+1];
662 cnum++;
663 if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
664 break;
665 }
666 }
667 }
668 if (cnum < 1) {
669 return 0; // this should not happen, yet does at times (demo_plane2d single precision).
670 }
671
672 // we can't generate more contacts than we actually have
673 int maxc = flags & NUMC_MASK;
674 if (maxc > cnum) maxc = cnum;
675 if (maxc < 1) maxc = 1; // Even though max count must not be zero this check is kept for backward compatibility as this is a public function
676
677 if (cnum <= maxc) {
678 // we have less contacts than we need, so we use them all
679 for (j=0; j < cnum; j++) {
680 dContactGeom *con = CONTACT(contact,skip*j);
681 for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
682 con->depth = dep[j];
683 }
684 }
685 else {
686 dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed
687 // we have more contacts than are wanted, some of them must be culled.
688 // find the deepest point, it is always the first contact.
689 int i1 = 0;
690 dReal maxdepth = dep[0];
691 for (i=1; i<cnum; i++) {
692 if (dep[i] > maxdepth) {
693 maxdepth = dep[i];
694 i1 = i;
695 }
696 }
697
698 int iret[8];
699 cullPoints (cnum,ret,maxc,i1,iret);
700
701 for (j=0; j < maxc; j++) {
702 dContactGeom *con = CONTACT(contact,skip*j);
703 for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
704 con->depth = dep[iret[j]];
705 }
706 cnum = maxc;
707 }
708
709 *return_code = code;
710 return cnum;
711 }
712
713
714
dCollideBoxBox(dxGeom * o1,dxGeom * o2,int flags,dContactGeom * contact,int skip)715 int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
716 dContactGeom *contact, int skip)
717 {
718 dIASSERT (skip >= (int)sizeof(dContactGeom));
719 dIASSERT (o1->type == dBoxClass);
720 dIASSERT (o2->type == dBoxClass);
721 dIASSERT ((flags & NUMC_MASK) >= 1);
722
723 dVector3 normal;
724 dReal depth;
725 int code;
726 dxBox *b1 = (dxBox*) o1;
727 dxBox *b2 = (dxBox*) o2;
728 int num = dBoxBox (o1->final_posr->pos,o1->final_posr->R,b1->side, o2->final_posr->pos,o2->final_posr->R,b2->side,
729 normal,&depth,&code,flags,contact,skip);
730 for (int i=0; i<num; i++) {
731 dContactGeom *currContact = CONTACT(contact,i*skip);
732 currContact->normal[0] = -normal[0];
733 currContact->normal[1] = -normal[1];
734 currContact->normal[2] = -normal[2];
735 currContact->g1 = o1;
736 currContact->g2 = o2;
737 currContact->side1 = -1;
738 currContact->side2 = -1;
739 }
740 return num;
741 }
742
743
dCollideBoxPlane(dxGeom * o1,dxGeom * o2,int flags,dContactGeom * contact,int skip)744 int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
745 int flags, dContactGeom *contact, int skip)
746 {
747 dIASSERT (skip >= (int)sizeof(dContactGeom));
748 dIASSERT (o1->type == dBoxClass);
749 dIASSERT (o2->type == dPlaneClass);
750 dIASSERT ((flags & NUMC_MASK) >= 1);
751
752 dxBox *box = (dxBox*) o1;
753 dxPlane *plane = (dxPlane*) o2;
754
755 contact->g1 = o1;
756 contact->g2 = o2;
757 contact->side1 = -1;
758 contact->side2 = -1;
759
760 int ret = 0;
761
762 //@@@ problem: using 4-vector (plane->p) as 3-vector (normal).
763 const dReal *R = o1->final_posr->R; // rotation of box
764 const dReal *n = plane->p; // normal vector
765
766 // project sides lengths along normal vector, get absolute values
767 dReal Q1 = dDOT14(n,R+0);
768 dReal Q2 = dDOT14(n,R+1);
769 dReal Q3 = dDOT14(n,R+2);
770 dReal A1 = box->side[0] * Q1;
771 dReal A2 = box->side[1] * Q2;
772 dReal A3 = box->side[2] * Q3;
773 dReal B1 = dFabs(A1);
774 dReal B2 = dFabs(A2);
775 dReal B3 = dFabs(A3);
776
777 // early exit test
778 dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dDOT(n,o1->final_posr->pos);
779 if (depth < 0) return 0;
780
781 // find number of contacts requested
782 int maxc = flags & NUMC_MASK;
783 // if (maxc < 1) maxc = 1; // an assertion is made on entry
784 if (maxc > 3) maxc = 3; // not more than 3 contacts per box allowed
785
786 // find deepest point
787 dVector3 p;
788 p[0] = o1->final_posr->pos[0];
789 p[1] = o1->final_posr->pos[1];
790 p[2] = o1->final_posr->pos[2];
791 #define FOO(i,op) \
792 p[0] op REAL(0.5)*box->side[i] * R[0+i]; \
793 p[1] op REAL(0.5)*box->side[i] * R[4+i]; \
794 p[2] op REAL(0.5)*box->side[i] * R[8+i];
795 #define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=) } else { FOO(i,+=) }
796 BAR(0,1);
797 BAR(1,2);
798 BAR(2,3);
799 #undef FOO
800 #undef BAR
801
802 // the deepest point is the first contact point
803 contact->pos[0] = p[0];
804 contact->pos[1] = p[1];
805 contact->pos[2] = p[2];
806 contact->normal[0] = n[0];
807 contact->normal[1] = n[1];
808 contact->normal[2] = n[2];
809 contact->depth = depth;
810 ret = 1; // ret is number of contact points found so far
811 if (maxc == 1) goto done;
812
813 // get the second and third contact points by starting from `p' and going
814 // along the two sides with the smallest projected length.
815
816 #define FOO(i,j,op) \
817 CONTACT(contact,i*skip)->pos[0] = p[0] op box->side[j] * R[0+j]; \
818 CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \
819 CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j];
820 #define BAR(ctact,side,sideinc) \
821 depth -= B ## sideinc; \
822 if (depth < 0) goto done; \
823 if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO(ctact,side,-); } \
824 CONTACT(contact,ctact*skip)->depth = depth; \
825 ret++;
826
827 CONTACT(contact,skip)->normal[0] = n[0];
828 CONTACT(contact,skip)->normal[1] = n[1];
829 CONTACT(contact,skip)->normal[2] = n[2];
830 if (maxc == 3) {
831 CONTACT(contact,2*skip)->normal[0] = n[0];
832 CONTACT(contact,2*skip)->normal[1] = n[1];
833 CONTACT(contact,2*skip)->normal[2] = n[2];
834 }
835
836 if (B1 < B2) {
837 if (B3 < B1) goto use_side_3; else {
838 BAR(1,0,1); // use side 1
839 if (maxc == 2) goto done;
840 if (B2 < B3) goto contact2_2; else goto contact2_3;
841 }
842 }
843 else {
844 if (B3 < B2) {
845 use_side_3: // use side 3
846 BAR(1,2,3);
847 if (maxc == 2) goto done;
848 if (B1 < B2) goto contact2_1; else goto contact2_2;
849 }
850 else {
851 BAR(1,1,2); // use side 2
852 if (maxc == 2) goto done;
853 if (B1 < B3) goto contact2_1; else goto contact2_3;
854 }
855 }
856
857 contact2_1: BAR(2,0,1); goto done;
858 contact2_2: BAR(2,1,2); goto done;
859 contact2_3: BAR(2,2,3); goto done;
860 #undef FOO
861 #undef BAR
862
863 done:
864 for (int i=0; i<ret; i++) {
865 dContactGeom *currContact = CONTACT(contact,i*skip);
866 currContact->g1 = o1;
867 currContact->g2 = o2;
868 currContact->side1 = -1;
869 currContact->side2 = -1;
870 }
871 return ret;
872 }
873