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