aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:22:23 +0000
committerdan miller2007-10-19 05:22:23 +0000
commit1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43 (patch)
tree51bcae7a1b8381a6bf6fd8025a7de1e30fe0045d /libraries/ode-0.9/ode/src/collision_trimesh_box.cpp
parentone more for the gipper (diff)
downloadopensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.zip
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.gz
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.bz2
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.xz
trying to fix my screwup, please hold on
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_trimesh_box.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_box.cpp1497
1 files changed, 0 insertions, 1497 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp
deleted file mode 100644
index 0194d02..0000000
--- a/libraries/ode-0.9/ode/src/collision_trimesh_box.cpp
+++ /dev/null
@@ -1,1497 +0,0 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23
24/*************************************************************************
25 * *
26 * Triangle-box collider by Alen Ladavac and Vedran Klanac. *
27 * Ported to ODE by Oskari Nyman. *
28 * *
29 *************************************************************************/
30
31
32#include <ode/collision.h>
33#include <ode/matrix.h>
34#include <ode/rotation.h>
35#include <ode/odemath.h>
36#include "collision_util.h"
37
38#define TRIMESH_INTERNAL
39#include "collision_trimesh_internal.h"
40
41#if dTRIMESH_ENABLED
42
43
44static void
45GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
46 dxGeom* in_g1, dxGeom* in_g2,
47 const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
48 int& OutTriCount);
49
50
51// largest number, double or float
52#if defined(dSINGLE)
53 #define MAXVALUE FLT_MAX
54#else
55 #define MAXVALUE DBL_MAX
56#endif
57
58
59// dVector3
60// r=a-b
61#define SUBTRACT(a,b,r) do{ \
62 (r)[0]=(a)[0] - (b)[0]; \
63 (r)[1]=(a)[1] - (b)[1]; \
64 (r)[2]=(a)[2] - (b)[2]; }while(0)
65
66
67// dVector3
68// a=b
69#define SET(a,b) do{ \
70 (a)[0]=(b)[0]; \
71 (a)[1]=(b)[1]; \
72 (a)[2]=(b)[2]; }while(0)
73
74
75// dMatrix3
76// a=b
77#define SETM(a,b) do{ \
78 (a)[0]=(b)[0]; \
79 (a)[1]=(b)[1]; \
80 (a)[2]=(b)[2]; \
81 (a)[3]=(b)[3]; \
82 (a)[4]=(b)[4]; \
83 (a)[5]=(b)[5]; \
84 (a)[6]=(b)[6]; \
85 (a)[7]=(b)[7]; \
86 (a)[8]=(b)[8]; \
87 (a)[9]=(b)[9]; \
88 (a)[10]=(b)[10]; \
89 (a)[11]=(b)[11]; }while(0)
90
91
92// dVector3
93// r=a+b
94#define ADD(a,b,r) do{ \
95 (r)[0]=(a)[0] + (b)[0]; \
96 (r)[1]=(a)[1] + (b)[1]; \
97 (r)[2]=(a)[2] + (b)[2]; }while(0)
98
99
100// dMatrix3, int, dVector3
101// v=column a from m
102#define GETCOL(m,a,v) do{ \
103 (v)[0]=(m)[(a)+0]; \
104 (v)[1]=(m)[(a)+4]; \
105 (v)[2]=(m)[(a)+8]; }while(0)
106
107
108// dVector4, dVector3
109// distance between plane p and point v
110#define POINTDISTANCE(p,v) \
111 ( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] )
112
113
114// dVector4, dVector3, dReal
115// construct plane from normal and d
116#define CONSTRUCTPLANE(plane,normal,d) do{ \
117 plane[0]=normal[0];\
118 plane[1]=normal[1];\
119 plane[2]=normal[2];\
120 plane[3]=d; }while(0)
121
122
123// dVector3
124// length of vector a
125#define LENGTHOF(a) \
126 dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2])
127
128
129// box data
130static dMatrix3 mHullBoxRot;
131static dVector3 vHullBoxPos;
132static dVector3 vBoxHalfSize;
133
134// mesh data
135static dVector3 vHullDstPos;
136
137// global collider data
138static dVector3 vBestNormal;
139static dReal fBestDepth;
140static int iBestAxis = 0;
141static int iExitAxis = 0;
142static dVector3 vE0, vE1, vE2, vN;
143
144// global info for contact creation
145static int iFlags;
146static dContactGeom *ContactGeoms;
147static int iStride;
148static dxGeom *Geom1;
149static dxGeom *Geom2;
150static int ctContacts = 0;
151
152
153
154// Test normal of mesh face as separating axis for intersection
155static bool _cldTestNormal( dReal fp0, dReal fR, dVector3 vNormal, int iAxis )
156{
157 // calculate overlapping interval of box and triangle
158 dReal fDepth = fR+fp0;
159
160 // if we do not overlap
161 if ( fDepth<0 ) {
162 // do nothing
163 return false;
164 }
165
166 // calculate normal's length
167 dReal fLength = LENGTHOF(vNormal);
168 // if long enough
169 if ( fLength > 0.0f ) {
170
171 dReal fOneOverLength = 1.0f/fLength;
172 // normalize depth
173 fDepth = fDepth*fOneOverLength;
174
175 // get minimum depth
176 if (fDepth<fBestDepth) {
177 vBestNormal[0] = -vNormal[0]*fOneOverLength;
178 vBestNormal[1] = -vNormal[1]*fOneOverLength;
179 vBestNormal[2] = -vNormal[2]*fOneOverLength;
180 iBestAxis = iAxis;
181 //dAASSERT(fDepth>=0);
182 fBestDepth = fDepth;
183 }
184
185 }
186
187 return true;
188}
189
190
191
192
193// Test box axis as separating axis
194static bool _cldTestFace( dReal fp0, dReal fp1, dReal fp2, dReal fR, dReal fD,
195 dVector3 vNormal, int iAxis )
196{
197 dReal fMin, fMax;
198
199 // find min of triangle interval
200 if ( fp0 < fp1 ) {
201 if ( fp0 < fp2 ) {
202 fMin = fp0;
203 } else {
204 fMin = fp2;
205 }
206 } else {
207 if( fp1 < fp2 ) {
208 fMin = fp1;
209 } else {
210 fMin = fp2;
211 }
212 }
213
214 // find max of triangle interval
215 if ( fp0 > fp1 ) {
216 if ( fp0 > fp2 ) {
217 fMax = fp0;
218 } else {
219 fMax = fp2;
220 }
221 } else {
222 if( fp1 > fp2 ) {
223 fMax = fp1;
224 } else {
225 fMax = fp2;
226 }
227 }
228
229 // calculate minimum and maximum depth
230 dReal fDepthMin = fR - fMin;
231 dReal fDepthMax = fMax + fR;
232
233 // if we dont't have overlapping interval
234 if ( fDepthMin < 0 || fDepthMax < 0 ) {
235 // do nothing
236 return false;
237 }
238
239 dReal fDepth = 0;
240
241 // if greater depth is on negative side
242 if ( fDepthMin > fDepthMax ) {
243 // use smaller depth (one from positive side)
244 fDepth = fDepthMax;
245 // flip normal direction
246 vNormal[0] = -vNormal[0];
247 vNormal[1] = -vNormal[1];
248 vNormal[2] = -vNormal[2];
249 fD = -fD;
250 // if greater depth is on positive side
251 } else {
252 // use smaller depth (one from negative side)
253 fDepth = fDepthMin;
254 }
255
256
257 // if lower depth than best found so far
258 if (fDepth<fBestDepth) {
259 // remember current axis as best axis
260 vBestNormal[0] = vNormal[0];
261 vBestNormal[1] = vNormal[1];
262 vBestNormal[2] = vNormal[2];
263 iBestAxis = iAxis;
264 //dAASSERT(fDepth>=0);
265 fBestDepth = fDepth;
266 }
267
268 return true;
269}
270
271
272
273
274
275// Test cross products of box axis and triangle edges as separating axis
276static bool _cldTestEdge( dReal fp0, dReal fp1, dReal fR, dReal fD,
277 dVector3 vNormal, int iAxis )
278{
279 dReal fMin, fMax;
280
281
282 // ===== Begin Patch by Francisco Leon, 2006/10/28 =====
283
284 // Fixed Null Normal. This prevents boxes passing
285 // through trimeshes at certain contact angles
286
287 fMin = vNormal[0] * vNormal[0] +
288 vNormal[1] * vNormal[1] +
289 vNormal[2] * vNormal[2];
290
291 if ( fMin <= dEpsilon ) /// THIS NORMAL WOULD BE DANGEROUS
292 return true;
293
294 // ===== Ending Patch by Francisco Leon =====
295
296
297 // calculate min and max interval values
298 if ( fp0 < fp1 ) {
299 fMin = fp0;
300 fMax = fp1;
301 } else {
302 fMin = fp1;
303 fMax = fp0;
304 }
305
306 // check if we overlapp
307 dReal fDepthMin = fR - fMin;
308 dReal fDepthMax = fMax + fR;
309
310 // if we don't overlapp
311 if ( fDepthMin < 0 || fDepthMax < 0 ) {
312 // do nothing
313 return false;
314 }
315
316 dReal fDepth;
317
318
319 // if greater depth is on negative side
320 if ( fDepthMin > fDepthMax ) {
321 // use smaller depth (one from positive side)
322 fDepth = fDepthMax;
323 // flip normal direction
324 vNormal[0] = -vNormal[0];
325 vNormal[1] = -vNormal[1];
326 vNormal[2] = -vNormal[2];
327 fD = -fD;
328 // if greater depth is on positive side
329 } else {
330 // use smaller depth (one from negative side)
331 fDepth = fDepthMin;
332 }
333
334 // calculate normal's length
335 dReal fLength = LENGTHOF(vNormal);
336
337 // if long enough
338 if ( fLength > 0.0f ) {
339
340 // normalize depth
341 dReal fOneOverLength = 1.0f/fLength;
342 fDepth = fDepth*fOneOverLength;
343 fD*=fOneOverLength;
344
345
346 // if lower depth than best found so far (favor face over edges)
347 if (fDepth*1.5f<fBestDepth) {
348 // remember current axis as best axis
349 vBestNormal[0] = vNormal[0]*fOneOverLength;
350 vBestNormal[1] = vNormal[1]*fOneOverLength;
351 vBestNormal[2] = vNormal[2]*fOneOverLength;
352 iBestAxis = iAxis;
353 //dAASSERT(fDepth>=0);
354 fBestDepth = fDepth;
355 }
356 }
357
358 return true;
359}
360
361
362
363
364
365// clip polygon with plane and generate new polygon points
366static void _cldClipPolyToPlane( dVector3 avArrayIn[], int ctIn,
367 dVector3 avArrayOut[], int &ctOut,
368 const dVector4 &plPlane )
369{
370 // start with no output points
371 ctOut = 0;
372
373 int i0 = ctIn-1;
374
375 // for each edge in input polygon
376 for (int i1=0; i1<ctIn; i0=i1, i1++) {
377
378
379 // calculate distance of edge points to plane
380 dReal fDistance0 = POINTDISTANCE( plPlane ,avArrayIn[i0] );
381 dReal fDistance1 = POINTDISTANCE( plPlane ,avArrayIn[i1] );
382
383
384 // if first point is in front of plane
385 if( fDistance0 >= 0 ) {
386 // emit point
387 avArrayOut[ctOut][0] = avArrayIn[i0][0];
388 avArrayOut[ctOut][1] = avArrayIn[i0][1];
389 avArrayOut[ctOut][2] = avArrayIn[i0][2];
390 ctOut++;
391 }
392
393 // if points are on different sides
394 if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) {
395
396 // find intersection point of edge and plane
397 dVector3 vIntersectionPoint;
398 vIntersectionPoint[0]= avArrayIn[i0][0] - (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
399 vIntersectionPoint[1]= avArrayIn[i0][1] - (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
400 vIntersectionPoint[2]= avArrayIn[i0][2] - (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
401
402 // emit intersection point
403 avArrayOut[ctOut][0] = vIntersectionPoint[0];
404 avArrayOut[ctOut][1] = vIntersectionPoint[1];
405 avArrayOut[ctOut][2] = vIntersectionPoint[2];
406 ctOut++;
407 }
408 }
409
410}
411
412
413
414
415static bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
416 // reset best axis
417 iBestAxis = 0;
418 iExitAxis = -1;
419 fBestDepth = MAXVALUE;
420
421 // calculate edges
422 SUBTRACT(v1,v0,vE0);
423 SUBTRACT(v2,v0,vE1);
424 SUBTRACT(vE1,vE0,vE2);
425
426 // calculate poly normal
427 dCROSS(vN,=,vE0,vE1);
428
429 // extract box axes as vectors
430 dVector3 vA0,vA1,vA2;
431 GETCOL(mHullBoxRot,0,vA0);
432 GETCOL(mHullBoxRot,1,vA1);
433 GETCOL(mHullBoxRot,2,vA2);
434
435 // box halfsizes
436 dReal fa0 = vBoxHalfSize[0];
437 dReal fa1 = vBoxHalfSize[1];
438 dReal fa2 = vBoxHalfSize[2];
439
440 // calculate relative position between box and triangle
441 dVector3 vD;
442 SUBTRACT(v0,vHullBoxPos,vD);
443
444 // calculate length of face normal
445 dReal fNLen = LENGTHOF( vN );
446
447 dVector3 vL;
448 dReal fp0, fp1, fp2, fR, fD;
449
450 // Test separating axes for intersection
451 // ************************************************
452 // Axis 1 - Triangle Normal
453 SET(vL,vN);
454 fp0 = dDOT(vL,vD);
455 fp1 = fp0;
456 fp2 = fp0;
457 fR=fa0*dFabs( dDOT(vN,vA0) ) + fa1 * dFabs( dDOT(vN,vA1) ) + fa2 * dFabs( dDOT(vN,vA2) );
458
459
460 if( !_cldTestNormal( fp0, fR, vL, 1) ) {
461 iExitAxis=1;
462 return false;
463 }
464
465 // ************************************************
466
467 // Test Faces
468 // ************************************************
469 // Axis 2 - Box X-Axis
470 SET(vL,vA0);
471 fD = dDOT(vL,vN)/fNLen;
472 fp0 = dDOT(vL,vD);
473 fp1 = fp0 + dDOT(vA0,vE0);
474 fp2 = fp0 + dDOT(vA0,vE1);
475 fR = fa0;
476
477
478 if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 2) ) {
479 iExitAxis=2;
480 return false;
481 }
482 // ************************************************
483
484 // ************************************************
485 // Axis 3 - Box Y-Axis
486 SET(vL,vA1);
487 fD = dDOT(vL,vN)/fNLen;
488 fp0 = dDOT(vL,vD);
489 fp1 = fp0 + dDOT(vA1,vE0);
490 fp2 = fp0 + dDOT(vA1,vE1);
491 fR = fa1;
492
493
494 if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 3) ) {
495 iExitAxis=3;
496 return false;
497 }
498
499 // ************************************************
500
501 // ************************************************
502 // Axis 4 - Box Z-Axis
503 SET(vL,vA2);
504 fD = dDOT(vL,vN)/fNLen;
505 fp0 = dDOT(vL,vD);
506 fp1 = fp0 + dDOT(vA2,vE0);
507 fp2 = fp0 + dDOT(vA2,vE1);
508 fR = fa2;
509
510
511 if( !_cldTestFace( fp0, fp1, fp2, fR, fD, vL, 4) ) {
512 iExitAxis=4;
513 return false;
514 }
515
516 // ************************************************
517
518 // Test Edges
519 // ************************************************
520 // Axis 5 - Box X-Axis cross Edge0
521 dCROSS(vL,=,vA0,vE0);
522 fD = dDOT(vL,vN)/fNLen;
523 fp0 = dDOT(vL,vD);
524 fp1 = fp0;
525 fp2 = fp0 + dDOT(vA0,vN);
526 fR = fa1 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA1,vE0));
527
528
529 if( !_cldTestEdge( fp1, fp2, fR, fD, vL, 5) ) {
530 iExitAxis=5;
531 return false;
532 }
533 // ************************************************
534
535 // ************************************************
536 // Axis 6 - Box X-Axis cross Edge1
537 dCROSS(vL,=,vA0,vE1);
538 fD = dDOT(vL,vN)/fNLen;
539 fp0 = dDOT(vL,vD);
540 fp1 = fp0 - dDOT(vA0,vN);
541 fp2 = fp0;
542 fR = fa1 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA1,vE1));
543
544
545 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 6) ) {
546 iExitAxis=6;
547 return false;
548 }
549 // ************************************************
550
551 // ************************************************
552 // Axis 7 - Box X-Axis cross Edge2
553 dCROSS(vL,=,vA0,vE2);
554 fD = dDOT(vL,vN)/fNLen;
555 fp0 = dDOT(vL,vD);
556 fp1 = fp0 - dDOT(vA0,vN);
557 fp2 = fp0 - dDOT(vA0,vN);
558 fR = fa1 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA1,vE2));
559
560
561 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 7) ) {
562 iExitAxis=7;
563 return false;
564 }
565
566 // ************************************************
567
568 // ************************************************
569 // Axis 8 - Box Y-Axis cross Edge0
570 dCROSS(vL,=,vA1,vE0);
571 fD = dDOT(vL,vN)/fNLen;
572 fp0 = dDOT(vL,vD);
573 fp1 = fp0;
574 fp2 = fp0 + dDOT(vA1,vN);
575 fR = fa0 * dFabs(dDOT(vA2,vE0)) + fa2 * dFabs(dDOT(vA0,vE0));
576
577
578 if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 8) ) {
579 iExitAxis=8;
580 return false;
581 }
582
583 // ************************************************
584
585 // ************************************************
586 // Axis 9 - Box Y-Axis cross Edge1
587 dCROSS(vL,=,vA1,vE1);
588 fD = dDOT(vL,vN)/fNLen;
589 fp0 = dDOT(vL,vD);
590 fp1 = fp0 - dDOT(vA1,vN);
591 fp2 = fp0;
592 fR = fa0 * dFabs(dDOT(vA2,vE1)) + fa2 * dFabs(dDOT(vA0,vE1));
593
594
595 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 9) ) {
596 iExitAxis=9;
597 return false;
598 }
599
600 // ************************************************
601
602 // ************************************************
603 // Axis 10 - Box Y-Axis cross Edge2
604 dCROSS(vL,=,vA1,vE2);
605 fD = dDOT(vL,vN)/fNLen;
606 fp0 = dDOT(vL,vD);
607 fp1 = fp0 - dDOT(vA1,vN);
608 fp2 = fp0 - dDOT(vA1,vN);
609 fR = fa0 * dFabs(dDOT(vA2,vE2)) + fa2 * dFabs(dDOT(vA0,vE2));
610
611
612 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 10) ) {
613 iExitAxis=10;
614 return false;
615 }
616
617 // ************************************************
618
619 // ************************************************
620 // Axis 11 - Box Z-Axis cross Edge0
621 dCROSS(vL,=,vA2,vE0);
622 fD = dDOT(vL,vN)/fNLen;
623 fp0 = dDOT(vL,vD);
624 fp1 = fp0;
625 fp2 = fp0 + dDOT(vA2,vN);
626 fR = fa0 * dFabs(dDOT(vA1,vE0)) + fa1 * dFabs(dDOT(vA0,vE0));
627
628
629 if( !_cldTestEdge( fp0, fp2, fR, fD, vL, 11) ) {
630 iExitAxis=11;
631 return false;
632 }
633 // ************************************************
634
635 // ************************************************
636 // Axis 12 - Box Z-Axis cross Edge1
637 dCROSS(vL,=,vA2,vE1);
638 fD = dDOT(vL,vN)/fNLen;
639 fp0 = dDOT(vL,vD);
640 fp1 = fp0 - dDOT(vA2,vN);
641 fp2 = fp0;
642 fR = fa0 * dFabs(dDOT(vA1,vE1)) + fa1 * dFabs(dDOT(vA0,vE1));
643
644
645 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 12) ) {
646 iExitAxis=12;
647 return false;
648 }
649 // ************************************************
650
651 // ************************************************
652 // Axis 13 - Box Z-Axis cross Edge2
653 dCROSS(vL,=,vA2,vE2);
654 fD = dDOT(vL,vN)/fNLen;
655 fp0 = dDOT(vL,vD);
656 fp1 = fp0 - dDOT(vA2,vN);
657 fp2 = fp0 - dDOT(vA2,vN);
658 fR = fa0 * dFabs(dDOT(vA1,vE2)) + fa1 * dFabs(dDOT(vA0,vE2));
659
660
661 if( !_cldTestEdge( fp0, fp1, fR, fD, vL, 13) ) {
662 iExitAxis=13;
663 return false;
664 }
665
666 // ************************************************
667 return true;
668}
669
670
671
672
673
674// find two closest points on two lines
675static bool _cldClosestPointOnTwoLines( dVector3 vPoint1, dVector3 vLenVec1,
676 dVector3 vPoint2, dVector3 vLenVec2,
677 dReal &fvalue1, dReal &fvalue2)
678{
679 // calulate denominator
680 dVector3 vp;
681 SUBTRACT(vPoint2,vPoint1,vp);
682 dReal fuaub = dDOT(vLenVec1,vLenVec2);
683 dReal fq1 = dDOT(vLenVec1,vp);
684 dReal fq2 = -dDOT(vLenVec2,vp);
685 dReal fd = 1.0f - fuaub * fuaub;
686
687 // if denominator is positive
688 if (fd > 0.0f) {
689 // calculate points of closest approach
690 fd = 1.0f/fd;
691 fvalue1 = (fq1 + fuaub*fq2)*fd;
692 fvalue2 = (fuaub*fq1 + fq2)*fd;
693 return true;
694 // otherwise
695 } else {
696 // lines are parallel
697 fvalue1 = 0.0f;
698 fvalue2 = 0.0f;
699 return false;
700 }
701
702}
703
704
705
706
707
708// clip and generate contacts
709static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
710 dIASSERT( !(iFlags & CONTACTS_UNIMPORTANT) || ctContacts < (iFlags & NUMC_MASK) ); // Do not call the function if there is no room to store results
711
712 // if we have edge/edge intersection
713 if ( iBestAxis > 4 ) {
714
715 dVector3 vub,vPb,vPa;
716
717 SET(vPa,vHullBoxPos);
718
719 // calculate point on box edge
720 for( int i=0; i<3; i++) {
721 dVector3 vRotCol;
722 GETCOL(mHullBoxRot,i,vRotCol);
723 dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f;
724
725 vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0];
726 vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1];
727 vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2];
728 }
729
730 int iEdge = (iBestAxis-5)%3;
731
732 // decide which edge is on triangle
733 if ( iEdge == 0 ) {
734 SET(vPb,v0);
735 SET(vub,vE0);
736 } else if ( iEdge == 1) {
737 SET(vPb,v2);
738 SET(vub,vE1);
739 } else {
740 SET(vPb,v1);
741 SET(vub,vE2);
742 }
743
744
745 // setup direction parameter for face edge
746 dNormalize3(vub);
747
748 dReal fParam1, fParam2;
749
750 // setup direction parameter for box edge
751 dVector3 vua;
752 int col=(iBestAxis-5)/3;
753 GETCOL(mHullBoxRot,col,vua);
754
755 // find two closest points on both edges
756 _cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 );
757 vPa[0] += vua[0]*fParam1;
758 vPa[1] += vua[1]*fParam1;
759 vPa[2] += vua[2]*fParam1;
760
761 vPb[0] += vub[0]*fParam2;
762 vPb[1] += vub[1]*fParam2;
763 vPb[2] += vub[2]*fParam2;
764
765 // calculate collision point
766 dVector3 vPntTmp;
767 ADD(vPa,vPb,vPntTmp);
768
769 vPntTmp[0]*=0.5f;
770 vPntTmp[1]*=0.5f;
771 vPntTmp[2]*=0.5f;
772
773 // generate contact point between two closest points
774#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
775 dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
776 Contact->depth = fBestDepth;
777 SET(Contact->normal,vBestNormal);
778 SET(Contact->pos,vPntTmp);
779 Contact->g1 = Geom1;
780 Contact->g2 = Geom2;
781 ctContacts++;
782#endif
783 GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
784 vPntTmp, vBestNormal, fBestDepth, ctContacts);
785
786
787 // if triangle is the referent face then clip box to triangle face
788 } else if ( iBestAxis == 1 ) {
789
790
791 dVector3 vNormal2;
792 vNormal2[0]=-vBestNormal[0];
793 vNormal2[1]=-vBestNormal[1];
794 vNormal2[2]=-vBestNormal[2];
795
796
797 // vNr is normal in box frame, pointing from triangle to box
798 dMatrix3 mTransposed;
799 mTransposed[0*4+0]=mHullBoxRot[0*4+0];
800 mTransposed[0*4+1]=mHullBoxRot[1*4+0];
801 mTransposed[0*4+2]=mHullBoxRot[2*4+0];
802
803 mTransposed[1*4+0]=mHullBoxRot[0*4+1];
804 mTransposed[1*4+1]=mHullBoxRot[1*4+1];
805 mTransposed[1*4+2]=mHullBoxRot[2*4+1];
806
807 mTransposed[2*4+0]=mHullBoxRot[0*4+2];
808 mTransposed[2*4+1]=mHullBoxRot[1*4+2];
809 mTransposed[2*4+2]=mHullBoxRot[2*4+2];
810
811 dVector3 vNr;
812 vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2];
813 vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2];
814 vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2];
815
816
817 dVector3 vAbsNormal;
818 vAbsNormal[0] = dFabs( vNr[0] );
819 vAbsNormal[1] = dFabs( vNr[1] );
820 vAbsNormal[2] = dFabs( vNr[2] );
821
822 // get closest face from box
823 int iB0, iB1, iB2;
824 if (vAbsNormal[1] > vAbsNormal[0]) {
825 if (vAbsNormal[1] > vAbsNormal[2]) {
826 iB1 = 0; iB0 = 1; iB2 = 2;
827 } else {
828 iB1 = 0; iB2 = 1; iB0 = 2;
829 }
830 } else {
831
832 if (vAbsNormal[0] > vAbsNormal[2]) {
833 iB0 = 0; iB1 = 1; iB2 = 2;
834 } else {
835 iB1 = 0; iB2 = 1; iB0 = 2;
836 }
837 }
838
839 // Here find center of box face we are going to project
840 dVector3 vCenter;
841 dVector3 vRotCol;
842 GETCOL(mHullBoxRot,iB0,vRotCol);
843
844 if (vNr[iB0] > 0) {
845 vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0];
846 vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1];
847 vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2];
848 } else {
849 vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0];
850 vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1];
851 vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2];
852 }
853
854 // Here find 4 corner points of box
855 dVector3 avPoints[4];
856
857 dVector3 vRotCol2;
858 GETCOL(mHullBoxRot,iB1,vRotCol);
859 GETCOL(mHullBoxRot,iB2,vRotCol2);
860
861 for(int x=0;x<3;x++) {
862 avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
863 avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
864 avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
865 avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
866 }
867
868
869 // clip Box face with 4 planes of triangle (1 face plane, 3 egde planes)
870 dVector3 avTempArray1[9];
871 dVector3 avTempArray2[9];
872 dVector4 plPlane;
873
874 int iTempCnt1=0;
875 int iTempCnt2=0;
876
877 // zeroify vectors - necessary?
878 for(int i=0; i<9; i++) {
879 avTempArray1[i][0]=0;
880 avTempArray1[i][1]=0;
881 avTempArray1[i][2]=0;
882
883 avTempArray2[i][0]=0;
884 avTempArray2[i][1]=0;
885 avTempArray2[i][2]=0;
886 }
887
888
889 // Normal plane
890 dVector3 vTemp;
891 vTemp[0]=-vN[0];
892 vTemp[1]=-vN[1];
893 vTemp[2]=-vN[2];
894 dNormalize3(vTemp);
895 CONSTRUCTPLANE(plPlane,vTemp,0);
896
897 _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane );
898
899
900 // Plane p0
901 dVector3 vTemp2;
902 SUBTRACT(v1,v0,vTemp2);
903 dCROSS(vTemp,=,vN,vTemp2);
904 dNormalize3(vTemp);
905 CONSTRUCTPLANE(plPlane,vTemp,0);
906
907 _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
908
909
910 // Plane p1
911 SUBTRACT(v2,v1,vTemp2);
912 dCROSS(vTemp,=,vN,vTemp2);
913 dNormalize3(vTemp);
914 SUBTRACT(v0,v2,vTemp2);
915 CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp));
916
917 _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
918
919
920 // Plane p2
921 SUBTRACT(v0,v2,vTemp2);
922 dCROSS(vTemp,=,vN,vTemp2);
923 dNormalize3(vTemp);
924 CONSTRUCTPLANE(plPlane,vTemp,0);
925
926 _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
927
928
929 // END of clipping polygons
930
931
932
933 // for each generated contact point
934 for ( int i=0; i<iTempCnt2; i++ ) {
935 // calculate depth
936 dReal fTempDepth = dDOT(vNormal2,avTempArray2[i]);
937
938 // clamp depth to zero
939 if (fTempDepth > 0) {
940 fTempDepth = 0;
941 }
942
943 dVector3 vPntTmp;
944 ADD(avTempArray2[i],v0,vPntTmp);
945
946#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
947 dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
948
949 Contact->depth = -fTempDepth;
950 SET(Contact->normal,vBestNormal);
951 SET(Contact->pos,vPntTmp);
952 Contact->g1 = Geom1;
953 Contact->g2 = Geom2;
954 ctContacts++;
955#endif
956 GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
957 vPntTmp, vBestNormal, -fTempDepth, ctContacts);
958
959 if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
960 break;
961 }
962 }
963
964 //dAASSERT(ctContacts>0);
965
966 // if box face is the referent face, then clip triangle on box face
967 } else { // 2 <= if iBestAxis <= 4
968
969 // get normal of box face
970 dVector3 vNormal2;
971 SET(vNormal2,vBestNormal);
972
973 // get indices of box axes in correct order
974 int iA0,iA1,iA2;
975 iA0 = iBestAxis-2;
976 if ( iA0 == 0 ) {
977 iA1 = 1; iA2 = 2;
978 } else if ( iA0 == 1 ) {
979 iA1 = 0; iA2 = 2;
980 } else {
981 iA1 = 0; iA2 = 1;
982 }
983
984 dVector3 avPoints[3];
985 // calculate triangle vertices in box frame
986 SUBTRACT(v0,vHullBoxPos,avPoints[0]);
987 SUBTRACT(v1,vHullBoxPos,avPoints[1]);
988 SUBTRACT(v2,vHullBoxPos,avPoints[2]);
989
990 // CLIP Polygons
991 // define temp data for clipping
992 dVector3 avTempArray1[9];
993 dVector3 avTempArray2[9];
994
995 int iTempCnt1, iTempCnt2;
996
997 // zeroify vectors - necessary?
998 for(int i=0; i<9; i++) {
999 avTempArray1[i][0]=0;
1000 avTempArray1[i][1]=0;
1001 avTempArray1[i][2]=0;
1002
1003 avTempArray2[i][0]=0;
1004 avTempArray2[i][1]=0;
1005 avTempArray2[i][2]=0;
1006 }
1007
1008 // clip triangle with 5 box planes (1 face plane, 4 edge planes)
1009
1010 dVector4 plPlane;
1011
1012 // Normal plane
1013 dVector3 vTemp;
1014 vTemp[0]=-vNormal2[0];
1015 vTemp[1]=-vNormal2[1];
1016 vTemp[2]=-vNormal2[2];
1017 CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA0]);
1018
1019 _cldClipPolyToPlane( avPoints, 3, avTempArray1, iTempCnt1, plPlane );
1020
1021
1022 // Plane p0
1023 GETCOL(mHullBoxRot,iA1,vTemp);
1024 CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]);
1025
1026 _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
1027
1028
1029 // Plane p1
1030 GETCOL(mHullBoxRot,iA1,vTemp);
1031 vTemp[0]=-vTemp[0];
1032 vTemp[1]=-vTemp[1];
1033 vTemp[2]=-vTemp[2];
1034 CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA1]);
1035
1036 _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
1037
1038
1039 // Plane p2
1040 GETCOL(mHullBoxRot,iA2,vTemp);
1041 CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]);
1042
1043 _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
1044
1045
1046 // Plane p3
1047 GETCOL(mHullBoxRot,iA2,vTemp);
1048 vTemp[0]=-vTemp[0];
1049 vTemp[1]=-vTemp[1];
1050 vTemp[2]=-vTemp[2];
1051 CONSTRUCTPLANE(plPlane,vTemp,vBoxHalfSize[iA2]);
1052
1053 _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
1054
1055
1056 // for each generated contact point
1057 for ( int i=0; i<iTempCnt1; i++ ) {
1058 // calculate depth
1059 dReal fTempDepth = dDOT(vNormal2,avTempArray1[i])-vBoxHalfSize[iA0];
1060
1061 // clamp depth to zero
1062 if (fTempDepth > 0) {
1063 fTempDepth = 0;
1064 }
1065
1066 // generate contact data
1067 dVector3 vPntTmp;
1068 ADD(avTempArray1[i],vHullBoxPos,vPntTmp);
1069
1070#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
1071 dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
1072
1073 Contact->depth = -fTempDepth;
1074 SET(Contact->normal,vBestNormal);
1075 SET(Contact->pos,vPntTmp);
1076 Contact->g1 = Geom1;
1077 Contact->g2 = Geom2;
1078 ctContacts++;
1079#endif
1080 GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
1081 vPntTmp, vBestNormal, -fTempDepth, ctContacts);
1082
1083 if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
1084 break;
1085 }
1086 }
1087
1088 //dAASSERT(ctContacts>0);
1089 }
1090
1091}
1092
1093
1094
1095
1096
1097// test one mesh triangle on intersection with given box
1098static void _cldTestOneTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2)//, void *pvUser)
1099{
1100 // do intersection test and find best separating axis
1101 if(!_cldTestSeparatingAxes(v0, v1, v2) ) {
1102 // if not found do nothing
1103 return;
1104 }
1105
1106 // if best separation axis is not found
1107 if ( iBestAxis == 0 ) {
1108 // this should not happen (we should already exit in that case)
1109 //dMessage (0, "best separation axis not found");
1110 // do nothing
1111 return;
1112 }
1113
1114 _cldClipping(v0, v1, v2);
1115}
1116
1117
1118
1119
1120
1121// OPCODE version of box to mesh collider
1122#if dTRIMESH_OPCODE
1123int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){
1124 dIASSERT (Stride >= (int)sizeof(dContactGeom));
1125 dIASSERT (g1->type == dTriMeshClass);
1126 dIASSERT (BoxGeom->type == dBoxClass);
1127 dIASSERT ((Flags & NUMC_MASK) >= 1);
1128
1129
1130 dxTriMesh* TriMesh = (dxTriMesh*)g1;
1131
1132
1133 // get source hull position, orientation and half size
1134 const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom);
1135 const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom);
1136
1137 // to global
1138 SETM(mHullBoxRot,mRotBox);
1139 SET(vHullBoxPos,vPosBox);
1140
1141 dGeomBoxGetLengths(BoxGeom, vBoxHalfSize);
1142 vBoxHalfSize[0] *= 0.5f;
1143 vBoxHalfSize[1] *= 0.5f;
1144 vBoxHalfSize[2] *= 0.5f;
1145
1146
1147
1148 // get destination hull position and orientation
1149 const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
1150 const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
1151
1152 // to global
1153 SET(vHullDstPos,vPosMesh);
1154
1155
1156
1157 // global info for contact creation
1158 ctContacts = 0;
1159 iStride=Stride;
1160 iFlags=Flags;
1161 ContactGeoms=Contacts;
1162 Geom1=TriMesh;
1163 Geom2=BoxGeom;
1164
1165
1166
1167 // reset stuff
1168 fBestDepth = MAXVALUE;
1169 vBestNormal[0]=0;
1170 vBestNormal[1]=0;
1171 vBestNormal[2]=0;
1172
1173 OBBCollider& Collider = TriMesh->_OBBCollider;
1174
1175
1176
1177
1178 // Make OBB
1179 OBB Box;
1180 Box.mCenter.x = vPosBox[0];
1181 Box.mCenter.y = vPosBox[1];
1182 Box.mCenter.z = vPosBox[2];
1183
1184 // It is a potential issue to explicitly cast to float
1185 // if custom width floating point type is introduced in OPCODE.
1186 // It is necessary to make a typedef and cast to it
1187 // (e.g. typedef float opc_float;)
1188 // However I'm not sure in what header it should be added.
1189
1190 Box.mExtents.x = /*(float)*/vBoxHalfSize[0];
1191 Box.mExtents.y = /*(float)*/vBoxHalfSize[1];
1192 Box.mExtents.z = /*(float)*/vBoxHalfSize[2];
1193
1194 Box.mRot.m[0][0] = /*(float)*/mRotBox[0];
1195 Box.mRot.m[1][0] = /*(float)*/mRotBox[1];
1196 Box.mRot.m[2][0] = /*(float)*/mRotBox[2];
1197
1198 Box.mRot.m[0][1] = /*(float)*/mRotBox[4];
1199 Box.mRot.m[1][1] = /*(float)*/mRotBox[5];
1200 Box.mRot.m[2][1] = /*(float)*/mRotBox[6];
1201
1202 Box.mRot.m[0][2] = /*(float)*/mRotBox[8];
1203 Box.mRot.m[1][2] = /*(float)*/mRotBox[9];
1204 Box.mRot.m[2][2] = /*(float)*/mRotBox[10];
1205
1206 Matrix4x4 amatrix;
1207 Matrix4x4 BoxMatrix = MakeMatrix(vPosBox, mRotBox, amatrix);
1208
1209 Matrix4x4 InvBoxMatrix;
1210 InvertPRMatrix(InvBoxMatrix, BoxMatrix);
1211
1212 // TC results
1213 if (TriMesh->doBoxTC) {
1214 dxTriMesh::BoxTC* BoxTC = 0;
1215 for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
1216 if (TriMesh->BoxTCCache[i].Geom == BoxGeom){
1217 BoxTC = &TriMesh->BoxTCCache[i];
1218 break;
1219 }
1220 }
1221 if (!BoxTC){
1222 TriMesh->BoxTCCache.push(dxTriMesh::BoxTC());
1223
1224 BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1];
1225 BoxTC->Geom = BoxGeom;
1226 BoxTC->FatCoeff = 1.1f; // Pierre recommends this, instead of 1.0
1227 }
1228
1229 // Intersect
1230 Collider.SetTemporalCoherence(true);
1231 Collider.Collide(*BoxTC, Box, TriMesh->Data->BVTree, null, &MakeMatrix(vPosMesh, mRotMesh, amatrix));
1232 }
1233 else {
1234 Collider.SetTemporalCoherence(false);
1235 Collider.Collide(dxTriMesh::defaultBoxCache, Box, TriMesh->Data->BVTree, null,
1236 &MakeMatrix(vPosMesh, mRotMesh, amatrix));
1237 }
1238
1239 if (! Collider.GetContactStatus()) {
1240 // no collision occurred
1241 return 0;
1242 }
1243
1244 // Retrieve data
1245 int TriCount = Collider.GetNbTouchedPrimitives();
1246 const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
1247
1248 if (TriCount != 0){
1249 if (TriMesh->ArrayCallback != null){
1250 TriMesh->ArrayCallback(TriMesh, BoxGeom, Triangles, TriCount);
1251 }
1252
1253 int ctContacts0 = 0;
1254
1255 // loop through all intersecting triangles
1256 for (int i = 0; i < TriCount; i++){
1257
1258
1259 const int Triint = Triangles[i];
1260 if (!Callback(TriMesh, BoxGeom, Triint)) continue;
1261
1262
1263 dVector3 dv[3];
1264 FetchTriangle(TriMesh, Triint, vPosMesh, mRotMesh, dv);
1265
1266
1267 // test this triangle
1268 _cldTestOneTriangle(dv[0],dv[1],dv[2]);
1269
1270 // fill-in tri index for generated contacts
1271 for (; ctContacts0<ctContacts; ctContacts0++)
1272 SAFECONTACT(iFlags, ContactGeoms, ctContacts0, iStride)->side1 = Triint;
1273
1274 /*
1275 NOTE by Oleh_Derevenko:
1276 The function continues checking triangles after maximal number
1277 of contacts is reached because it selects maximal penetration depths.
1278 See also comments in GenerateContact()
1279 */
1280 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1281 if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
1282 break;
1283 }
1284 }
1285
1286
1287 return ctContacts;
1288}
1289#endif
1290
1291// GIMPACT version of box to mesh collider
1292#if dTRIMESH_GIMPACT
1293int dCollideBTL(dxGeom* g1, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride)
1294{
1295 dIASSERT (Stride >= (int)sizeof(dContactGeom));
1296 dIASSERT (g1->type == dTriMeshClass);
1297 dIASSERT (BoxGeom->type == dBoxClass);
1298 dIASSERT ((Flags & NUMC_MASK) >= 1);
1299
1300
1301 dxTriMesh* TriMesh = (dxTriMesh*)g1;
1302
1303
1304 // get source hull position, orientation and half size
1305 const dMatrix3& mRotBox=*(const dMatrix3*)dGeomGetRotation(BoxGeom);
1306 const dVector3& vPosBox=*(const dVector3*)dGeomGetPosition(BoxGeom);
1307
1308 // to global
1309 SETM(mHullBoxRot,mRotBox);
1310 SET(vHullBoxPos,vPosBox);
1311
1312 dGeomBoxGetLengths(BoxGeom, vBoxHalfSize);
1313 vBoxHalfSize[0] *= 0.5f;
1314 vBoxHalfSize[1] *= 0.5f;
1315 vBoxHalfSize[2] *= 0.5f;
1316
1317 // get destination hull position and orientation
1318 /*const dMatrix3& mRotMesh=*(const dMatrix3*)dGeomGetRotation(TriMesh);
1319 const dVector3& vPosMesh=*(const dVector3*)dGeomGetPosition(TriMesh);
1320
1321 // to global
1322 SET(vHullDstPos,vPosMesh);*/
1323
1324 // global info for contact creation
1325 ctContacts = 0;
1326 iStride=Stride;
1327 iFlags=Flags;
1328 ContactGeoms=Contacts;
1329 Geom1=TriMesh;
1330 Geom2=BoxGeom;
1331
1332
1333 // reset stuff
1334 fBestDepth = MAXVALUE;
1335 vBestNormal[0]=0;
1336 vBestNormal[1]=0;
1337 vBestNormal[2]=0;
1338
1339
1340//*****at first , collide box aabb******//
1341
1342 GIM_TRIMESH * ptrimesh = &TriMesh->m_collision_trimesh;
1343 aabb3f test_aabb;
1344
1345 test_aabb.minX = BoxGeom->aabb[0];
1346 test_aabb.maxX = BoxGeom->aabb[1];
1347 test_aabb.minY = BoxGeom->aabb[2];
1348 test_aabb.maxY = BoxGeom->aabb[3];
1349 test_aabb.minZ = BoxGeom->aabb[4];
1350 test_aabb.maxZ = BoxGeom->aabb[5];
1351
1352 GDYNAMIC_ARRAY collision_result;
1353 GIM_CREATE_BOXQUERY_LIST(collision_result);
1354
1355 gim_aabbset_box_collision(&test_aabb, &ptrimesh->m_aabbset , &collision_result);
1356
1357 if(collision_result.m_size==0)
1358 {
1359 GIM_DYNARRAY_DESTROY(collision_result);
1360 return 0;
1361 }
1362//*****Set globals for box collision******//
1363
1364 //collide triangles
1365
1366 GUINT * boxesresult = GIM_DYNARRAY_POINTER(GUINT,collision_result);
1367 gim_trimesh_locks_work_data(ptrimesh);
1368
1369 int ctContacts0 = 0;
1370
1371 for(unsigned int i=0;i<collision_result.m_size;i++)
1372 {
1373 dVector3 dv[3];
1374
1375 int Triint = boxesresult[i];
1376 gim_trimesh_get_triangle_vertices(ptrimesh, Triint,dv[0],dv[1],dv[2]);
1377 // test this triangle
1378 _cldTestOneTriangle(dv[0],dv[1],dv[2]);
1379
1380 // fill-in tri index for generated contacts
1381 for (; ctContacts0<ctContacts; ctContacts0++)
1382 SAFECONTACT(iFlags, ContactGeoms, ctContacts0, iStride)->side1 = Triint;
1383
1384 /*
1385 NOTE by Oleh_Derevenko:
1386 The function continues checking triangles after maximal number
1387 of contacts is reached because it selects maximal penetration depths.
1388 See also comments in GenerateContact()
1389 */
1390 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
1391 if ((ctContacts | CONTACTS_UNIMPORTANT) == (iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
1392 break;
1393 }
1394
1395 gim_trimesh_unlocks_work_data(ptrimesh);
1396 GIM_DYNARRAY_DESTROY(collision_result);
1397
1398 return ctContacts;
1399}
1400#endif
1401
1402
1403// GenerateContact - Written by Jeff Smith (jeff@burri.to)
1404// Generate a "unique" contact. A unique contact has a unique
1405// position or normal. If the potential contact has the same
1406// position and normal as an existing contact, but a larger
1407// penetration depth, this new depth is used instead
1408//
1409static void
1410GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
1411 dxGeom* in_g1, dxGeom* in_g2,
1412 const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
1413 int& OutTriCount)
1414{
1415 /*
1416 NOTE by Oleh_Derevenko:
1417 This function is called after maximal number of contacts has already been
1418 collected because it has a side effect of replacing penetration depth of
1419 existing contact with larger penetration depth of another matching normal contact.
1420 If this logic is not necessary any more, you can bail out on reach of contact
1421 number maximum immediately in dCollideBTL(). You will also need to correct
1422 conditional statements after invocations of GenerateContact() in _cldClipping().
1423 */
1424 do
1425 {
1426 dContactGeom* Contact;
1427 dVector3 diff;
1428
1429 if (!(in_Flags & CONTACTS_UNIMPORTANT))
1430 {
1431 bool duplicate = false;
1432 for (int i=0; i<OutTriCount; i++)
1433 {
1434 Contact = SAFECONTACT(in_Flags, in_Contacts, i, in_Stride);
1435
1436 // same position?
1437 for (int j=0; j<3; j++)
1438 diff[j] = in_ContactPos[j] - Contact->pos[j];
1439 if (dDOT(diff, diff) < dEpsilon)
1440 {
1441 // same normal?
1442 if (dFabs(dDOT(in_Normal, Contact->normal)) > (REAL(1.0)-dEpsilon))
1443 {
1444 if (in_Depth > Contact->depth)
1445 Contact->depth = in_Depth;
1446 duplicate = true;
1447 /*
1448 NOTE by Oleh_Derevenko:
1449 There may be a case when two normals are close to each other but not duplicate
1450 while third normal is detected to be duplicate for both of them.
1451 This is the only reason I can think of, there is no "break" statement.
1452 Perhaps author considered it to be logical that the third normal would
1453 replace the depth in both of initial contacts.
1454 However, I consider it a questionable practice which should not
1455 be applied without deep understanding of underlaying physics.
1456 Even more, is this situation with close normal triplet acceptable at all?
1457 Should not be two initial contacts reduced to one (replaced with the latter)?
1458 If you know the answers for these questions, you may want to change this code.
1459 See the same statement in GenerateContact() of collision_trimesh_trimesh.cpp
1460 */
1461 }
1462 }
1463 }
1464 if (duplicate || OutTriCount == (in_Flags & NUMC_MASK))
1465 {
1466 break;
1467 }
1468 }
1469 else
1470 {
1471 dIASSERT(OutTriCount < (in_Flags & NUMC_MASK));
1472 }
1473
1474 // Add a new contact
1475 Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride);
1476
1477 Contact->pos[0] = in_ContactPos[0];
1478 Contact->pos[1] = in_ContactPos[1];
1479 Contact->pos[2] = in_ContactPos[2];
1480 Contact->pos[3] = 0.0;
1481
1482 Contact->normal[0] = in_Normal[0];
1483 Contact->normal[1] = in_Normal[1];
1484 Contact->normal[2] = in_Normal[2];
1485 Contact->normal[3] = 0.0;
1486
1487 Contact->depth = in_Depth;
1488
1489 Contact->g1 = in_g1;
1490 Contact->g2 = in_g2;
1491
1492 OutTriCount++;
1493 }
1494 while (false);
1495}
1496
1497#endif // dTRIMESH_ENABLED
generated by cgit v1.1 at 2024-12-26 23:22:58 +0000