aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:15:33 +0000
committerdan miller2007-10-19 05:15:33 +0000
commit79eca25c945a535a7a0325999034bae17da92412 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp
parentadding ode source to /libraries (diff)
downloadopensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.zip
opensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.tar.gz
opensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.tar.bz2
opensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.tar.xz
resubmitting ode
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp1145
1 files changed, 1145 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp b/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp
new file mode 100644
index 0000000..342be04
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_cylinder_trimesh.cpp
@@ -0,0 +1,1145 @@
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-trimesh collider by Alen Ladavac
25 * Ported to ODE by Nguyen Binh
26 */
27
28
29#include <ode/collision.h>
30#include <ode/matrix.h>
31#include <ode/rotation.h>
32#include <ode/odemath.h>
33#include "collision_util.h"
34
35#define TRIMESH_INTERNAL
36#include "collision_trimesh_internal.h"
37
38#define MAX_REAL dInfinity
39static const int nCYLINDER_AXIS = 2;
40static const int nCYLINDER_CIRCLE_SEGMENTS = 8;
41static const int nMAX_CYLINDER_TRIANGLE_CLIP_POINTS = 12;
42
43#define OPTIMIZE_CONTACTS 1
44
45// Local contacts data
46typedef struct _sLocalContactData
47{
48 dVector3 vPos;
49 dVector3 vNormal;
50 dReal fDepth;
51 int triIndex;
52 int nFlags; // 0 = filtered out, 1 = OK
53}sLocalContactData;
54
55typedef struct _sCylinderTrimeshColliderData
56{
57 // cylinder data
58 dMatrix3 mCylinderRot;
59 dQuaternion qCylinderRot;
60 dQuaternion qInvCylinderRot;
61 dVector3 vCylinderPos;
62 dVector3 vCylinderAxis;
63 dReal fCylinderRadius;
64 dReal fCylinderSize;
65 dVector3 avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS];
66
67 // mesh data
68 dQuaternion qTrimeshRot;
69 dQuaternion qInvTrimeshRot;
70 dMatrix3 mTrimeshRot;
71 dVector3 vTrimeshPos;
72
73 // global collider data
74 dVector3 vBestPoint;
75 dReal fBestDepth;
76 dReal fBestCenter;
77 dReal fBestrt;
78 int iBestAxis;
79 dVector3 vContactNormal;
80 dVector3 vNormal;
81 dVector3 vE0;
82 dVector3 vE1;
83 dVector3 vE2;
84
85 // ODE stuff
86 dGeomID gCylinder;
87 dxTriMesh* gTrimesh;
88 dContactGeom* gContact;
89 int iFlags;
90 int iSkip;
91 int nContacts;// = 0;
92 sLocalContactData* gLocalContacts;
93} sCylinderTrimeshColliderData;
94
95// Short type name
96typedef sCylinderTrimeshColliderData sData;
97
98// Use to classify contacts to be "near" in position
99static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4
100// Use to classify contacts to be "near" in normal direction
101static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4
102
103// If this two contact can be classified as "near"
104inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2)
105{
106 int bPosNear = 0;
107 int bSameDir = 0;
108 dVector3 vDiff;
109
110 // First check if they are "near" in position
111 dVector3Subtract(c1.vPos,c2.vPos,vDiff);
112 if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon)
113 &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon)
114 &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon))
115 {
116 bPosNear = 1;
117 }
118
119 // Second check if they are "near" in normal direction
120 dVector3Subtract(c1.vNormal,c2.vNormal,vDiff);
121 if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon)
122 &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon)
123 &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) )
124 {
125 bSameDir = 1;
126 }
127
128 // Will be "near" if position and normal direction are "near"
129 return (bPosNear && bSameDir);
130}
131
132inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2)
133{
134 // The not better will be throw away
135 // You can change the selection criteria here
136 return (c1.fDepth > c2.fDepth);
137}
138
139// iterate through gLocalContacts and filtered out "near contact"
140inline void _OptimizeLocalContacts(sData& cData)
141{
142 int nContacts = cData.nContacts;
143
144 for (int i = 0; i < nContacts-1; i++)
145 {
146 for (int j = i+1; j < nContacts; j++)
147 {
148 if (_IsNearContacts(cData.gLocalContacts[i],cData.gLocalContacts[j]))
149 {
150 // If they are seem to be the same then filtered
151 // out the least penetrate one
152 if (_IsBetter(cData.gLocalContacts[j],cData.gLocalContacts[i]))
153 {
154 cData.gLocalContacts[i].nFlags = 0; // filtered 1st contact
155 }
156 else
157 {
158 cData.gLocalContacts[j].nFlags = 0; // filtered 2nd contact
159 }
160
161 // NOTE
162 // There is other way is to add two depth together but
163 // it not work so well. Why???
164 }
165 }
166 }
167}
168
169inline int _ProcessLocalContacts(sData& cData)
170{
171 if (cData.nContacts == 0)
172 {
173 return 0;
174 }
175
176#ifdef OPTIMIZE_CONTACTS
177 if (cData.nContacts > 1 && !(cData.iFlags & CONTACTS_UNIMPORTANT))
178 {
179 // Can be optimized...
180 _OptimizeLocalContacts(cData);
181 }
182#endif
183
184 int iContact = 0;
185 dContactGeom* Contact = 0;
186
187 int nFinalContact = 0;
188
189 for (iContact = 0; iContact < cData.nContacts; iContact ++)
190 {
191 if (1 == cData.gLocalContacts[iContact].nFlags)
192 {
193 Contact = SAFECONTACT(cData.iFlags, cData.gContact, nFinalContact, cData.iSkip);
194 Contact->depth = cData.gLocalContacts[iContact].fDepth;
195 dVector3Copy(cData.gLocalContacts[iContact].vNormal,Contact->normal);
196 dVector3Copy(cData.gLocalContacts[iContact].vPos,Contact->pos);
197 Contact->g1 = cData.gCylinder;
198 Contact->g2 = cData.gTrimesh;
199 Contact->side2 = cData.gLocalContacts[iContact].triIndex;
200 dVector3Inv(Contact->normal);
201
202 nFinalContact++;
203 }
204 }
205 // debug
206 //if (nFinalContact != cData.nContacts)
207 //{
208 // printf("[Info] %d contacts generated,%d filtered.\n",cData.nContacts,cData.nContacts-nFinalContact);
209 //}
210
211 return nFinalContact;
212}
213
214
215bool _cldTestAxis(sData& cData,
216 const dVector3 &v0,
217 const dVector3 &v1,
218 const dVector3 &v2,
219 dVector3& vAxis,
220 int iAxis,
221 bool bNoFlip = false)
222{
223
224 // calculate length of separating axis vector
225 dReal fL = dVector3Length(vAxis);
226 // if not long enough
227 if ( fL < REAL(1e-5) )
228 {
229 // do nothing
230 return true;
231 }
232
233 // otherwise normalize it
234 vAxis[0] /= fL;
235 vAxis[1] /= fL;
236 vAxis[2] /= fL;
237
238 dReal fdot1 = dVector3Dot(cData.vCylinderAxis,vAxis);
239 // project capsule on vAxis
240 dReal frc;
241
242 if (dFabs(fdot1) > REAL(1.0) )
243 {
244// fdot1 = REAL(1.0);
245 frc = dFabs(cData.fCylinderSize* REAL(0.5));
246 }
247 else
248 {
249 frc = dFabs((cData.fCylinderSize* REAL(0.5)) * fdot1)
250 + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1));
251 }
252
253 dVector3 vV0;
254 dVector3Subtract(v0,cData.vCylinderPos,vV0);
255 dVector3 vV1;
256 dVector3Subtract(v1,cData.vCylinderPos,vV1);
257 dVector3 vV2;
258 dVector3Subtract(v2,cData.vCylinderPos,vV2);
259
260 // project triangle on vAxis
261 dReal afv[3];
262 afv[0] = dVector3Dot( vV0 , vAxis );
263 afv[1] = dVector3Dot( vV1 , vAxis );
264 afv[2] = dVector3Dot( vV2 , vAxis );
265
266 dReal fMin = MAX_REAL;
267 dReal fMax = -MAX_REAL;
268
269 // for each vertex
270 for(int i = 0; i < 3; i++)
271 {
272 // find minimum
273 if (afv[i]<fMin)
274 {
275 fMin = afv[i];
276 }
277 // find maximum
278 if (afv[i]>fMax)
279 {
280 fMax = afv[i];
281 }
282 }
283
284 // find capsule's center of interval on axis
285 dReal fCenter = (fMin+fMax)* REAL(0.5);
286 // calculate triangles halfinterval
287 dReal fTriangleRadius = (fMax-fMin)*REAL(0.5);
288
289 // if they do not overlap,
290 if( dFabs(fCenter) > (frc+fTriangleRadius) )
291 {
292 // exit, we have no intersection
293 return false;
294 }
295
296 // calculate depth
297 dReal fDepth = -(dFabs(fCenter) - (frc + fTriangleRadius ) );
298
299 // if greater then best found so far
300 if ( fDepth < cData.fBestDepth )
301 {
302 // remember depth
303 cData.fBestDepth = fDepth;
304 cData.fBestCenter = fCenter;
305 cData.fBestrt = frc;
306 dVector3Copy(vAxis,cData.vContactNormal);
307 cData.iBestAxis = iAxis;
308
309 // flip normal if interval is wrong faced
310 if ( fCenter< REAL(0.0) && !bNoFlip)
311 {
312 dVector3Inv(cData.vContactNormal);
313 cData.fBestCenter = -fCenter;
314 }
315 }
316
317 return true;
318}
319
320// intersection test between edge and circle
321bool _cldTestCircleToEdgeAxis(sData& cData,
322 const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
323 const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1,
324 const dVector3 &vVx0, const dVector3 &vVx1, int iAxis)
325{
326 // calculate direction of edge
327 dVector3 vkl;
328 dVector3Subtract( vVx1 , vVx0 , vkl);
329 dNormalize3(vkl);
330 // starting point of edge
331 dVector3 vol;
332 dVector3Copy(vVx0,vol);
333
334 // calculate angle cosine between cylinder axis and edge
335 dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1);
336
337 // if edge is perpendicular to cylinder axis
338 if(dFabs(fdot2)<REAL(1e-5))
339 {
340 // this can't be separating axis, because edge is parallel to circle plane
341 return true;
342 }
343
344 // find point of intersection between edge line and circle plane
345 dVector3 vTemp;
346 dVector3Subtract(vCenterPoint,vol,vTemp);
347 dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1);
348 dVector3 vpnt;// = vol + vkl * (fdot1/fdot2);
349 vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2;
350 vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2;
351 vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2;
352
353 // find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt)
354 dVector3 vTangent;
355 dVector3Subtract(vCenterPoint,vpnt,vTemp);
356 dVector3Cross(vTemp,vCylinderAxis1,vTangent);
357
358 // find vector orthogonal both to tangent and edge direction
359 dVector3 vAxis;
360 dVector3Cross(vTangent,vkl,vAxis);
361
362 // use that vector as separating axis
363 return _cldTestAxis( cData ,v0, v1, v2, vAxis, iAxis );
364}
365
366// helper for less key strokes
367// r = ( (v1 - v2) cross v3 ) cross v3
368inline void _CalculateAxis(const dVector3& v1,
369 const dVector3& v2,
370 const dVector3& v3,
371 dVector3& r)
372{
373 dVector3 t1;
374 dVector3 t2;
375
376 dVector3Subtract(v1,v2,t1);
377 dVector3Cross(t1,v3,t2);
378 dVector3Cross(t2,v3,r);
379}
380
381bool _cldTestSeparatingAxes(sData& cData,
382 const dVector3 &v0,
383 const dVector3 &v1,
384 const dVector3 &v2)
385{
386
387 // calculate edge vectors
388 dVector3Subtract(v1 ,v0 , cData.vE0);
389 // cData.vE1 has been calculated before -> so save some cycles here
390 dVector3Subtract(v0 ,v2 , cData.vE2);
391
392 // calculate caps centers in absolute space
393 dVector3 vCp0;
394 vCp0[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5));
395 vCp0[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5));
396 vCp0[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5));
397
398 dVector3 vCp1;
399 vCp1[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5));
400 vCp1[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5));
401 vCp1[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5));
402
403 // reset best axis
404 cData.iBestAxis = 0;
405 dVector3 vAxis;
406
407 // axis cData.vNormal
408 //vAxis = -cData.vNormal;
409 vAxis[0] = -cData.vNormal[0];
410 vAxis[1] = -cData.vNormal[1];
411 vAxis[2] = -cData.vNormal[2];
412 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 1, true))
413 {
414 return false;
415 }
416
417 // axis CxE0
418 // vAxis = ( cData.vCylinderAxis cross cData.vE0 );
419 dVector3Cross(cData.vCylinderAxis, cData.vE0,vAxis);
420 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 2))
421 {
422 return false;
423 }
424
425 // axis CxE1
426 // vAxis = ( cData.vCylinderAxis cross cData.vE1 );
427 dVector3Cross(cData.vCylinderAxis, cData.vE1,vAxis);
428 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 3))
429 {
430 return false;
431 }
432
433 // axis CxE2
434 // vAxis = ( cData.vCylinderAxis cross cData.vE2 );
435 dVector3Cross(cData.vCylinderAxis, cData.vE2,vAxis);
436 if (!_cldTestAxis( cData ,v0, v1, v2, vAxis, 4))
437 {
438 return false;
439 }
440
441 // first vertex on triangle
442 // axis ((V0-Cp0) x C) x C
443 //vAxis = ( ( v0-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
444 _CalculateAxis(v0 , vCp0 , cData.vCylinderAxis , vAxis);
445 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 11))
446 {
447 return false;
448 }
449
450 // second vertex on triangle
451 // axis ((V1-Cp0) x C) x C
452 // vAxis = ( ( v1-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
453 _CalculateAxis(v1 , vCp0 , cData.vCylinderAxis , vAxis);
454 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 12))
455 {
456 return false;
457 }
458
459 // third vertex on triangle
460 // axis ((V2-Cp0) x C) x C
461 //vAxis = ( ( v2-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis;
462 _CalculateAxis(v2 , vCp0 , cData.vCylinderAxis , vAxis);
463 if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 13))
464 {
465 return false;
466 }
467
468 // test cylinder axis
469 // vAxis = cData.vCylinderAxis;
470 dVector3Copy(cData.vCylinderAxis , vAxis);
471 if (!_cldTestAxis(cData , v0, v1, v2, vAxis, 14))
472 {
473 return false;
474 }
475
476 // Test top and bottom circle ring of cylinder for separation
477 dVector3 vccATop;
478 vccATop[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5));
479 vccATop[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5));
480 vccATop[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5));
481
482 dVector3 vccABottom;
483 vccABottom[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5));
484 vccABottom[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5));
485 vccABottom[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5));
486
487
488 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v1, 15))
489 {
490 return false;
491 }
492
493 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v1, v2, 16))
494 {
495 return false;
496 }
497
498 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v2, 17))
499 {
500 return false;
501 }
502
503 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v1, 18))
504 {
505 return false;
506 }
507
508 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v1, v2, 19))
509 {
510 return false;
511 }
512
513 if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v2, 20))
514 {
515 return false;
516 }
517
518 return true;
519}
520
521bool _cldClipCylinderEdgeToTriangle(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)
522{
523 // translate cylinder
524 dReal fTemp = dVector3Dot(cData.vCylinderAxis , cData.vContactNormal);
525 dVector3 vN2;
526 vN2[0] = cData.vContactNormal[0] - cData.vCylinderAxis[0]*fTemp;
527 vN2[1] = cData.vContactNormal[1] - cData.vCylinderAxis[1]*fTemp;
528 vN2[2] = cData.vContactNormal[2] - cData.vCylinderAxis[2]*fTemp;
529
530 fTemp = dVector3Length(vN2);
531 if (fTemp < REAL(1e-5))
532 {
533 return false;
534 }
535
536 // Normalize it
537 vN2[0] /= fTemp;
538 vN2[1] /= fTemp;
539 vN2[2] /= fTemp;
540
541 // calculate caps centers in absolute space
542 dVector3 vCposTrans;
543 vCposTrans[0] = cData.vCylinderPos[0] + vN2[0]*cData.fCylinderRadius;
544 vCposTrans[1] = cData.vCylinderPos[1] + vN2[1]*cData.fCylinderRadius;
545 vCposTrans[2] = cData.vCylinderPos[2] + vN2[2]*cData.fCylinderRadius;
546
547 dVector3 vCEdgePoint0;
548 vCEdgePoint0[0] = vCposTrans[0] + cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5));
549 vCEdgePoint0[1] = vCposTrans[1] + cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5));
550 vCEdgePoint0[2] = vCposTrans[2] + cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5));
551
552 dVector3 vCEdgePoint1;
553 vCEdgePoint1[0] = vCposTrans[0] - cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5));
554 vCEdgePoint1[1] = vCposTrans[1] - cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5));
555 vCEdgePoint1[2] = vCposTrans[2] - cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5));
556
557 // transform cylinder edge points into triangle space
558 vCEdgePoint0[0] -= v0[0];
559 vCEdgePoint0[1] -= v0[1];
560 vCEdgePoint0[2] -= v0[2];
561
562 vCEdgePoint1[0] -= v0[0];
563 vCEdgePoint1[1] -= v0[1];
564 vCEdgePoint1[2] -= v0[2];
565
566 dVector4 plPlane;
567 dVector3 vPlaneNormal;
568
569 // triangle plane
570 //plPlane = Plane4f( -cData.vNormal, 0);
571 vPlaneNormal[0] = -cData.vNormal[0];
572 vPlaneNormal[1] = -cData.vNormal[1];
573 vPlaneNormal[2] = -cData.vNormal[2];
574 dConstructPlane(vPlaneNormal,REAL(0.0),plPlane);
575 if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
576 {
577 return false;
578 }
579
580 // plane with edge 0
581 //plPlane = Plane4f( ( cData.vNormal cross cData.vE0 ), REAL(1e-5));
582 dVector3Cross(cData.vNormal,cData.vE0,vPlaneNormal);
583 dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane);
584 if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
585 {
586 return false;
587 }
588
589 // plane with edge 1
590 //dVector3 vTemp = ( cData.vNormal cross cData.vE1 );
591 dVector3Cross(cData.vNormal,cData.vE1,vPlaneNormal);
592 fTemp = dVector3Dot(cData.vE0 , vPlaneNormal) - REAL(1e-5);
593 //plPlane = Plane4f( vTemp, -(( cData.vE0 dot vTemp )-REAL(1e-5)));
594 dConstructPlane(vPlaneNormal,-fTemp,plPlane);
595 if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
596 {
597 return false;
598 }
599
600 // plane with edge 2
601 // plPlane = Plane4f( ( cData.vNormal cross cData.vE2 ), REAL(1e-5));
602 dVector3Cross(cData.vNormal,cData.vE2,vPlaneNormal);
603 dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane);
604 if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
605 {
606 return false;
607 }
608
609 // return capsule edge points into absolute space
610 vCEdgePoint0[0] += v0[0];
611 vCEdgePoint0[1] += v0[1];
612 vCEdgePoint0[2] += v0[2];
613
614 vCEdgePoint1[0] += v0[0];
615 vCEdgePoint1[1] += v0[1];
616 vCEdgePoint1[2] += v0[2];
617
618 // calculate depths for both contact points
619 dVector3 vTemp;
620 dVector3Subtract(vCEdgePoint0,cData.vCylinderPos, vTemp);
621 dReal fRestDepth0 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt;
622 dVector3Subtract(vCEdgePoint1,cData.vCylinderPos, vTemp);
623 dReal fRestDepth1 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt;
624
625 dReal fDepth0 = cData.fBestDepth - (fRestDepth0);
626 dReal fDepth1 = cData.fBestDepth - (fRestDepth1);
627
628 // clamp depths to zero
629 if(fDepth0 < REAL(0.0) )
630 {
631 fDepth0 = REAL(0.0);
632 }
633
634 if(fDepth1<REAL(0.0))
635 {
636 fDepth1 = REAL(0.0);
637 }
638
639 // Generate contact 0
640 {
641 cData.gLocalContacts[cData.nContacts].fDepth = fDepth0;
642 dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
643 dVector3Copy(vCEdgePoint0,cData.gLocalContacts[cData.nContacts].vPos);
644 cData.gLocalContacts[cData.nContacts].nFlags = 1;
645 cData.nContacts++;
646 if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
647 return true;
648 }
649
650 // Generate contact 1
651 {
652 // generate contacts
653 cData.gLocalContacts[cData.nContacts].fDepth = fDepth1;
654 dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
655 dVector3Copy(vCEdgePoint1,cData.gLocalContacts[cData.nContacts].vPos);
656 cData.gLocalContacts[cData.nContacts].nFlags = 1;
657 cData.nContacts++;
658 }
659
660 return true;
661}
662
663void _cldClipCylinderToTriangle(sData& cData,const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)
664{
665 int i = 0;
666 dVector3 avPoints[3];
667 dVector3 avTempArray1[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS];
668 dVector3 avTempArray2[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS];
669
670 dSetZero(&avTempArray1[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4);
671 dSetZero(&avTempArray2[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4);
672
673 // setup array of triangle vertices
674 dVector3Copy(v0,avPoints[0]);
675 dVector3Copy(v1,avPoints[1]);
676 dVector3Copy(v2,avPoints[2]);
677
678 dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
679 dSetZero(vCylinderCircleNormal_Rel,4);
680 // check which circle from cylinder we take for clipping
681 if ( dVector3Dot(cData.vCylinderAxis , cData.vContactNormal) > REAL(0.0))
682 {
683 // get top circle
684 vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
685 vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
686 vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
687
688 vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
689 }
690 else
691 {
692 // get bottom circle
693 vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
694 vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
695 vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
696
697 vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
698 }
699
700 dVector3 vTemp;
701 dQuatInv(cData.qCylinderRot , cData.qInvCylinderRot);
702 // transform triangle points to space of cylinder circle
703 for(i=0; i<3; i++)
704 {
705 dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp);
706 dQuatTransform(cData.qInvCylinderRot,vTemp,avPoints[i]);
707 }
708
709 int iTmpCounter1 = 0;
710 int iTmpCounter2 = 0;
711 dVector4 plPlane;
712
713 // plane of cylinder that contains circle for intersection
714 //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f );
715 dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
716 dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane);
717
718 // Body of base circle of Cylinder
719 int nCircleSegment = 0;
720 for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; nCircleSegment++)
721 {
722 dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane);
723
724 if (0 == (nCircleSegment % 2))
725 {
726 dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane);
727 }
728 else
729 {
730 dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane );
731 }
732
733 dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS );
734 dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS );
735 }
736
737 // back transform clipped points to absolute space
738 dReal ftmpdot;
739 dReal fTempDepth;
740 dVector3 vPoint;
741
742 if (nCircleSegment %2)
743 {
744 for( i=0; i<iTmpCounter2; i++)
745 {
746 dQuatTransform(cData.qCylinderRot,avTempArray2[i], vPoint);
747 vPoint[0] += vCylinderCirclePos[0];
748 vPoint[1] += vCylinderCirclePos[1];
749 vPoint[2] += vCylinderCirclePos[2];
750
751 dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
752 ftmpdot = dFabs(dVector3Dot(vTemp, cData.vContactNormal));
753 fTempDepth = cData.fBestrt - ftmpdot;
754 // Depth must be positive
755 if (fTempDepth > REAL(0.0))
756 {
757 cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth;
758 dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
759 dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos);
760 cData.gLocalContacts[cData.nContacts].nFlags = 1;
761 cData.nContacts++;
762 if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
763 return;;
764 }
765 }
766 }
767 else
768 {
769 for( i=0; i<iTmpCounter1; i++)
770 {
771 dQuatTransform(cData.qCylinderRot,avTempArray1[i], vPoint);
772 vPoint[0] += vCylinderCirclePos[0];
773 vPoint[1] += vCylinderCirclePos[1];
774 vPoint[2] += vCylinderCirclePos[2];
775
776 dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
777 ftmpdot = dFabs(dVector3Dot(vTemp, cData.vContactNormal));
778 fTempDepth = cData.fBestrt - ftmpdot;
779 // Depth must be positive
780 if (fTempDepth > REAL(0.0))
781 {
782 cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth;
783 dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal);
784 dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos);
785 cData.gLocalContacts[cData.nContacts].nFlags = 1;
786 cData.nContacts++;
787 if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
788 return;;
789 }
790 }
791 }
792}
793
794void TestOneTriangleVsCylinder( sData& cData,
795 const dVector3 &v0,
796 const dVector3 &v1,
797 const dVector3 &v2,
798 const bool bDoubleSided)
799{
800
801 // calculate triangle normal
802 dVector3Subtract( v2 , v1 ,cData.vE1);
803 dVector3 vTemp;
804 dVector3Subtract( v0 , v1 ,vTemp);
805 dVector3Cross(cData.vE1 , vTemp , cData.vNormal );
806
807 dNormalize3( cData.vNormal);
808
809 // create plane from triangle
810 //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 );
811 dReal plDistance = -dVector3Dot(v0, cData.vNormal);
812 dVector4 plTrianglePlane;
813 dConstructPlane( cData.vNormal,plDistance,plTrianglePlane);
814
815 // calculate sphere distance to plane
816 dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(cData.vCylinderPos , plTrianglePlane);
817
818 // Sphere must be over positive side of triangle
819 if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided)
820 {
821 // if not don't generate contacts
822 return;
823 }
824
825 dVector3 vPnt0;
826 dVector3 vPnt1;
827 dVector3 vPnt2;
828
829 if (fDistanceCylinderCenterToPlane < REAL(0.0) )
830 {
831 // flip it
832 dVector3Copy(v0 , vPnt0);
833 dVector3Copy(v1 , vPnt2);
834 dVector3Copy(v2 , vPnt1);
835 }
836 else
837 {
838 dVector3Copy(v0 , vPnt0);
839 dVector3Copy(v1 , vPnt1);
840 dVector3Copy(v2 , vPnt2);
841 }
842
843 cData.fBestDepth = MAX_REAL;
844
845 // do intersection test and find best separating axis
846 if(!_cldTestSeparatingAxes(cData , vPnt0, vPnt1, vPnt2) )
847 {
848 // if not found do nothing
849 return;
850 }
851
852 // if best separation axis is not found
853 if ( cData.iBestAxis == 0 )
854 {
855 // this should not happen (we should already exit in that case)
856 dIASSERT(false);
857 // do nothing
858 return;
859 }
860
861 dReal fdot = dVector3Dot( cData.vContactNormal , cData.vCylinderAxis );
862
863 // choose which clipping method are we going to apply
864 if (dFabs(fdot) < REAL(0.9) )
865 {
866 if (!_cldClipCylinderEdgeToTriangle(cData ,vPnt0, vPnt1, vPnt2))
867 {
868 return;
869 }
870 }
871 else
872 {
873 _cldClipCylinderToTriangle(cData ,vPnt0, vPnt1, vPnt2);
874 }
875
876}
877
878void _InitCylinderTrimeshData(sData& cData)
879{
880 // get cylinder information
881 // Rotation
882 const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
883 dMatrix3Copy(pRotCyc,cData.mCylinderRot);
884 dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot);
885
886 // Position
887 const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
888 dVector3Copy(*pPosCyc,cData.vCylinderPos);
889 // Cylinder axis
890 dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
891 // get cylinder radius and size
892 dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
893
894 // get trimesh position and orientation
895 const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh);
896 dMatrix3Copy(pRotTris,cData.mTrimeshRot);
897 dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot);
898
899 // Position
900 const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh);
901 dVector3Copy(*pPosTris,cData.vTrimeshPos);
902
903
904 // calculate basic angle for 8-gon
905 dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS;
906 // calculate angle increment
907 dReal fAngleIncrement = fAngle*REAL(2.0);
908
909 // calculate plane normals
910 // axis dependant code
911 for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++)
912 {
913 cData.avCylinderNormals[i][0] = -dCos(fAngle);
914 cData.avCylinderNormals[i][1] = -dSin(fAngle);
915 cData.avCylinderNormals[i][2] = REAL(0.0);
916
917 fAngle += fAngleIncrement;
918 }
919
920 dSetZero(cData.vBestPoint,4);
921 // reset best depth
922 cData.fBestCenter = REAL(0.0);
923}
924
925#if dTRIMESH_ENABLED
926
927// OPCODE version of cylinder to mesh collider
928#if dTRIMESH_OPCODE
929int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
930{
931 dIASSERT( skip >= (int)sizeof( dContactGeom ) );
932 dIASSERT( o1->type == dCylinderClass );
933 dIASSERT( o2->type == dTriMeshClass );
934 dIASSERT ((flags & NUMC_MASK) >= 1);
935
936 // Main data holder
937 sData cData;
938
939 // Assign ODE stuff
940 cData.gCylinder = o1;
941 cData.gTrimesh = (dxTriMesh*)o2;
942 cData.iFlags = flags;
943 cData.iSkip = skip;
944 cData.gContact = contact;
945 cData.nContacts = 0;
946
947 _InitCylinderTrimeshData(cData);
948
949 OBBCollider& Collider = cData.gTrimesh->_OBBCollider;
950
951 Point cCenter(cData.vCylinderPos[0],cData.vCylinderPos[1],cData.vCylinderPos[2]);
952
953 Point cExtents(cData.fCylinderRadius,cData.fCylinderRadius,cData.fCylinderRadius);
954 cExtents[nCYLINDER_AXIS] = cData.fCylinderSize * REAL(0.5);
955
956 Matrix3x3 obbRot;
957
958 // It is a potential issue to explicitly cast to float
959 // if custom width floating point type is introduced in OPCODE.
960 // It is necessary to make a typedef and cast to it
961 // (e.g. typedef float opc_float;)
962 // However I'm not sure in what header it should be added.
963
964 obbRot[0][0] = /*(float)*/cData.mCylinderRot[0];
965 obbRot[1][0] = /*(float)*/cData.mCylinderRot[1];
966 obbRot[2][0] = /*(float)*/cData.mCylinderRot[2];
967
968 obbRot[0][1] = /*(float)*/cData.mCylinderRot[4];
969 obbRot[1][1] = /*(float)*/cData.mCylinderRot[5];
970 obbRot[2][1] = /*(float)*/cData.mCylinderRot[6];
971
972 obbRot[0][2] = /*(float)*/cData.mCylinderRot[8];
973 obbRot[1][2] = /*(float)*/cData.mCylinderRot[9];
974 obbRot[2][2] = /*(float)*/cData.mCylinderRot[10];
975
976 OBB obbCapsule(cCenter,cExtents,obbRot);
977
978 Matrix4x4 CapsuleMatrix;
979 MakeMatrix(cData.vCylinderPos, cData.mCylinderRot, CapsuleMatrix);
980
981 Matrix4x4 MeshMatrix;
982 MakeMatrix(cData.vTrimeshPos, cData.mTrimeshRot, MeshMatrix);
983
984 // TC results
985 if (cData.gTrimesh->doBoxTC)
986 {
987 dxTriMesh::BoxTC* BoxTC = 0;
988 for (int i = 0; i < cData.gTrimesh->BoxTCCache.size(); i++)
989 {
990 if (cData.gTrimesh->BoxTCCache[i].Geom == cData.gCylinder)
991 {
992 BoxTC = &cData.gTrimesh->BoxTCCache[i];
993 break;
994 }
995 }
996 if (!BoxTC)
997 {
998 cData.gTrimesh->BoxTCCache.push(dxTriMesh::BoxTC());
999
1000 BoxTC = &cData.gTrimesh->BoxTCCache[cData.gTrimesh->BoxTCCache.size() - 1];
1001 BoxTC->Geom = cData.gCylinder;
1002 BoxTC->FatCoeff = REAL(1.0);
1003 }
1004
1005 // Intersect
1006 Collider.SetTemporalCoherence(true);
1007 Collider.Collide(*BoxTC, obbCapsule, cData.gTrimesh->Data->BVTree, null, &MeshMatrix);
1008 }
1009 else
1010 {
1011 Collider.SetTemporalCoherence(false);
1012 Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, cData.gTrimesh->Data->BVTree, null,&MeshMatrix);
1013 }
1014
1015 // Retrieve data
1016 int TriCount = Collider.GetNbTouchedPrimitives();
1017 const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
1018
1019
1020 if (TriCount != 0)
1021 {
1022 if (cData.gTrimesh->ArrayCallback != null)
1023 {
1024 cData.gTrimesh->ArrayCallback(cData.gTrimesh, cData.gCylinder, Triangles, TriCount);
1025 }
1026
1027 // allocate buffer for local contacts on stack
1028 cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK));
1029
1030 int ctContacts0 = 0;
1031
1032 // loop through all intersecting triangles
1033 for (int i = 0; i < TriCount; i++)
1034 {
1035 const int Triint = Triangles[i];
1036 if (!Callback(cData.gTrimesh, cData.gCylinder, Triint)) continue;
1037
1038
1039 dVector3 dv[3];
1040 FetchTriangle(cData.gTrimesh, Triint, cData.vTrimeshPos, cData.mTrimeshRot, dv);
1041
1042 // test this triangle
1043 TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false);
1044
1045 // fill-in tri index for generated contacts
1046 for (; ctContacts0<cData.nContacts; ctContacts0++)
1047 cData.gLocalContacts[ctContacts0].triIndex = Triint;
1048
1049 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1050 if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
1051 {
1052 break;
1053 }
1054 }
1055 }
1056
1057 return _ProcessLocalContacts(cData);
1058}
1059#endif
1060
1061// GIMPACT version of cylinder to mesh collider
1062#if dTRIMESH_GIMPACT
1063int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
1064{
1065 dIASSERT( skip >= (int)sizeof( dContactGeom ) );
1066 dIASSERT( o1->type == dCylinderClass );
1067 dIASSERT( o2->type == dTriMeshClass );
1068 dIASSERT ((flags & NUMC_MASK) >= 1);
1069
1070 // Main data holder
1071 sData cData;
1072
1073 // Assign ODE stuff
1074 cData.gCylinder = o1;
1075 cData.gTrimesh = (dxTriMesh*)o2;
1076 cData.iFlags = flags;
1077 cData.iSkip = skip;
1078 cData.gContact = contact;
1079 cData.nContacts = 0;
1080
1081 _InitCylinderTrimeshData(cData);
1082
1083//*****at first , collide box aabb******//
1084
1085 aabb3f test_aabb;
1086
1087 test_aabb.minX = o1->aabb[0];
1088 test_aabb.maxX = o1->aabb[1];
1089 test_aabb.minY = o1->aabb[2];
1090 test_aabb.maxY = o1->aabb[3];
1091 test_aabb.minZ = o1->aabb[4];
1092 test_aabb.maxZ = o1->aabb[5];
1093
1094
1095 GDYNAMIC_ARRAY collision_result;
1096 GIM_CREATE_BOXQUERY_LIST(collision_result);
1097
1098 gim_aabbset_box_collision(&test_aabb, &cData.gTrimesh->m_collision_trimesh.m_aabbset , &collision_result);
1099
1100 if(collision_result.m_size==0)
1101 {
1102 GIM_DYNARRAY_DESTROY(collision_result);
1103 return 0;
1104 }
1105//*****Set globals for box collision******//
1106
1107 int ctContacts0 = 0;
1108 cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK));
1109
1110 GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
1111 GIM_TRIMESH * ptrimesh = &cData.gTrimesh->m_collision_trimesh;
1112
1113 gim_trimesh_locks_work_data(ptrimesh);
1114
1115
1116 for(unsigned int i=0;i<collision_result.m_size;i++)
1117 {
1118 const int Triint = boxesresult[i];
1119
1120 dVector3 dv[3];
1121 gim_trimesh_get_triangle_vertices(ptrimesh, Triint,dv[0],dv[1],dv[2]);
1122 // test this triangle
1123 TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false);
1124
1125 // fill-in triangle index for generated contacts
1126 for (; ctContacts0<cData.nContacts; ctContacts0++)
1127 cData.gLocalContacts[ctContacts0].triIndex = Triint;
1128
1129 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1130 if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
1131 {
1132 break;
1133 }
1134 }
1135
1136 gim_trimesh_unlocks_work_data(ptrimesh);
1137 GIM_DYNARRAY_DESTROY(collision_result);
1138
1139 return _ProcessLocalContacts(cData);
1140}
1141#endif
1142
1143#endif // dTRIMESH_ENABLED
1144
1145
generated by cgit v1.1 at 2024-09-19 20:33:44 +0000