|
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 * Cylinder-box collider by Alen Ladavac |
|
25 * Ported to ODE by Nguyen Binh |
|
26 */ |
|
27 |
|
28 #include <ode/collision.h> |
|
29 #include <ode/matrix.h> |
|
30 #include <ode/rotation.h> |
|
31 #include <ode/odemath.h> |
|
32 #include "collision_util.h" |
|
33 #include <ode/lookup_tables.h> |
|
34 |
|
35 static const int MAX_CYLBOX_CLIP_POINTS = 16; |
|
36 static const int nCYLINDER_AXIS = 2; |
|
37 // Number of segment of cylinder base circle. |
|
38 // Must be divisible by 4. |
|
39 static const int nCYLINDER_SEGMENT = 8; |
|
40 |
|
41 #define MAX_FLOAT dInfinity |
|
42 |
|
43 // Data that passed through the collider's functions |
|
44 typedef struct _sCylinderBoxData |
|
45 { |
|
46 // cylinder parameters |
|
47 dMatrix3 mCylinderRot; |
|
48 dVector3 vCylinderPos; |
|
49 dVector3 vCylinderAxis; |
|
50 dReal fCylinderRadius; |
|
51 dReal fCylinderSize; |
|
52 dVector3 avCylinderNormals[nCYLINDER_SEGMENT]; |
|
53 |
|
54 // box parameters |
|
55 |
|
56 dMatrix3 mBoxRot; |
|
57 dVector3 vBoxPos; |
|
58 dVector3 vBoxHalfSize; |
|
59 // box vertices array : 8 vertices |
|
60 dVector3 avBoxVertices[8]; |
|
61 |
|
62 // global collider data |
|
63 dVector3 vDiff; |
|
64 dVector3 vNormal; |
|
65 dReal fBestDepth; |
|
66 dReal fBestrb; |
|
67 dReal fBestrc; |
|
68 int iBestAxis; |
|
69 |
|
70 // contact data |
|
71 dVector3 vEp0, vEp1; |
|
72 dReal fDepth0, fDepth1; |
|
73 |
|
74 // ODE stuff |
|
75 dGeomID gBox; |
|
76 dGeomID gCylinder; |
|
77 dContactGeom* gContact; |
|
78 int iFlags; |
|
79 int iSkip; |
|
80 int nContacts; |
|
81 |
|
82 } sCylinderBoxData; |
|
83 |
|
84 |
|
85 // initialize collision data |
|
86 void _cldInitCylinderBox(sCylinderBoxData& cData) |
|
87 { |
|
88 // get cylinder position, orientation |
|
89 const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); |
|
90 dMatrix3Copy(pRotCyc,cData.mCylinderRot); |
|
91 |
|
92 const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder); |
|
93 dVector3Copy(*pPosCyc,cData.vCylinderPos); |
|
94 |
|
95 dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis); |
|
96 |
|
97 // get cylinder radius and size |
|
98 dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize); |
|
99 |
|
100 // get box position, orientation, size |
|
101 const dReal* pRotBox = dGeomGetRotation(cData.gBox); |
|
102 dMatrix3Copy(pRotBox,cData.mBoxRot); |
|
103 const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox); |
|
104 dVector3Copy(*pPosBox,cData.vBoxPos); |
|
105 |
|
106 dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize); |
|
107 cData.vBoxHalfSize[0] = dMUL(cData.vBoxHalfSize[0],REAL(0.5)); |
|
108 cData.vBoxHalfSize[1] = dMUL(cData.vBoxHalfSize[1],REAL(0.5)); |
|
109 cData.vBoxHalfSize[2] = dMUL(cData.vBoxHalfSize[2],REAL(0.5)); |
|
110 |
|
111 // vertex 0 |
|
112 cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0]; |
|
113 cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1]; |
|
114 cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2]; |
|
115 |
|
116 // vertex 1 |
|
117 cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0]; |
|
118 cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1]; |
|
119 cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2]; |
|
120 |
|
121 // vertex 2 |
|
122 cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0]; |
|
123 cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1]; |
|
124 cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2]; |
|
125 |
|
126 // vertex 3 |
|
127 cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0]; |
|
128 cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1]; |
|
129 cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2]; |
|
130 |
|
131 // vertex 4 |
|
132 cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0]; |
|
133 cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1]; |
|
134 cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2]; |
|
135 |
|
136 // vertex 5 |
|
137 cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0]; |
|
138 cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1]; |
|
139 cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2]; |
|
140 |
|
141 // vertex 6 |
|
142 cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0]; |
|
143 cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1]; |
|
144 cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2]; |
|
145 |
|
146 // vertex 7 |
|
147 cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0]; |
|
148 cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1]; |
|
149 cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2]; |
|
150 |
|
151 // temp index |
|
152 int i = 0; |
|
153 dVector3 vTempBoxVertices[8]; |
|
154 // transform vertices in absolute space |
|
155 for(i=0; i < 8; i++) |
|
156 { |
|
157 dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]); |
|
158 dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]); |
|
159 } |
|
160 |
|
161 // find relative position |
|
162 dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff); |
|
163 cData.fBestDepth = MAX_FLOAT; |
|
164 cData.vNormal[0] = REAL(0.0); |
|
165 cData.vNormal[1] = REAL(0.0); |
|
166 cData.vNormal[2] = REAL(0.0); |
|
167 |
|
168 // calculate basic angle for nCYLINDER_SEGMENT-gon |
|
169 dReal fAngle = dPI/nCYLINDER_SEGMENT; |
|
170 |
|
171 // calculate angle increment |
|
172 dReal fAngleIncrement = dMUL(fAngle,REAL(2.0)); |
|
173 |
|
174 // calculate nCYLINDER_SEGMENT-gon points |
|
175 for(i = 0; i < nCYLINDER_SEGMENT; i++) |
|
176 { |
|
177 cData.avCylinderNormals[i][0] = -dCos(fAngle); |
|
178 cData.avCylinderNormals[i][1] = -dSin(fAngle); |
|
179 cData.avCylinderNormals[i][2] = 0; |
|
180 |
|
181 fAngle += fAngleIncrement; |
|
182 } |
|
183 |
|
184 cData.fBestrb = 0; |
|
185 cData.fBestrc = 0; |
|
186 cData.iBestAxis = 0; |
|
187 cData.nContacts = 0; |
|
188 |
|
189 } |
|
190 |
|
191 // test for given separating axis |
|
192 int _cldTestAxis(sCylinderBoxData& cData, dVector3& vInputNormal, int iAxis ) |
|
193 { |
|
194 // check length of input normal |
|
195 dReal fL = dVector3Length(vInputNormal); |
|
196 // if not long enough |
|
197 if ( fL < REAL(2e-5f) ) |
|
198 { |
|
199 // do nothing |
|
200 return 1; |
|
201 } |
|
202 |
|
203 // otherwise make it unit for sure |
|
204 dNormalize3(vInputNormal); |
|
205 |
|
206 // project box and Cylinder on mAxis |
|
207 dReal fdot1 = dVector3Dot(cData.vCylinderAxis, vInputNormal); |
|
208 |
|
209 dReal frc; |
|
210 |
|
211 if (fdot1 > REAL(1.0)) |
|
212 { |
|
213 fdot1 = REAL(1.0); |
|
214 frc = dFabs(dMUL(cData.fCylinderSize,REAL(0.5))); |
|
215 } |
|
216 |
|
217 // project box and capsule on iAxis |
|
218 frc = dFabs( dMUL(fdot1,(cData.fCylinderSize*REAL(0.5)))) + dMUL(cData.fCylinderRadius,dSqrt(REAL(1.0)-(dMUL(fdot1,fdot1)))); |
|
219 |
|
220 dVector3 vTemp1; |
|
221 dReal frb = REAL(0.0); |
|
222 |
|
223 dMat3GetCol(cData.mBoxRot,0,vTemp1); |
|
224 frb = dMUL(dFabs(dVector3Dot(vTemp1,vInputNormal)),cData.vBoxHalfSize[0]); |
|
225 |
|
226 dMat3GetCol(cData.mBoxRot,1,vTemp1); |
|
227 frb += dMUL(dFabs(dVector3Dot(vTemp1,vInputNormal)),cData.vBoxHalfSize[1]); |
|
228 |
|
229 dMat3GetCol(cData.mBoxRot,2,vTemp1); |
|
230 frb += dMUL(dFabs(dVector3Dot(vTemp1,vInputNormal)),cData.vBoxHalfSize[2]); |
|
231 |
|
232 // project their distance on separating axis |
|
233 dReal fd = dVector3Dot(cData.vDiff,vInputNormal); |
|
234 |
|
235 // if they do not overlap exit, we have no intersection |
|
236 if ( dFabs(fd) > frc+frb ) |
|
237 { |
|
238 return 0; |
|
239 } |
|
240 |
|
241 // get depth |
|
242 dReal fDepth = - dFabs(fd) + (frc+frb); |
|
243 |
|
244 // get maximum depth |
|
245 if ( fDepth < cData.fBestDepth ) |
|
246 { |
|
247 cData.fBestDepth = fDepth; |
|
248 dVector3Copy(vInputNormal,cData.vNormal); |
|
249 cData.iBestAxis = iAxis; |
|
250 cData.fBestrb = frb; |
|
251 cData.fBestrc = frc; |
|
252 |
|
253 // flip normal if interval is wrong faced |
|
254 if (fd > 0) |
|
255 { |
|
256 dVector3Inv(cData.vNormal); |
|
257 } |
|
258 } |
|
259 |
|
260 return 1; |
|
261 } |
|
262 |
|
263 |
|
264 // check for separation between box edge and cylinder circle edge |
|
265 int _cldTestEdgeCircleAxis( sCylinderBoxData& cData, |
|
266 const dVector3 &vCenterPoint, |
|
267 const dVector3 &vVx0, const dVector3 &vVx1, |
|
268 int iAxis ) |
|
269 { |
|
270 // calculate direction of edge |
|
271 dVector3 vDirEdge; |
|
272 dVector3Subtract(vVx1,vVx0,vDirEdge); |
|
273 dNormalize3(vDirEdge); |
|
274 // starting point of edge |
|
275 dVector3 vEStart; |
|
276 dVector3Copy(vVx0,vEStart); |
|
277 |
|
278 // calculate angle cosine between cylinder axis and edge |
|
279 dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis); |
|
280 |
|
281 // if edge is perpendicular to cylinder axis |
|
282 if(dFabs(fdot2) < REAL(2e-5f)) |
|
283 { |
|
284 // this can't be separating axis, because edge is parallel to circle plane |
|
285 return 1; |
|
286 } |
|
287 |
|
288 // find point of intersection between edge line and circle plane |
|
289 dVector3 vTemp1; |
|
290 dVector3Subtract(vCenterPoint,vEStart,vTemp1); |
|
291 dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis); |
|
292 dVector3 vpnt; |
|
293 vpnt[0]= vEStart[0] + dMUL(vDirEdge[0],dDIV(fdot1,fdot2)); |
|
294 vpnt[1]= vEStart[1] + dMUL(vDirEdge[1],dDIV(fdot1,fdot2)); |
|
295 vpnt[2]= vEStart[2] + dMUL(vDirEdge[2],dDIV(fdot1,fdot2)); |
|
296 |
|
297 // find tangent vector on circle with same center (vCenterPoint) that |
|
298 // touches point of intersection (vpnt) |
|
299 dVector3 vTangent; |
|
300 dVector3Subtract(vCenterPoint,vpnt,vTemp1); |
|
301 dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent); |
|
302 |
|
303 // find vector orthogonal both to tangent and edge direction |
|
304 dVector3 vAxis; |
|
305 dVector3Cross(vTangent,vDirEdge,vAxis); |
|
306 |
|
307 // use that vector as separating axis |
|
308 return _cldTestAxis( cData, vAxis, iAxis ); |
|
309 } |
|
310 |
|
311 // Test separating axis for collision |
|
312 int _cldTestSeparatingAxes(sCylinderBoxData& cData) |
|
313 { |
|
314 // reset best axis |
|
315 cData.fBestDepth = MAX_FLOAT; |
|
316 cData.iBestAxis = 0; |
|
317 cData.fBestrb = 0; |
|
318 cData.fBestrc = 0; |
|
319 cData.nContacts = 0; |
|
320 |
|
321 dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; |
|
322 |
|
323 // Epsilon value for checking axis vector length |
|
324 const dReal fEpsilon = REAL(2e-5f); |
|
325 |
|
326 // axis A0 |
|
327 dMat3GetCol(cData.mBoxRot, 0 , vAxis); |
|
328 if (!_cldTestAxis( cData, vAxis, 1 )) |
|
329 { |
|
330 return 0; |
|
331 } |
|
332 |
|
333 // axis A1 |
|
334 dMat3GetCol(cData.mBoxRot, 1 , vAxis); |
|
335 if (!_cldTestAxis( cData, vAxis, 2 )) |
|
336 { |
|
337 return 0; |
|
338 } |
|
339 |
|
340 // axis A2 |
|
341 dMat3GetCol(cData.mBoxRot, 2 , vAxis); |
|
342 if (!_cldTestAxis( cData, vAxis, 3 )) |
|
343 { |
|
344 return 0; |
|
345 } |
|
346 |
|
347 // axis C - Cylinder Axis |
|
348 //vAxis = vCylinderAxis; |
|
349 dVector3Copy(cData.vCylinderAxis , vAxis); |
|
350 if (!_cldTestAxis( cData, vAxis, 4 )) |
|
351 { |
|
352 return 0; |
|
353 } |
|
354 |
|
355 // axis CxA0 |
|
356 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 )); |
|
357 dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis); |
|
358 if(dVector3Length2( vAxis ) > fEpsilon ) |
|
359 { |
|
360 if (!_cldTestAxis( cData, vAxis, 5 )) |
|
361 { |
|
362 return 0; |
|
363 } |
|
364 } |
|
365 |
|
366 // axis CxA1 |
|
367 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 )); |
|
368 dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis); |
|
369 if(dVector3Length2( vAxis ) > fEpsilon ) |
|
370 { |
|
371 if (!_cldTestAxis( cData, vAxis, 6 )) |
|
372 { |
|
373 return 0; |
|
374 } |
|
375 } |
|
376 |
|
377 // axis CxA2 |
|
378 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 )); |
|
379 dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis); |
|
380 if(dVector3Length2( vAxis ) > fEpsilon ) |
|
381 { |
|
382 if (!_cldTestAxis( cData, vAxis, 7 )) |
|
383 { |
|
384 return 0; |
|
385 } |
|
386 } |
|
387 |
|
388 int i = 0; |
|
389 dVector3 vTemp1; |
|
390 dVector3 vTemp2; |
|
391 // here we check box's vertices axis |
|
392 for(i=0; i< 8; i++) |
|
393 { |
|
394 //vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos)); |
|
395 dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1); |
|
396 dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2); |
|
397 //vAxis = ( vCylinderAxis cross vAxis ); |
|
398 dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis); |
|
399 if(dVector3Length2( vAxis ) > fEpsilon ) |
|
400 { |
|
401 if (!_cldTestAxis( cData, vAxis, 8 + i )) |
|
402 { |
|
403 return 0; |
|
404 } |
|
405 } |
|
406 } |
|
407 |
|
408 // ************************************ |
|
409 // this is defined for first 12 axes |
|
410 // normal of plane that contains top circle of cylinder |
|
411 // center of top circle of cylinder |
|
412 dVector3 vcc; |
|
413 vcc[0] = (cData.vCylinderPos)[0] + dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
414 vcc[1] = (cData.vCylinderPos)[1] + dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
415 vcc[2] = (cData.vCylinderPos)[2] + dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
416 // ************************************ |
|
417 |
|
418 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16)) |
|
419 { |
|
420 return 0; |
|
421 } |
|
422 |
|
423 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17)) |
|
424 { |
|
425 return 0; |
|
426 } |
|
427 |
|
428 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18)) |
|
429 { |
|
430 return 0; |
|
431 } |
|
432 |
|
433 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19)) |
|
434 { |
|
435 return 0; |
|
436 } |
|
437 |
|
438 |
|
439 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20)) |
|
440 { |
|
441 return 0; |
|
442 } |
|
443 |
|
444 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21)) |
|
445 { |
|
446 return 0; |
|
447 } |
|
448 |
|
449 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22)) |
|
450 { |
|
451 return 0; |
|
452 } |
|
453 |
|
454 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23)) |
|
455 { |
|
456 return 0; |
|
457 } |
|
458 |
|
459 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24)) |
|
460 { |
|
461 return 0; |
|
462 } |
|
463 |
|
464 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25)) |
|
465 { |
|
466 return 0; |
|
467 } |
|
468 |
|
469 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26)) |
|
470 { |
|
471 return 0; |
|
472 } |
|
473 |
|
474 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27)) |
|
475 { |
|
476 return 0; |
|
477 } |
|
478 |
|
479 // ************************************ |
|
480 // this is defined for second 12 axes |
|
481 // normal of plane that contains bottom circle of cylinder |
|
482 // center of bottom circle of cylinder |
|
483 // vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5)); |
|
484 vcc[0] = (cData.vCylinderPos)[0] - dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
485 vcc[1] = (cData.vCylinderPos)[1] - dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
486 vcc[2] = (cData.vCylinderPos)[2] - dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
487 // ************************************ |
|
488 |
|
489 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28)) |
|
490 { |
|
491 return 0; |
|
492 } |
|
493 |
|
494 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29)) |
|
495 { |
|
496 return 0; |
|
497 } |
|
498 |
|
499 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30)) |
|
500 { |
|
501 return 0; |
|
502 } |
|
503 |
|
504 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31)) |
|
505 { |
|
506 return 0; |
|
507 } |
|
508 |
|
509 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32)) |
|
510 { |
|
511 return 0; |
|
512 } |
|
513 |
|
514 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33)) |
|
515 { |
|
516 return 0; |
|
517 } |
|
518 |
|
519 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34)) |
|
520 { |
|
521 return 0; |
|
522 } |
|
523 |
|
524 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35)) |
|
525 { |
|
526 return 0; |
|
527 } |
|
528 |
|
529 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36)) |
|
530 { |
|
531 return 0; |
|
532 } |
|
533 |
|
534 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 37)) |
|
535 { |
|
536 return 0; |
|
537 } |
|
538 |
|
539 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 38)) |
|
540 { |
|
541 return 0; |
|
542 } |
|
543 |
|
544 if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 39)) |
|
545 { |
|
546 return 0; |
|
547 } |
|
548 |
|
549 return 1; |
|
550 } |
|
551 |
|
552 int _cldClipCylinderToBox(sCylinderBoxData& cData) |
|
553 { |
|
554 |
|
555 // calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal |
|
556 dVector3 vN; |
|
557 dReal fTemp1 = dVector3Dot(cData.vCylinderAxis,cData.vNormal); |
|
558 vN[0] = cData.vNormal[0] - dMUL(cData.vCylinderAxis[0],fTemp1); |
|
559 vN[1] = cData.vNormal[1] - dMUL(cData.vCylinderAxis[1],fTemp1); |
|
560 vN[2] = cData.vNormal[2] - dMUL(cData.vCylinderAxis[2],fTemp1); |
|
561 |
|
562 // normalize that vector |
|
563 dNormalize3(vN); |
|
564 |
|
565 // translate cylinder end points by the vector |
|
566 dVector3 vCposTrans; |
|
567 vCposTrans[0] = cData.vCylinderPos[0] + dMUL(vN[0],cData.fCylinderRadius); |
|
568 vCposTrans[1] = cData.vCylinderPos[1] + dMUL(vN[1],cData.fCylinderRadius); |
|
569 vCposTrans[2] = cData.vCylinderPos[2] + dMUL(vN[2],cData.fCylinderRadius); |
|
570 |
|
571 cData.vEp0[0] = vCposTrans[0] + dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
572 cData.vEp0[1] = vCposTrans[1] + dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
573 cData.vEp0[2] = vCposTrans[2] + dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
574 |
|
575 cData.vEp1[0] = vCposTrans[0] - dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
576 cData.vEp1[1] = vCposTrans[1] - dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
577 cData.vEp1[2] = vCposTrans[2] - dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
578 |
|
579 // transform edge points in box space |
|
580 cData.vEp0[0] -= cData.vBoxPos[0]; |
|
581 cData.vEp0[1] -= cData.vBoxPos[1]; |
|
582 cData.vEp0[2] -= cData.vBoxPos[2]; |
|
583 |
|
584 cData.vEp1[0] -= cData.vBoxPos[0]; |
|
585 cData.vEp1[1] -= cData.vBoxPos[1]; |
|
586 cData.vEp1[2] -= cData.vBoxPos[2]; |
|
587 |
|
588 dVector3 vTemp1; |
|
589 // clip the edge to box |
|
590 dVector4 plPlane; |
|
591 // plane 0 +x |
|
592 dMat3GetCol(cData.mBoxRot,0,vTemp1); |
|
593 dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane); |
|
594 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) |
|
595 { |
|
596 return 0; |
|
597 } |
|
598 |
|
599 // plane 1 +y |
|
600 dMat3GetCol(cData.mBoxRot,1,vTemp1); |
|
601 dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane); |
|
602 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) |
|
603 { |
|
604 return 0; |
|
605 } |
|
606 |
|
607 // plane 2 +z |
|
608 dMat3GetCol(cData.mBoxRot,2,vTemp1); |
|
609 dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane); |
|
610 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) |
|
611 { |
|
612 return 0; |
|
613 } |
|
614 |
|
615 // plane 3 -x |
|
616 dMat3GetCol(cData.mBoxRot,0,vTemp1); |
|
617 dVector3Inv(vTemp1); |
|
618 dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane); |
|
619 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) |
|
620 { |
|
621 return 0; |
|
622 } |
|
623 |
|
624 // plane 4 -y |
|
625 dMat3GetCol(cData.mBoxRot,1,vTemp1); |
|
626 dVector3Inv(vTemp1); |
|
627 dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane); |
|
628 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) |
|
629 { |
|
630 return 0; |
|
631 } |
|
632 |
|
633 // plane 5 -z |
|
634 dMat3GetCol(cData.mBoxRot,2,vTemp1); |
|
635 dVector3Inv(vTemp1); |
|
636 dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane); |
|
637 if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) |
|
638 { |
|
639 return 0; |
|
640 } |
|
641 |
|
642 // calculate depths for both contact points |
|
643 cData.fDepth0 = cData.fBestrb + dVector3Dot(cData.vEp0, cData.vNormal); |
|
644 cData.fDepth1 = cData.fBestrb + dVector3Dot(cData.vEp1, cData.vNormal); |
|
645 |
|
646 // clamp depths to 0 |
|
647 if(cData.fDepth0<0) |
|
648 { |
|
649 cData.fDepth0 = REAL(0.0); |
|
650 } |
|
651 |
|
652 if(cData.fDepth1<0) |
|
653 { |
|
654 cData.fDepth1 = REAL(0.0); |
|
655 } |
|
656 |
|
657 // back transform edge points from box to absolute space |
|
658 cData.vEp0[0] += cData.vBoxPos[0]; |
|
659 cData.vEp0[1] += cData.vBoxPos[1]; |
|
660 cData.vEp0[2] += cData.vBoxPos[2]; |
|
661 |
|
662 cData.vEp1[0] += cData.vBoxPos[0]; |
|
663 cData.vEp1[1] += cData.vBoxPos[1]; |
|
664 cData.vEp1[2] += cData.vBoxPos[2]; |
|
665 |
|
666 dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); |
|
667 Contact0->depth = cData.fDepth0; |
|
668 dVector3Copy(cData.vNormal,Contact0->normal); |
|
669 dVector3Copy(cData.vEp0,Contact0->pos); |
|
670 Contact0->g1 = cData.gCylinder; |
|
671 Contact0->g2 = cData.gBox; |
|
672 dVector3Inv(Contact0->normal); |
|
673 cData.nContacts++; |
|
674 |
|
675 dContactGeom* Contact1 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); |
|
676 Contact1->depth = cData.fDepth1; |
|
677 dVector3Copy(cData.vNormal,Contact1->normal); |
|
678 dVector3Copy(cData.vEp1,Contact1->pos); |
|
679 Contact1->g1 = cData.gCylinder; |
|
680 Contact1->g2 = cData.gBox; |
|
681 dVector3Inv(Contact1->normal); |
|
682 cData.nContacts++; |
|
683 |
|
684 return 1; |
|
685 } |
|
686 |
|
687 |
|
688 void _cldClipBoxToCylinder(sCylinderBoxData& cData ) |
|
689 { |
|
690 dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; |
|
691 // check which circle from cylinder we take for clipping |
|
692 if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) ) |
|
693 { |
|
694 // get top circle |
|
695 vCylinderCirclePos[0] = cData.vCylinderPos[0] + dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
696 vCylinderCirclePos[1] = cData.vCylinderPos[1] + dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
697 vCylinderCirclePos[2] = cData.vCylinderPos[2] + dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
698 |
|
699 vCylinderCircleNormal_Rel[0] = REAL(0.0); |
|
700 vCylinderCircleNormal_Rel[1] = REAL(0.0); |
|
701 vCylinderCircleNormal_Rel[2] = REAL(0.0); |
|
702 vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); |
|
703 } |
|
704 else |
|
705 { |
|
706 // get bottom circle |
|
707 vCylinderCirclePos[0] = cData.vCylinderPos[0] - dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
708 vCylinderCirclePos[1] = cData.vCylinderPos[1] - dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
709 vCylinderCirclePos[2] = cData.vCylinderPos[2] - dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5))); |
|
710 |
|
711 vCylinderCircleNormal_Rel[0] = REAL(0.0); |
|
712 vCylinderCircleNormal_Rel[1] = REAL(0.0); |
|
713 vCylinderCircleNormal_Rel[2] = REAL(0.0); |
|
714 vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); |
|
715 } |
|
716 |
|
717 // vNr is normal in Box frame, pointing from Cylinder to Box |
|
718 dVector3 vNr; |
|
719 dMatrix3 mBoxInv; |
|
720 |
|
721 // Find a way to use quaternion |
|
722 dMatrix3Inv(cData.mBoxRot,mBoxInv); |
|
723 dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr); |
|
724 |
|
725 dVector3 vAbsNormal; |
|
726 |
|
727 vAbsNormal[0] = dFabs( vNr[0] ); |
|
728 vAbsNormal[1] = dFabs( vNr[1] ); |
|
729 vAbsNormal[2] = dFabs( vNr[2] ); |
|
730 |
|
731 // find which face in box is closest to cylinder |
|
732 int iB0, iB1, iB2; |
|
733 |
|
734 // Different from Croteam's code |
|
735 if (vAbsNormal[1] > vAbsNormal[0]) |
|
736 { |
|
737 // 1 > 0 |
|
738 if (vAbsNormal[0]> vAbsNormal[2]) |
|
739 { |
|
740 // 0 > 2 -> 1 > 0 >2 |
|
741 iB0 = 1; iB1 = 0; iB2 = 2; |
|
742 } |
|
743 else |
|
744 { |
|
745 // 2 > 0-> Must compare 1 and 2 |
|
746 if (vAbsNormal[1] > vAbsNormal[2]) |
|
747 { |
|
748 // 1 > 2 -> 1 > 2 > 0 |
|
749 iB0 = 1; iB1 = 2; iB2 = 0; |
|
750 } |
|
751 else |
|
752 { |
|
753 // 2 > 1 -> 2 > 1 > 0; |
|
754 iB0 = 2; iB1 = 1; iB2 = 0; |
|
755 } |
|
756 } |
|
757 } |
|
758 else |
|
759 { |
|
760 // 0 > 1 |
|
761 if (vAbsNormal[1] > vAbsNormal[2]) |
|
762 { |
|
763 // 1 > 2 -> 0 > 1 > 2 |
|
764 iB0 = 0; iB1 = 1; iB2 = 2; |
|
765 } |
|
766 else |
|
767 { |
|
768 // 2 > 1 -> Must compare 0 and 2 |
|
769 if (vAbsNormal[0] > vAbsNormal[2]) |
|
770 { |
|
771 // 0 > 2 -> 0 > 2 > 1; |
|
772 iB0 = 0; iB1 = 2; iB2 = 1; |
|
773 } |
|
774 else |
|
775 { |
|
776 // 2 > 0 -> 2 > 0 > 1; |
|
777 iB0 = 2; iB1 = 0; iB2 = 1; |
|
778 } |
|
779 } |
|
780 } |
|
781 |
|
782 dVector3 vCenter; |
|
783 // find center of box polygon |
|
784 dVector3 vTemp; |
|
785 if (vNr[iB0] > 0) |
|
786 { |
|
787 dMat3GetCol(cData.mBoxRot,iB0,vTemp); |
|
788 vCenter[0] = cData.vBoxPos[0] - dMUL(cData.vBoxHalfSize[iB0],vTemp[0]); |
|
789 vCenter[1] = cData.vBoxPos[1] - dMUL(cData.vBoxHalfSize[iB0],vTemp[1]); |
|
790 vCenter[2] = cData.vBoxPos[2] - dMUL(cData.vBoxHalfSize[iB0],vTemp[2]); |
|
791 } |
|
792 else |
|
793 { |
|
794 dMat3GetCol(cData.mBoxRot,iB0,vTemp); |
|
795 vCenter[0] = cData.vBoxPos[0] + dMUL(cData.vBoxHalfSize[iB0],vTemp[0]); |
|
796 vCenter[1] = cData.vBoxPos[1] + dMUL(cData.vBoxHalfSize[iB0],vTemp[1]); |
|
797 vCenter[2] = cData.vBoxPos[2] + dMUL(cData.vBoxHalfSize[iB0],vTemp[2]); |
|
798 } |
|
799 |
|
800 // find the vertices of box polygon |
|
801 dVector3 avPoints[4]; |
|
802 dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS]; |
|
803 dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS]; |
|
804 |
|
805 int i=0; |
|
806 for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++) |
|
807 { |
|
808 avTempArray1[i][0] = REAL(0.0); |
|
809 avTempArray1[i][1] = REAL(0.0); |
|
810 avTempArray1[i][2] = REAL(0.0); |
|
811 |
|
812 avTempArray2[i][0] = REAL(0.0); |
|
813 avTempArray2[i][1] = REAL(0.0); |
|
814 avTempArray2[i][2] = REAL(0.0); |
|
815 } |
|
816 |
|
817 dVector3 vAxis1, vAxis2; |
|
818 |
|
819 dMat3GetCol(cData.mBoxRot,iB1,vAxis1); |
|
820 dMat3GetCol(cData.mBoxRot,iB2,vAxis2); |
|
821 |
|
822 avPoints[0][0] = vCenter[0] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[0]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[0]); |
|
823 avPoints[0][1] = vCenter[1] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[1]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[1]); |
|
824 avPoints[0][2] = vCenter[2] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[2]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[2]); |
|
825 |
|
826 avPoints[1][0] = vCenter[0] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[0]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[0]); |
|
827 avPoints[1][1] = vCenter[1] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[1]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[1]); |
|
828 avPoints[1][2] = vCenter[2] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[2]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[2]); |
|
829 |
|
830 avPoints[2][0] = vCenter[0] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[0]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[0]); |
|
831 avPoints[2][1] = vCenter[1] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[1]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[1]); |
|
832 avPoints[2][2] = vCenter[2] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[2]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[2]); |
|
833 |
|
834 avPoints[3][0] = vCenter[0] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[0]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[0]); |
|
835 avPoints[3][1] = vCenter[1] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[1]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[1]); |
|
836 avPoints[3][2] = vCenter[2] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[2]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[2]); |
|
837 |
|
838 // transform box points to space of cylinder circle |
|
839 dMatrix3 mCylinderInv; |
|
840 dMatrix3Inv(cData.mCylinderRot,mCylinderInv); |
|
841 |
|
842 for(i=0; i<4; i++) |
|
843 { |
|
844 dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp); |
|
845 dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]); |
|
846 } |
|
847 |
|
848 int iTmpCounter1 = 0; |
|
849 int iTmpCounter2 = 0; |
|
850 dVector4 plPlane; |
|
851 |
|
852 // plane of cylinder that contains circle for intersection |
|
853 dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); |
|
854 dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane); |
|
855 |
|
856 |
|
857 // Body of base circle of Cylinder |
|
858 int nCircleSegment = 0; |
|
859 for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++) |
|
860 { |
|
861 dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane); |
|
862 |
|
863 if (0 == (nCircleSegment % 2)) |
|
864 { |
|
865 dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); |
|
866 } |
|
867 else |
|
868 { |
|
869 dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); |
|
870 } |
|
871 |
|
872 } |
|
873 |
|
874 // back transform clipped points to absolute space |
|
875 dReal ftmpdot; |
|
876 dReal fTempDepth; |
|
877 dVector3 vPoint; |
|
878 |
|
879 if (nCircleSegment %2) |
|
880 { |
|
881 for( i=0; i<iTmpCounter2; i++) |
|
882 { |
|
883 dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray2[i]); |
|
884 vPoint[0] += vCylinderCirclePos[0]; |
|
885 vPoint[1] += vCylinderCirclePos[1]; |
|
886 vPoint[2] += vCylinderCirclePos[2]; |
|
887 |
|
888 dVector3Subtract(vPoint,cData.vCylinderPos,vTemp); |
|
889 ftmpdot = dVector3Dot(vTemp, cData.vNormal); |
|
890 fTempDepth = cData.fBestrc - ftmpdot; |
|
891 // Depth must be positive |
|
892 if (fTempDepth > REAL(0.0)) |
|
893 { |
|
894 // generate contacts |
|
895 dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); |
|
896 Contact0->depth = fTempDepth; |
|
897 dVector3Copy(cData.vNormal,Contact0->normal); |
|
898 dVector3Copy(vPoint,Contact0->pos); |
|
899 Contact0->g1 = cData.gCylinder; |
|
900 Contact0->g2 = cData.gBox; |
|
901 dVector3Inv(Contact0->normal); |
|
902 cData.nContacts++; |
|
903 } |
|
904 } |
|
905 } |
|
906 else |
|
907 { |
|
908 for( i=0; i<iTmpCounter1; i++) |
|
909 { |
|
910 dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray1[i]); |
|
911 vPoint[0] += vCylinderCirclePos[0]; |
|
912 vPoint[1] += vCylinderCirclePos[1]; |
|
913 vPoint[2] += vCylinderCirclePos[2]; |
|
914 |
|
915 dVector3Subtract(vPoint,cData.vCylinderPos,vTemp); |
|
916 ftmpdot = dVector3Dot(vTemp, cData.vNormal); |
|
917 fTempDepth = cData.fBestrc - ftmpdot; |
|
918 // Depth must be positive |
|
919 if (fTempDepth > REAL(0.0)) |
|
920 { |
|
921 // generate contacts |
|
922 dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); |
|
923 Contact0->depth = fTempDepth; |
|
924 dVector3Copy(cData.vNormal,Contact0->normal); |
|
925 dVector3Copy(vPoint,Contact0->pos); |
|
926 Contact0->g1 = cData.gCylinder; |
|
927 Contact0->g2 = cData.gBox; |
|
928 dVector3Inv(Contact0->normal); |
|
929 cData.nContacts++; |
|
930 } |
|
931 } |
|
932 } |
|
933 } |
|
934 |
|
935 |
|
936 // Cylinder - Box by CroTeam |
|
937 // Ported by Nguyen Binh |
|
938 int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) |
|
939 { |
|
940 sCylinderBoxData cData; |
|
941 |
|
942 // Assign ODE stuff |
|
943 cData.gCylinder = o1; |
|
944 cData.gBox = o2; |
|
945 cData.iFlags = flags; |
|
946 cData.iSkip = skip; |
|
947 cData.gContact = contact; |
|
948 |
|
949 // initialize collider |
|
950 _cldInitCylinderBox( cData ); |
|
951 |
|
952 // do intersection test and find best separating axis |
|
953 if(!_cldTestSeparatingAxes( cData ) ) |
|
954 { |
|
955 // if not found do nothing |
|
956 return 0; |
|
957 } |
|
958 |
|
959 // if best separation axis is not found |
|
960 if ( cData.iBestAxis == 0 ) |
|
961 { |
|
962 // this should not happen (we should already exit in that case) |
|
963 |
|
964 // do nothing |
|
965 return 0; |
|
966 } |
|
967 |
|
968 dReal fdot = dVector3Dot(cData.vNormal,cData.vCylinderAxis); |
|
969 // choose which clipping method are we going to apply |
|
970 if (dFabs(fdot) < REAL(0.9) ) |
|
971 { |
|
972 // clip cylinder over box |
|
973 if(!_cldClipCylinderToBox(cData)) |
|
974 { |
|
975 return 0; |
|
976 } |
|
977 } |
|
978 else |
|
979 { |
|
980 _cldClipBoxToCylinder(cData); |
|
981 } |
|
982 |
|
983 return cData.nContacts; |
|
984 } |
|
985 |