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 | |
---|
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 | } |
---|
56 | |
---|
57 | |
---|
58 | void 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 | |
---|
78 | dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz) |
---|
79 | { |
---|
80 | return new dxBox (space,lx,ly,lz); |
---|
81 | } |
---|
82 | |
---|
83 | |
---|
84 | void 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 | |
---|
96 | void 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 | |
---|
106 | dReal 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 | |
---|
184 | static 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 | |
---|
246 | void 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 | |
---|
328 | int 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 | |
---|
699 | int 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 | |
---|
725 | int 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 | } |
---|