aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp1181
1 files changed, 1181 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp
new file mode 100644
index 0000000..da235e0
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_trimesh_ccylinder.cpp
@@ -0,0 +1,1181 @@
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 * Triangle-Capsule(Capsule) collider by Alen Ladavac
25 * Ported to ODE by Nguyen Binh
26 */
27
28// NOTES from Nguyen Binh
29// 14 Apr : Seem to be robust
30// There is a problem when you use original Step and set contact friction
31// surface.mu = dInfinity;
32// More description :
33// When I dropped Capsule over the bunny ears, it seems to stuck
34// there for a while. I think the cause is when you set surface.mu = dInfinity;
35// the friction force is too high so it just hang the capsule there.
36// So the good cure for this is to set mu = around 1.5 (in my case)
37// For StepFast1, this become as solid as rock : StepFast1 just approximate
38// friction force.
39
40// NOTES from Croteam's Alen
41//As a side note... there are some extra contacts that can be generated
42//on the edge between two triangles, and if the capsule penetrates deeply into
43//the triangle (usually happens with large mass or low FPS), some such
44//contacts can in some cases push the capsule away from the edge instead of
45//away from the two triangles. This shows up as capsule slowing down a bit
46//when hitting an edge while sliding along a flat tesselated grid of
47//triangles. This is only if capsule is standing upwards.
48
49//Same thing can appear whenever a smooth object (e.g sphere) hits such an
50//edge, and it needs to be solved as a special case probably. This is a
51//problem we are looking forward to address soon.
52
53#include <ode/collision.h>
54#include <ode/matrix.h>
55#include <ode/rotation.h>
56#include <ode/odemath.h>
57#include "collision_util.h"
58
59#define TRIMESH_INTERNAL
60#include "collision_trimesh_internal.h"
61
62#if dTRIMESH_ENABLED
63
64// OPCODE version
65#if dTRIMESH_OPCODE
66// largest number, double or float
67#if defined(dSINGLE)
68#define MAX_REAL FLT_MAX
69#define MIN_REAL (-FLT_MAX)
70#else
71#define MAX_REAL DBL_MAX
72#define MIN_REAL (-DBL_MAX)
73#endif
74
75// To optimize before send contacts to dynamic part
76#define OPTIMIZE_CONTACTS
77
78// dVector3
79// r=a-b
80#define SUBTRACT(a,b,r) \
81 (r)[0]=(a)[0] - (b)[0]; \
82 (r)[1]=(a)[1] - (b)[1]; \
83 (r)[2]=(a)[2] - (b)[2];
84
85
86// dVector3
87// a=b
88#define SET(a,b) \
89 (a)[0]=(b)[0]; \
90 (a)[1]=(b)[1]; \
91 (a)[2]=(b)[2];
92
93
94// dMatrix3
95// a=b
96#define SETM(a,b) \
97 (a)[0]=(b)[0]; \
98 (a)[1]=(b)[1]; \
99 (a)[2]=(b)[2]; \
100 (a)[3]=(b)[3]; \
101 (a)[4]=(b)[4]; \
102 (a)[5]=(b)[5]; \
103 (a)[6]=(b)[6]; \
104 (a)[7]=(b)[7]; \
105 (a)[8]=(b)[8]; \
106 (a)[9]=(b)[9]; \
107 (a)[10]=(b)[10]; \
108 (a)[11]=(b)[11];
109
110
111// dVector3
112// r=a+b
113#define ADD(a,b,r) \
114 (r)[0]=(a)[0] + (b)[0]; \
115 (r)[1]=(a)[1] + (b)[1]; \
116 (r)[2]=(a)[2] + (b)[2];
117
118
119// dMatrix3, int, dVector3
120// v=column a from m
121#define GETCOL(m,a,v) \
122 (v)[0]=(m)[(a)+0]; \
123 (v)[1]=(m)[(a)+4]; \
124 (v)[2]=(m)[(a)+8];
125
126
127// dVector4, dVector3
128// distance between plane p and point v
129#define POINTDISTANCE(p,v) \
130 ( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] ); \
131
132
133// dVector4, dVector3, dReal
134// construct plane from normal and d
135#define CONSTRUCTPLANE(plane,normal,d) \
136 plane[0]=normal[0];\
137 plane[1]=normal[1];\
138 plane[2]=normal[2];\
139 plane[3]=d;
140
141
142// dVector3
143// length of vector a
144#define LENGTHOF(a) \
145 dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);\
146
147inline dReal _length2OfVector3(dVector3 v)
148{
149 return (v[0] * v[0] + v[1] * v[1] + v[2] * v[2] );
150}
151
152
153// Local contacts data
154typedef struct _sLocalContactData
155{
156 dVector3 vPos;
157 dVector3 vNormal;
158 dReal fDepth;
159 int triIndex;
160 int nFlags; // 0 = filtered out, 1 = OK
161}sLocalContactData;
162
163static sLocalContactData *gLocalContacts;
164static unsigned int ctContacts = 0;
165
166// capsule data
167// real time data
168static dMatrix3 mCapsuleRotation;
169static dVector3 vCapsulePosition;
170static dVector3 vCapsuleAxis;
171// static data
172static dReal vCapsuleRadius;
173static dReal fCapsuleSize;
174
175// mesh data
176static dMatrix4 mHullDstPl;
177static dMatrix3 mTriMeshRot;
178static dVector3 mTriMeshPos;
179static dVector3 vE0, vE1, vE2;
180
181// Two geom
182dxGeom* gCylinder;
183dxGeom* gTriMesh;
184
185// global collider data
186static dVector3 vNormal;
187static dReal fBestDepth;
188static dReal fBestCenter;
189static dReal fBestrt;
190static int iBestAxis;
191static dVector3 vN = {0,0,0,0};
192
193static dVector3 vV0;
194static dVector3 vV1;
195static dVector3 vV2;
196
197// ODE contact's specific
198static unsigned int iFlags;
199static dContactGeom *ContactGeoms;
200static int iStride;
201
202// Capsule lie on axis number 3 = (Z axis)
203static const int nCAPSULE_AXIS = 2;
204
205// Use to classify contacts to be "near" in position
206static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4
207// Use to classify contacts to be "near" in normal direction
208static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4
209
210
211// If this two contact can be classified as "near"
212inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2)
213{
214 int bPosNear = 0;
215 int bSameDir = 0;
216 dVector3 vDiff;
217
218 // First check if they are "near" in position
219 SUBTRACT(c1.vPos,c2.vPos,vDiff);
220 if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon)
221 &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon)
222 &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon))
223 {
224 bPosNear = 1;
225 }
226
227 // Second check if they are "near" in normal direction
228 SUBTRACT(c1.vNormal,c2.vNormal,vDiff);
229 if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon)
230 &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon)
231 &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) )
232 {
233 bSameDir = 1;
234 }
235
236 // Will be "near" if position and normal direction are "near"
237 return (bPosNear && bSameDir);
238}
239
240inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2)
241{
242 // The not better will be throw away
243 // You can change the selection criteria here
244 return (c1.fDepth > c2.fDepth);
245}
246
247// iterate through gLocalContacts and filtered out "near contact"
248inline void _OptimizeLocalContacts()
249{
250 int nContacts = ctContacts;
251
252 for (int i = 0; i < nContacts-1; i++)
253 {
254 for (int j = i+1; j < nContacts; j++)
255 {
256 if (_IsNearContacts(gLocalContacts[i],gLocalContacts[j]))
257 {
258 // If they are seem to be the samed then filtered
259 // out the least penetrate one
260 if (_IsBetter(gLocalContacts[j],gLocalContacts[i]))
261 {
262 gLocalContacts[i].nFlags = 0; // filtered 1st contact
263 }
264 else
265 {
266 gLocalContacts[j].nFlags = 0; // filtered 2nd contact
267 }
268
269 // NOTE
270 // There is other way is to add two depth together but
271 // it not work so well. Why???
272 }
273 }
274 }
275}
276
277inline int _ProcessLocalContacts()
278{
279 if (ctContacts == 0)
280 {
281 return 0;
282 }
283
284#ifdef OPTIMIZE_CONTACTS
285 if (ctContacts > 1 && !(iFlags & CONTACTS_UNIMPORTANT))
286 {
287 // Can be optimized...
288 _OptimizeLocalContacts();
289 }
290#endif
291
292 unsigned int iContact = 0;
293 dContactGeom* Contact = 0;
294
295 unsigned int nFinalContact = 0;
296
297 for (iContact = 0; iContact < ctContacts; iContact ++)
298 {
299 // Ensure that we haven't created too many contacts
300 if( nFinalContact >= (iFlags & NUMC_MASK))
301 {
302 break;
303 }
304
305 if (1 == gLocalContacts[iContact].nFlags)
306 {
307 Contact = SAFECONTACT(iFlags, ContactGeoms, nFinalContact, iStride);
308 Contact->depth = gLocalContacts[iContact].fDepth;
309 SET(Contact->normal,gLocalContacts[iContact].vNormal);
310 SET(Contact->pos,gLocalContacts[iContact].vPos);
311 Contact->g1 = gTriMesh;
312 Contact->g2 = gCylinder;
313 Contact->side2 = gLocalContacts[iContact].triIndex;
314
315 nFinalContact++;
316 }
317 }
318 // debug
319 //if (nFinalContact != ctContacts)
320 //{
321 // printf("[Info] %d contacts generated,%d filtered.\n",ctContacts,ctContacts-nFinalContact);
322 //}
323
324 return nFinalContact;
325}
326
327BOOL _cldClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane)
328{
329 // calculate distance of edge points to plane
330 dReal fDistance0 = POINTDISTANCE( plPlane, vEpnt0 );
331 dReal fDistance1 = POINTDISTANCE( plPlane, vEpnt1 );
332
333 // if both points are behind the plane
334 if ( fDistance0 < 0 && fDistance1 < 0 )
335 {
336 // do nothing
337 return FALSE;
338 // if both points in front of the plane
339 } else if ( fDistance0 > 0 && fDistance1 > 0 )
340 {
341 // accept them
342 return TRUE;
343 // if we have edge/plane intersection
344 } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0))
345 {
346
347 // find intersection point of edge and plane
348 dVector3 vIntersectionPoint;
349 vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1);
350 vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1);
351 vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1);
352
353 // clamp correct edge to intersection point
354 if ( fDistance0 < 0 )
355 {
356 SET(vEpnt0,vIntersectionPoint);
357 } else
358 {
359 SET(vEpnt1,vIntersectionPoint);
360 }
361 return TRUE;
362 }
363 return TRUE;
364}
365
366static BOOL _cldTestAxis(const dVector3 &v0,
367 const dVector3 &v1,
368 const dVector3 &v2,
369 dVector3 vAxis,
370 int iAxis,
371 BOOL bNoFlip = FALSE)
372{
373
374 // calculate length of separating axis vector
375 dReal fL = LENGTHOF(vAxis);
376 // if not long enough
377 // TODO : dReal epsilon please
378 if ( fL < REAL(1e-5) )
379 {
380 // do nothing
381 //iLastOutAxis = 0;
382 return TRUE;
383 }
384
385 // otherwise normalize it
386 dNormalize3(vAxis);
387
388 // project capsule on vAxis
389 dReal frc = dFabs(dDOT(vCapsuleAxis,vAxis))*(fCapsuleSize*REAL(0.5)-vCapsuleRadius) + vCapsuleRadius;
390
391 // project triangle on vAxis
392 dReal afv[3];
393 afv[0] = dDOT( vV0 , vAxis );
394 afv[1] = dDOT( vV1 , vAxis );
395 afv[2] = dDOT( vV2 , vAxis );
396
397 dReal fMin = MAX_REAL;
398 dReal fMax = MIN_REAL;
399
400 // for each vertex
401 for(int i=0; i<3; i++)
402 {
403 // find minimum
404 if (afv[i]<fMin)
405 {
406 fMin = afv[i];
407 }
408 // find maximum
409 if (afv[i]>fMax)
410 {
411 fMax = afv[i];
412 }
413 }
414
415 // find triangle's center of interval on axis
416 dReal fCenter = (fMin+fMax)*REAL(0.5);
417 // calculate triangles half interval
418 dReal fTriangleRadius = (fMax-fMin)*REAL(0.5);
419
420 // if they do not overlap,
421 if( dFabs(fCenter) > ( frc + fTriangleRadius ) )
422 {
423 // exit, we have no intersection
424 return FALSE;
425 }
426
427 // calculate depth
428 dReal fDepth = dFabs(fCenter) - (frc+fTriangleRadius);
429
430 // if greater then best found so far
431 if ( fDepth > fBestDepth )
432 {
433 // remember depth
434 fBestDepth = fDepth;
435 fBestCenter = fCenter;
436 fBestrt = fTriangleRadius;
437
438 vNormal[0] = vAxis[0];
439 vNormal[1] = vAxis[1];
440 vNormal[2] = vAxis[2];
441
442 iBestAxis = iAxis;
443
444 // flip normal if interval is wrong faced
445 if (fCenter<0 && !bNoFlip)
446 {
447 vNormal[0] = -vNormal[0];
448 vNormal[1] = -vNormal[1];
449 vNormal[2] = -vNormal[2];
450
451 fBestCenter = -fCenter;
452 }
453 }
454
455 return TRUE;
456}
457
458// helper for less key strokes
459inline void _CalculateAxis(const dVector3& v1,
460 const dVector3& v2,
461 const dVector3& v3,
462 const dVector3& v4,
463 dVector3& r)
464{
465 dVector3 t1;
466 dVector3 t2;
467
468 SUBTRACT(v1,v2,t1);
469 dCROSS(t2,=,t1,v3);
470 dCROSS(r,=,t2,v4);
471}
472
473static BOOL _cldTestSeparatingAxesOfCapsule(const dVector3 &v0,
474 const dVector3 &v1,
475 const dVector3 &v2,
476 uint8 flags)
477{
478 // calculate caps centers in absolute space
479 dVector3 vCp0;
480 vCp0[0] = vCapsulePosition[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
481 vCp0[1] = vCapsulePosition[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
482 vCp0[2] = vCapsulePosition[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
483
484 dVector3 vCp1;
485 vCp1[0] = vCapsulePosition[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
486 vCp1[1] = vCapsulePosition[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
487 vCp1[2] = vCapsulePosition[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
488
489 // reset best axis
490 iBestAxis = 0;
491 // reset best depth
492 fBestDepth = -MAX_REAL;
493 // reset separating axis vector
494 dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
495
496 // Epsilon value for checking axis vector length
497 const dReal fEpsilon = 1e-6f;
498
499 // Translate triangle to Cc cord.
500 SUBTRACT(v0 , vCapsulePosition, vV0);
501 SUBTRACT(v1 , vCapsulePosition, vV1);
502 SUBTRACT(v2 , vCapsulePosition, vV2);
503
504 // We begin to test for 19 separating axis now
505 // I wonder does it help if we employ the method like ISA-GJK???
506 // Or at least we should do experiment and find what axis will
507 // be most likely to be separating axis to check it first.
508
509 // Original
510 // axis vN
511 //vAxis = -vN;
512 vAxis[0] = - vN[0];
513 vAxis[1] = - vN[1];
514 vAxis[2] = - vN[2];
515 if (!_cldTestAxis( v0, v1, v2, vAxis, 1, TRUE))
516 {
517 return FALSE;
518 }
519
520 if (flags & dxTriMeshData::kEdge0)
521 {
522 // axis CxE0 - Edge 0
523 dCROSS(vAxis,=,vCapsuleAxis,vE0);
524 //vAxis = dCROSS( vCapsuleAxis cross vE0 );
525 if( _length2OfVector3( vAxis ) > fEpsilon ) {
526 if (!_cldTestAxis( v0, v1, v2, vAxis, 2)) {
527 return FALSE;
528 }
529 }
530 }
531
532 if (flags & dxTriMeshData::kEdge1)
533 {
534 // axis CxE1 - Edge 1
535 dCROSS(vAxis,=,vCapsuleAxis,vE1);
536 //vAxis = ( vCapsuleAxis cross vE1 );
537 if(_length2OfVector3( vAxis ) > fEpsilon ) {
538 if (!_cldTestAxis( v0, v1, v2, vAxis, 3)) {
539 return FALSE;
540 }
541 }
542 }
543
544 if (flags & dxTriMeshData::kEdge2)
545 {
546 // axis CxE2 - Edge 2
547 //vAxis = ( vCapsuleAxis cross vE2 );
548 dCROSS(vAxis,=,vCapsuleAxis,vE2);
549 if(_length2OfVector3( vAxis ) > fEpsilon ) {
550 if (!_cldTestAxis( v0, v1, v2, vAxis, 4)) {
551 return FALSE;
552 }
553 }
554 }
555
556 if (flags & dxTriMeshData::kEdge0)
557 {
558 // first capsule point
559 // axis ((Cp0-V0) x E0) x E0
560 _CalculateAxis(vCp0,v0,vE0,vE0,vAxis);
561 // vAxis = ( ( vCp0-v0) cross vE0 ) cross vE0;
562 if(_length2OfVector3( vAxis ) > fEpsilon ) {
563 if (!_cldTestAxis( v0, v1, v2, vAxis, 5)) {
564 return FALSE;
565 }
566 }
567 }
568
569 if (flags & dxTriMeshData::kEdge1)
570 {
571 // axis ((Cp0-V1) x E1) x E1
572 _CalculateAxis(vCp0,v1,vE1,vE1,vAxis);
573 //vAxis = ( ( vCp0-v1) cross vE1 ) cross vE1;
574 if(_length2OfVector3( vAxis ) > fEpsilon ) {
575 if (!_cldTestAxis( v0, v1, v2, vAxis, 6)) {
576 return FALSE;
577 }
578 }
579 }
580
581 if (flags & dxTriMeshData::kEdge2)
582 {
583 // axis ((Cp0-V2) x E2) x E2
584 _CalculateAxis(vCp0,v2,vE2,vE2,vAxis);
585 //vAxis = ( ( vCp0-v2) cross vE2 ) cross vE2;
586 if(_length2OfVector3( vAxis ) > fEpsilon ) {
587 if (!_cldTestAxis( v0, v1, v2, vAxis, 7)) {
588 return FALSE;
589 }
590 }
591 }
592
593 if (flags & dxTriMeshData::kEdge0)
594 {
595 // second capsule point
596 // axis ((Cp1-V0) x E0) x E0
597 _CalculateAxis(vCp1,v0,vE0,vE0,vAxis);
598 //vAxis = ( ( vCp1-v0 ) cross vE0 ) cross vE0;
599 if(_length2OfVector3( vAxis ) > fEpsilon ) {
600 if (!_cldTestAxis( v0, v1, v2, vAxis, 8)) {
601 return FALSE;
602 }
603 }
604 }
605
606 if (flags & dxTriMeshData::kEdge1)
607 {
608 // axis ((Cp1-V1) x E1) x E1
609 _CalculateAxis(vCp1,v1,vE1,vE1,vAxis);
610 //vAxis = ( ( vCp1-v1 ) cross vE1 ) cross vE1;
611 if(_length2OfVector3( vAxis ) > fEpsilon ) {
612 if (!_cldTestAxis( v0, v1, v2, vAxis, 9)) {
613 return FALSE;
614 }
615 }
616 }
617
618 if (flags & dxTriMeshData::kEdge2)
619 {
620 // axis ((Cp1-V2) x E2) x E2
621 _CalculateAxis(vCp1,v2,vE2,vE2,vAxis);
622 //vAxis = ( ( vCp1-v2 ) cross vE2 ) cross vE2;
623 if(_length2OfVector3( vAxis ) > fEpsilon ) {
624 if (!_cldTestAxis( v0, v1, v2, vAxis, 10)) {
625 return FALSE;
626 }
627 }
628 }
629
630 if (flags & dxTriMeshData::kVert0)
631 {
632 // first vertex on triangle
633 // axis ((V0-Cp0) x C) x C
634 _CalculateAxis(v0,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);
635 //vAxis = ( ( v0-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
636 if(_length2OfVector3( vAxis ) > fEpsilon ) {
637 if (!_cldTestAxis( v0, v1, v2, vAxis, 11)) {
638 return FALSE;
639 }
640 }
641 }
642
643 if (flags & dxTriMeshData::kVert1)
644 {
645 // second vertex on triangle
646 // axis ((V1-Cp0) x C) x C
647 _CalculateAxis(v1,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);
648 //vAxis = ( ( v1-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
649 if(_length2OfVector3( vAxis ) > fEpsilon ) {
650 if (!_cldTestAxis( v0, v1, v2, vAxis, 12)) {
651 return FALSE;
652 }
653 }
654 }
655
656 if (flags & dxTriMeshData::kVert2)
657 {
658 // third vertex on triangle
659 // axis ((V2-Cp0) x C) x C
660 _CalculateAxis(v2,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);
661 //vAxis = ( ( v2-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
662 if(_length2OfVector3( vAxis ) > fEpsilon ) {
663 if (!_cldTestAxis( v0, v1, v2, vAxis, 13)) {
664 return FALSE;
665 }
666 }
667 }
668
669 // Test as separating axes direction vectors between each triangle
670 // edge and each capsule's cap center
671
672 if (flags & dxTriMeshData::kVert0)
673 {
674 // first triangle vertex and first capsule point
675 //vAxis = v0 - vCp0;
676 SUBTRACT(v0,vCp0,vAxis);
677 if(_length2OfVector3( vAxis ) > fEpsilon ) {
678 if (!_cldTestAxis( v0, v1, v2, vAxis, 14)) {
679 return FALSE;
680 }
681 }
682 }
683
684 if (flags & dxTriMeshData::kVert1)
685 {
686 // second triangle vertex and first capsule point
687 //vAxis = v1 - vCp0;
688 SUBTRACT(v1,vCp0,vAxis);
689 if(_length2OfVector3( vAxis ) > fEpsilon ) {
690 if (!_cldTestAxis( v0, v1, v2, vAxis, 15)) {
691 return FALSE;
692 }
693 }
694 }
695
696 if (flags & dxTriMeshData::kVert2)
697 {
698 // third triangle vertex and first capsule point
699 //vAxis = v2 - vCp0;
700 SUBTRACT(v2,vCp0,vAxis);
701 if(_length2OfVector3( vAxis ) > fEpsilon ) {
702 if (!_cldTestAxis( v0, v1, v2, vAxis, 16)) {
703 return FALSE;
704 }
705 }
706 }
707
708 if (flags & dxTriMeshData::kVert0)
709 {
710 // first triangle vertex and second capsule point
711 //vAxis = v0 - vCp1;
712 SUBTRACT(v0,vCp1,vAxis);
713 if(_length2OfVector3( vAxis ) > fEpsilon ) {
714 if (!_cldTestAxis( v0, v1, v2, vAxis, 17)) {
715 return FALSE;
716 }
717 }
718 }
719
720 if (flags & dxTriMeshData::kVert1)
721 {
722 // second triangle vertex and second capsule point
723 //vAxis = v1 - vCp1;
724 SUBTRACT(v1,vCp1,vAxis);
725 if(_length2OfVector3( vAxis ) > fEpsilon ) {
726 if (!_cldTestAxis( v0, v1, v2, vAxis, 18)) {
727 return FALSE;
728 }
729 }
730 }
731
732 if (flags & dxTriMeshData::kVert2)
733 {
734 // third triangle vertex and second capsule point
735 //vAxis = v2 - vCp1;
736 SUBTRACT(v2,vCp1,vAxis);
737 if(_length2OfVector3( vAxis ) > fEpsilon ) {
738 if (!_cldTestAxis( v0, v1, v2, vAxis, 19)) {
739 return FALSE;
740 }
741 }
742 }
743
744 return TRUE;
745}
746
747// test one mesh triangle on intersection with capsule
748static void _cldTestOneTriangleVSCapsule( const dVector3 &v0,
749 const dVector3 &v1,
750 const dVector3 &v2,
751 uint8 flags)
752{
753
754 // calculate edges
755 SUBTRACT(v1,v0,vE0);
756 SUBTRACT(v2,v1,vE1);
757 SUBTRACT(v0,v2,vE2);
758
759 dVector3 _minus_vE0;
760 SUBTRACT(v0,v1,_minus_vE0);
761
762 // calculate poly normal
763 dCROSS(vN,=,vE1,_minus_vE0);
764 dNormalize3(vN);
765
766 // create plane from triangle
767 dReal plDistance = -dDOT(v0,vN);
768 dVector4 plTrianglePlane;
769 CONSTRUCTPLANE(plTrianglePlane,vN,plDistance);
770
771 // calculate capsule distance to plane
772 dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,vCapsulePosition);
773
774 // Capsule must be over positive side of triangle
775 if(fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/)
776 {
777 // if not don't generate contacts
778 return;
779 }
780
781 dVector3 vPnt0;
782 SET (vPnt0,v0);
783 dVector3 vPnt1;
784 SET (vPnt1,v1);
785 dVector3 vPnt2;
786 SET (vPnt2,v2);
787
788 if (fDistanceCapsuleCenterToPlane < 0 )
789 {
790 SET (vPnt0,v0);
791 SET (vPnt1,v2);
792 SET (vPnt2,v1);
793 }
794
795 // do intersection test and find best separating axis
796 if(!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags) )
797 {
798 // if not found do nothing
799 return;
800 }
801
802 // if best separation axis is not found
803 if ( iBestAxis == 0 )
804 {
805 // this should not happen (we should already exit in that case)
806 dIASSERT(FALSE);
807 // do nothing
808 return;
809 }
810
811 // calculate caps centers in absolute space
812 dVector3 vCposTrans;
813 vCposTrans[0] = vCapsulePosition[0] + vNormal[0]*vCapsuleRadius;
814 vCposTrans[1] = vCapsulePosition[1] + vNormal[1]*vCapsuleRadius;
815 vCposTrans[2] = vCapsulePosition[2] + vNormal[2]*vCapsuleRadius;
816
817 dVector3 vCEdgePoint0;
818 vCEdgePoint0[0] = vCposTrans[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
819 vCEdgePoint0[1] = vCposTrans[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
820 vCEdgePoint0[2] = vCposTrans[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
821
822 dVector3 vCEdgePoint1;
823 vCEdgePoint1[0] = vCposTrans[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
824 vCEdgePoint1[1] = vCposTrans[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
825 vCEdgePoint1[2] = vCposTrans[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
826
827 // transform capsule edge points into triangle space
828 vCEdgePoint0[0] -= vPnt0[0];
829 vCEdgePoint0[1] -= vPnt0[1];
830 vCEdgePoint0[2] -= vPnt0[2];
831
832 vCEdgePoint1[0] -= vPnt0[0];
833 vCEdgePoint1[1] -= vPnt0[1];
834 vCEdgePoint1[2] -= vPnt0[2];
835
836 dVector4 plPlane;
837 dVector3 _minus_vN;
838 _minus_vN[0] = -vN[0];
839 _minus_vN[1] = -vN[1];
840 _minus_vN[2] = -vN[2];
841 // triangle plane
842 CONSTRUCTPLANE(plPlane,_minus_vN,0);
843 //plPlane = Plane4f( -vN, 0);
844
845 if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
846 {
847 return;
848 }
849
850 // plane with edge 0
851 dVector3 vTemp;
852 dCROSS(vTemp,=,vN,vE0);
853 CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
854 if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
855 {
856 return;
857 }
858
859 dCROSS(vTemp,=,vN,vE1);
860 CONSTRUCTPLANE(plPlane, vTemp, -(dDOT(vE0,vTemp)-REAL(1e-5)));
861 if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
862 {
863 return;
864 }
865
866 dCROSS(vTemp,=,vN,vE2);
867 CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
868 if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) {
869 return;
870 }
871
872 // return capsule edge points into absolute space
873 vCEdgePoint0[0] += vPnt0[0];
874 vCEdgePoint0[1] += vPnt0[1];
875 vCEdgePoint0[2] += vPnt0[2];
876
877 vCEdgePoint1[0] += vPnt0[0];
878 vCEdgePoint1[1] += vPnt0[1];
879 vCEdgePoint1[2] += vPnt0[2];
880
881 // calculate depths for both contact points
882 SUBTRACT(vCEdgePoint0,vCapsulePosition,vTemp);
883 dReal fDepth0 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt);
884 SUBTRACT(vCEdgePoint1,vCapsulePosition,vTemp);
885 dReal fDepth1 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt);
886
887 // clamp depths to zero
888 if(fDepth0 < 0)
889 {
890 fDepth0 = 0.0f;
891 }
892
893 if(fDepth1 < 0 )
894 {
895 fDepth1 = 0.0f;
896 }
897
898 // Cached contacts's data
899 // contact 0
900 dIASSERT(ctContacts < (iFlags & NUMC_MASK)); // Do not call function if there is no room to store result
901 gLocalContacts[ctContacts].fDepth = fDepth0;
902 SET(gLocalContacts[ctContacts].vNormal,vNormal);
903 SET(gLocalContacts[ctContacts].vPos,vCEdgePoint0);
904 gLocalContacts[ctContacts].nFlags = 1;
905 ctContacts++;
906
907 if (ctContacts < (iFlags & NUMC_MASK)) {
908 // contact 1
909 gLocalContacts[ctContacts].fDepth = fDepth1;
910 SET(gLocalContacts[ctContacts].vNormal,vNormal);
911 SET(gLocalContacts[ctContacts].vPos,vCEdgePoint1);
912 gLocalContacts[ctContacts].nFlags = 1;
913 ctContacts++;
914 }
915
916}
917
918// capsule - trimesh by CroTeam
919// Ported by Nguyem Binh
920int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
921{
922 dIASSERT (skip >= (int)sizeof(dContactGeom));
923 dIASSERT (o1->type == dTriMeshClass);
924 dIASSERT (o2->type == dCapsuleClass);
925 dIASSERT ((flags & NUMC_MASK) >= 1);
926
927 dxTriMesh* TriMesh = (dxTriMesh*)o1;
928 gCylinder = o2;
929 gTriMesh = o1;
930
931 const dMatrix3* pRot = (const dMatrix3*) dGeomGetRotation(gCylinder);
932 memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
933
934 const dVector3* pDst = (const dVector3*)dGeomGetPosition(gCylinder);
935 memcpy(vCapsulePosition,pDst,sizeof(dVector3));
936
937 vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
938 vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
939 vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
940
941 // Get size of Capsule
942 dGeomCapsuleGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
943 fCapsuleSize += 2*vCapsuleRadius;
944
945 const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh);
946 memcpy(mTriMeshRot,pTriRot,sizeof(dMatrix3));
947
948 const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh);
949 memcpy(mTriMeshPos,pTriPos,sizeof(dVector3));
950
951 // global info for contact creation
952 iStride =skip;
953 iFlags =flags;
954 ContactGeoms =contact;
955
956 // reset contact counter
957 ctContacts = 0;
958
959 // reset best depth
960 fBestDepth = - MAX_REAL;
961 fBestCenter = 0;
962 fBestrt = 0;
963
964
965
966
967 // reset collision normal
968 vNormal[0] = REAL(0.0);
969 vNormal[1] = REAL(0.0);
970 vNormal[2] = REAL(0.0);
971
972 // Will it better to use LSS here? -> confirm Pierre.
973 OBBCollider& Collider = TriMesh->_OBBCollider;
974
975 // It is a potential issue to explicitly cast to float
976 // if custom width floating point type is introduced in OPCODE.
977 // It is necessary to make a typedef and cast to it
978 // (e.g. typedef float opc_float;)
979 // However I'm not sure in what header it should be added.
980
981 Point cCenter(/*(float)*/ vCapsulePosition[0], /*(float)*/ vCapsulePosition[1], /*(float)*/ vCapsulePosition[2]);
982 Point cExtents(/*(float)*/ vCapsuleRadius, /*(float)*/ vCapsuleRadius,/*(float)*/ fCapsuleSize/2);
983
984 Matrix3x3 obbRot;
985
986 obbRot[0][0] = /*(float)*/ mCapsuleRotation[0];
987 obbRot[1][0] = /*(float)*/ mCapsuleRotation[1];
988 obbRot[2][0] = /*(float)*/ mCapsuleRotation[2];
989
990 obbRot[0][1] = /*(float)*/ mCapsuleRotation[4];
991 obbRot[1][1] = /*(float)*/ mCapsuleRotation[5];
992 obbRot[2][1] = /*(float)*/ mCapsuleRotation[6];
993
994 obbRot[0][2] = /*(float)*/ mCapsuleRotation[8];
995 obbRot[1][2] = /*(float)*/ mCapsuleRotation[9];
996 obbRot[2][2] = /*(float)*/ mCapsuleRotation[10];
997
998 OBB obbCapsule(cCenter,cExtents,obbRot);
999
1000 Matrix4x4 CapsuleMatrix;
1001 MakeMatrix(vCapsulePosition, mCapsuleRotation, CapsuleMatrix);
1002
1003 Matrix4x4 MeshMatrix;
1004 MakeMatrix(mTriMeshPos, mTriMeshRot, MeshMatrix);
1005
1006 // TC results
1007 if (TriMesh->doBoxTC) {
1008 dxTriMesh::BoxTC* BoxTC = 0;
1009 for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
1010 if (TriMesh->BoxTCCache[i].Geom == gCylinder){
1011 BoxTC = &TriMesh->BoxTCCache[i];
1012 break;
1013 }
1014 }
1015 if (!BoxTC){
1016 TriMesh->BoxTCCache.push(dxTriMesh::BoxTC());
1017
1018 BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1];
1019 BoxTC->Geom = gCylinder;
1020 BoxTC->FatCoeff = 1.0f;
1021 }
1022
1023 // Intersect
1024 Collider.SetTemporalCoherence(true);
1025 Collider.Collide(*BoxTC, obbCapsule, TriMesh->Data->BVTree, null, &MeshMatrix);
1026 }
1027 else {
1028 Collider.SetTemporalCoherence(false);
1029 Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, TriMesh->Data->BVTree, null,&MeshMatrix);
1030 }
1031
1032 if (! Collider.GetContactStatus()) {
1033 // no collision occurred
1034 return 0;
1035 }
1036
1037 // Retrieve data
1038 int TriCount = Collider.GetNbTouchedPrimitives();
1039 const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
1040
1041 if (TriCount != 0)
1042 {
1043 if (TriMesh->ArrayCallback != null)
1044 {
1045 TriMesh->ArrayCallback(TriMesh, gCylinder, Triangles, TriCount);
1046 }
1047
1048 // allocate buffer for local contacts on stack
1049 gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(iFlags & NUMC_MASK));
1050
1051 unsigned int ctContacts0 = ctContacts;
1052
1053 uint8* UseFlags = TriMesh->Data->UseFlags;
1054
1055 // loop through all intersecting triangles
1056 for (int i = 0; i < TriCount; i++)
1057 {
1058 const int Triint = Triangles[i];
1059 if (!Callback(TriMesh, gCylinder, Triint)) continue;
1060
1061
1062 dVector3 dv[3];
1063 FetchTriangle(TriMesh, Triint, mTriMeshPos, mTriMeshRot, dv);
1064
1065 uint8 flags = UseFlags ? UseFlags[Triint] : dxTriMeshData::kUseAll;
1066
1067 // test this triangle
1068 _cldTestOneTriangleVSCapsule(dv[0],dv[1],dv[2], flags);
1069
1070 // fill-in tri index for generated contacts
1071 for (; ctContacts0<ctContacts; ctContacts0++)
1072 gLocalContacts[ctContacts0].triIndex = Triint;
1073
1074 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1075 if(ctContacts>=(iFlags & NUMC_MASK))
1076 {
1077 break;
1078 }
1079
1080 }
1081 }
1082
1083 return _ProcessLocalContacts();
1084}
1085#endif
1086
1087// GIMPACT version
1088#if dTRIMESH_GIMPACT
1089#define nCAPSULE_AXIS 2
1090// capsule - trimesh By francisco leon
1091int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
1092{
1093 dIASSERT (skip >= (int)sizeof(dContactGeom));
1094 dIASSERT (o1->type == dTriMeshClass);
1095 dIASSERT (o2->type == dCapsuleClass);
1096 dIASSERT ((flags & NUMC_MASK) >= 1);
1097
1098 dxTriMesh* TriMesh = (dxTriMesh*)o1;
1099 dxGeom* gCylinder = o2;
1100
1101 //Get capsule params
1102 dMatrix3 mCapsuleRotation;
1103 dVector3 vCapsulePosition;
1104 dVector3 vCapsuleAxis;
1105 dReal vCapsuleRadius;
1106 dReal fCapsuleSize;
1107 dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder);
1108 memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
1109 dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder);
1110 memcpy(vCapsulePosition,pDst,sizeof(dVector3));
1111 //Axis
1112 vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
1113 vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
1114 vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
1115 // Get size of CCylinder
1116 dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
1117 fCapsuleSize*=0.5f;
1118 //Set Capsule params
1119 GIM_CAPSULE_DATA capsule;
1120
1121 capsule.m_radius = vCapsuleRadius;
1122 VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis);
1123 VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1);
1124 VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis);
1125 VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2);
1126
1127
1128//Create contact list
1129 GDYNAMIC_ARRAY trimeshcontacts;
1130 GIM_CREATE_CONTACT_LIST(trimeshcontacts);
1131
1132 //Collide trimeshe vs capsule
1133 gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts);
1134
1135
1136 if(trimeshcontacts.m_size == 0)
1137 {
1138 GIM_DYNARRAY_DESTROY(trimeshcontacts);
1139 return 0;
1140 }
1141
1142 GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
1143
1144 unsigned contactcount = trimeshcontacts.m_size;
1145 unsigned contactmax = (unsigned)(flags & NUMC_MASK);
1146 if (contactcount > contactmax)
1147 {
1148 contactcount = contactmax;
1149 }
1150
1151 dContactGeom* pcontact;
1152 unsigned i;
1153
1154 for (i=0;i<contactcount;i++)
1155 {
1156 pcontact = SAFECONTACT(flags, contact, i, skip);
1157
1158 pcontact->pos[0] = ptrimeshcontacts->m_point[0];
1159 pcontact->pos[1] = ptrimeshcontacts->m_point[1];
1160 pcontact->pos[2] = ptrimeshcontacts->m_point[2];
1161 pcontact->pos[3] = 1.0f;
1162
1163 pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
1164 pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
1165 pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
1166 pcontact->normal[3] = 0;
1167
1168 pcontact->depth = ptrimeshcontacts->m_depth;
1169 pcontact->g1 = TriMesh;
1170 pcontact->g2 = gCylinder;
1171
1172 ptrimeshcontacts++;
1173 }
1174
1175 GIM_DYNARRAY_DESTROY(trimeshcontacts);
1176
1177 return (int)contactcount;
1178}
1179#endif
1180
1181#endif // dTRIMESH_ENABLED
generated by cgit v1.1 at 2024-12-26 23:12:00 +0000