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