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