1 /*************************************************************************
2 * *
3 * Tokamak Physics Engine, Copyright (C) 2002-2007 David Lam. *
4 * All rights reserved. Email: david@tokamakphysics.com *
5 * Web: www.tokamakphysics.com *
6 * *
7 * This library is distributed in the hope that it will be useful, *
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
10 * LICENSE.TXT for more details. *
11 * *
12 *************************************************************************/
13
14 #include "tokamak.h"
15 #include "containers.h"
16 #include "scenery.h"
17 #include "collision.h"
18 #include "collision2.h"
19 #include "dcd.h"
20 //#include "rigidbody.h"
21
22 #include <assert.h>
23 #include <stdio.h>
24
25 extern s32 currentMicroStep;
26
27 //extern void DrawLine(const neV3 & colour, neV3 * startpoint, s32 count);
28
29 /****************************************************************************
30 *
31 * Box2TriangleTest
32 *
33 ****************************************************************************/
34
Box2TriangleTest(neCollisionResult & result,TConvex & convexA,neT3 & transA,TConvex & convexB,neT3 & transB)35 void Box2TriangleTest(neCollisionResult & result, TConvex & convexA, neT3 & transA, TConvex & convexB, neT3 & transB)
36 {
37 ConvexTestResult res;
38
39 BoxTestParam boxParamA;
40
41 boxParamA.convex = &convexA;
42 boxParamA.trans = &transA;
43 boxParamA.radii[0] = transA.rot[0] * convexA.as.box.boxSize[0];
44 boxParamA.radii[1] = transA.rot[1] * convexA.as.box.boxSize[1];
45 boxParamA.radii[2] = transA.rot[2] * convexA.as.box.boxSize[2];
46
47 TriangleParam triParam;
48
49 triParam.vert[0] = transB * convexB.vertices[convexB.as.tri.indices[0]];
50 triParam.vert[1] = transB * convexB.vertices[convexB.as.tri.indices[1]];
51 triParam.vert[2] = transB * convexB.vertices[convexB.as.tri.indices[2]];
52
53 triParam.edges[0] = triParam.vert[1] - triParam.vert[0];
54 triParam.edges[1] = triParam.vert[2] - triParam.vert[1];
55 triParam.edges[2] = triParam.vert[0] - triParam.vert[2];
56 triParam.normal = triParam.edges[1].Cross(triParam.edges[0]);
57 triParam.normal.Normalize();
58 triParam.d = triParam.normal.Dot(triParam.vert[0]);
59
60 if (boxParamA.TriTest(res, triParam))
61 {
62 result.penetrate = true;
63
64 result.depth = res.depth;
65
66 // result.collisionFrame[0] = res.contactX;
67 // result.collisionFrame[1] = res.contactY;
68 result.collisionFrame[2] = res.contactNormal;
69
70 if (res.isEdgeEdge)
71 {
72 result.contactA = res.contactA;
73
74 result.contactB = res.contactB;
75 }
76 else
77 {
78 result.contactA = res.contactA;
79
80 result.contactB = res.contactA;
81 }
82 }
83 else
84 {
85 result.penetrate = false;
86 }
87 }
88
PointInYProjection(neV3 & point)89 NEINLINE bool TriangleParam::PointInYProjection(neV3 & point)
90 {
91 f32 sign1, sign2;
92
93 neV3 line1 = point - vert[0];
94
95 neV3 line2 = point - vert[1];
96
97 sign1 = line1[2] * edges[2][0] - line1[0] * edges[2][2];
98
99 sign2 = line2[2] * edges[1][0] - line2[0] * edges[1][2];
100
101 f32 mul = sign1 * sign2;
102
103 if (mul < 0.0f)
104 return false;
105
106 f32 sign3 = line1[2] * edges[0][0] - line1[0] * edges[0][2];
107
108 mul = sign1 * sign3;
109
110 if (mul < 0.0f)
111 return false;
112
113 return true;
114 /*
115 if (normal[1] > 0.0f)
116 {
117 return (sign1 < 0.0f);
118 }
119 else
120 {
121 return (sign1 > 0.0f);
122 }
123 */
124 }
125
IsPointInside(const neV3 & point)126 s32 TriangleParam::IsPointInside(const neV3 & point)
127 {
128 //select coordinate
129 s32 dim0, dim1, plane;
130 f32 clockness; // 1.0 counter clockwise, -1.0 clockwise
131
132 if (neAbs(normal[1]) > neAbs(normal[2]))
133 {
134 if (neAbs(normal[1]) > neAbs(normal[0])) //use y plane
135 {
136 plane = 1;
137 dim0 = 2;//0;
138 dim1 = 0;//2;
139 }
140 else //use x plane
141 {
142 plane = 0;
143 dim0 = 1;
144 dim1 = 2;
145 }
146 }
147 else if (neAbs(normal[2]) > neAbs(normal[0])) //use z plane
148 {
149 plane = 2;
150 dim0 = 0;
151 dim1 = 1;
152 }
153 else //use x plane
154 {
155 plane = 0;
156 dim0 = 1;
157 dim1 = 2;
158 }
159
160 clockness = normal[plane] > 0.0f ? 1.0f : -1.0f;
161
162 f32 det0, det1, det2;
163
164 #define pointA (vert[0])
165 #define pointB (vert[1])
166 #define pointC (vert[2])
167
168 det0 = (point[dim0] - pointA[dim0]) * (pointA[dim1] - pointB[dim1]) +
169 (pointA[dim1] - point[dim1]) * (pointA[dim0] - pointB[dim0]);
170
171 det1 = (point[dim0] - pointB[dim0]) * (pointB[dim1] - pointC[dim1]) +
172 (pointB[dim1] - point[dim1]) * (pointB[dim0] - pointC[dim0]);
173
174 det2 = (point[dim0] - pointC[dim0]) * (pointC[dim1] - pointA[dim1]) +
175 (pointC[dim1] - point[dim1]) * (pointC[dim0] - pointA[dim0]);
176
177 s32 ret;
178
179 if (det0 > 0.0f)
180 {
181 if (det1 > 0.0f)
182 {
183 if (det2 > 0.0f)
184 {
185 ret = -1; // inside
186 }
187 else
188 {
189 ret = 5; // outside edge 2
190 }
191 }
192 else
193 {
194 if (det2 > 0.0f)
195 {
196 ret = 3; // outside edge 1
197 }
198 else
199 {
200 ret = 4; // outside vertex 2
201 }
202 }
203 }
204 else
205 {
206 if (det1 > 0.0f)
207 {
208 if (det2 > 0.0f)
209 {
210 ret = 1; // outside edge 0
211 }
212 else
213 {
214 ret = 0; // outside vertex 0
215 }
216 }
217 else
218 {
219 if (det2 > 0.0f)
220 {
221 ret = 2; // outside vertex 1
222 }
223 else
224 {
225 ret = -1; // inside
226 }
227 }
228 }
229
230 if (ret == -1)
231 return ret;
232
233 if (clockness == -1.0f)
234 {
235 ret = (ret + 3) % 6;
236 }
237
238 return ret;
239
240 /*
241 if (det0 > 0.0f && det1 > 0.0f && det2 > 0.0f)
242 return true;
243
244 if (det0 < 0.0f && det1 < 0.0f && det2 < 0.0f)
245 return true;
246
247 return false;
248 */
249 }
250
ConputeExtraInfo()251 void TriangleParam::ConputeExtraInfo()
252 {
253 s32 i;
254
255 for (i = 0; i < 3; i++)
256 {
257 edgeNormals[i] = normal.Cross(edges[i]);
258
259 edgeNormals[i].Normalize();
260
261 neV3 diff = vert[neNextDim2[i]] - vert[i];
262
263 if (diff.Dot(edgeNormals[i]) > 0.0f)
264 {
265 edgeNormals[i] *= -1.0f;
266 }
267 }
268 for (i = 0; i < 3; i++)
269 {
270 vertNormals[i] = edgeNormals[i] + edgeNormals[neNextDim2[i]];
271
272 vertNormals[i].Normalize();
273 }
274 }
275
Transform(const TriangleParam & from,neT3 & trans)276 void TriangleParam::Transform(const TriangleParam & from, neT3 & trans)
277 {
278 s32 i;
279
280 for (i = 0; i < 3; i++)
281 {
282 vert[i] = trans * from.vert[i];
283 }
284 for (i = 0; i < 3; i++)
285 {
286 edges[i] = vert[neNextDim1[i]] - vert[i];
287 }
288 normal = trans.rot * from.normal;
289
290 d = normal.Dot(vert[i]);
291 }
292
TriHeightTest(ConvexTestResult & result,TriangleParam & tri)293 bool BoxTestParam::TriHeightTest(ConvexTestResult & result, TriangleParam & tri)
294 {
295 if (!isVertCalc)
296 CalcVertInWorld();
297
298 f32 deepest = 0.0f;
299
300 bool found = false;
301
302 for (s32 i = 0; i < 8; i++)
303 {
304 if (!tri.PointInYProjection(verts[i])) // vert in tri projection
305 continue;
306
307 f32 height = tri.d - tri.normal[0] * verts[i][0] - tri.normal[2] * verts[i][2];
308
309 height /= tri.normal[1];
310
311 f32 penetrate = height - verts[i][1];
312
313 if (penetrate > deepest)
314 {
315 deepest = penetrate;
316
317 result.depth = penetrate;
318
319 result.contactA = verts[i];
320
321 result.contactB = verts[i];
322
323 result.contactB[1] = height;//verts[i][1] + penetrate;
324
325 result.valid = true;
326
327 result.contactNormal = tri.normal;
328
329 found = true;
330 }
331 }
332 return found;
333 }
334
TriTest(ConvexTestResult & result,TriangleParam & tri)335 bool BoxTestParam::TriTest(ConvexTestResult & result, TriangleParam & tri)
336 {
337 result.depth = 1.e5f;
338 result.isEdgeEdge = false;
339 result.valid = false;
340
341 if (!MeasurePlanePenetration(result, tri.normal, tri.d))
342 return false;
343
344 ConvexTestResult result0;
345
346 result0.valid = false;
347 result0.depth = result.depth;
348
349 if (MeasureBoxFaceTrianglePenetration(result0, tri, 0) &&
350 MeasureBoxFaceTrianglePenetration(result0, tri, 1) &&
351 MeasureBoxFaceTrianglePenetration(result0, tri, 2)
352 )
353 {
354 if (result0.valid)
355 {
356 result = result0;
357 }
358 }
359 else
360 return false;
361
362 ConvexTestResult result2;
363
364 result2.valid = false;
365
366 result2.depth = result.depth;
367
368 bool edgeCollided = false;
369
370 if (!MeasureBoxEdgeTriangleEdgePenetration(result2, tri, 0, 0))
371 return false;
372 if (!MeasureBoxEdgeTriangleEdgePenetration(result2, tri, 0, 1))
373 return false;
374 if (!MeasureBoxEdgeTriangleEdgePenetration(result2, tri, 0, 2))
375 return false;
376 if (!MeasureBoxEdgeTriangleEdgePenetration(result2, tri, 1, 0))
377 return false;
378 if (!MeasureBoxEdgeTriangleEdgePenetration(result2, tri, 1, 1))
379 return false;
380 if (!MeasureBoxEdgeTriangleEdgePenetration(result2, tri, 1, 2))
381 return false;
382 if (!MeasureBoxEdgeTriangleEdgePenetration(result2, tri, 2, 0))
383 return false;
384 if (!MeasureBoxEdgeTriangleEdgePenetration(result2, tri, 2, 1))
385 return false;
386 if (!MeasureBoxEdgeTriangleEdgePenetration(result2, tri, 2, 2))
387 return false;
388
389 if (result2.valid)
390 edgeCollided = true;
391
392 if (edgeCollided)
393 {
394 ConvexTestResult result3;
395
396 if (result2.ComputerEdgeContactPoint(result3))
397 {
398 result.isEdgeEdge = true;
399 result.contactA = result3.contactA;
400 result.contactB = result3.contactB;
401 result.depth = result2.depth;
402 //result.contactX = result2.contactX;// * -1.0f;
403 //result.contactY = result2.contactY;
404 result.contactNormal = result2.contactNormal;// * -1.0f;
405 }
406 else
407 {
408 return result.valid;
409 }
410 }
411 else
412 {
413 return result.valid;
414 }
415
416 return true;
417 }
418
MeasurePlanePenetration(ConvexTestResult & result,const neV3 & normal,f32 d)419 NEINLINE bool BoxTestParam::MeasurePlanePenetration(ConvexTestResult & result, const neV3 & normal, f32 d)
420 {
421 f32 dot = normal.Dot(trans->pos);
422
423 f32 penetrated = dot - d;
424
425 neV3 contactPoint = trans->pos;
426
427 neV3 contactNormal;
428
429 if (penetrated < 0.0f)
430 {
431 contactNormal = normal * -1.0f;
432 }
433 else
434 {
435 contactNormal = normal;
436 penetrated *= -1.0f;
437 }
438 neV3 progression = contactNormal * radii;
439
440 neV3 sign;
441
442 sign[0] = progression[0] > 0.0f ? 1.0f: -1.0f;
443 sign[1] = progression[1] > 0.0f ? 1.0f: -1.0f;
444 sign[2] = progression[2] > 0.0f ? 1.0f: -1.0f;
445
446 penetrated += (progression[0] * sign[0]);
447 penetrated += (progression[1] * sign[1]);
448 penetrated += (progression[2] * sign[2]);
449
450 contactPoint -= (radii[0] * sign[0]);
451 contactPoint -= (radii[1] * sign[1]);
452 contactPoint -= (radii[2] * sign[2]);
453
454 if (penetrated < 0.0f)
455 return false;
456
457 if (penetrated < result.depth)
458 {
459 result.depth = penetrated;
460 result.contactA = contactPoint;
461 result.contactB = contactPoint + contactNormal * penetrated;//need to project point onto triangle face
462 result.valid = true;
463 result.contactNormal = contactNormal;
464 //ChooseAxis(result.contactX, result.contactY, result.contactNormal);
465 }
466 return true;
467 }
468
MeasureBoxFaceTrianglePenetration(ConvexTestResult & result,TriangleParam & tri,s32 whichFace)469 bool BoxTestParam::MeasureBoxFaceTrianglePenetration(ConvexTestResult & result, TriangleParam & tri, s32 whichFace)
470 {
471 neV3 contactNormal = trans->rot[whichFace];
472
473 f32 triMin;
474 f32 triMax;
475 s32 minVert = 0;
476 s32 maxVert = 0;
477
478 triMin = triMax = contactNormal.Dot(tri.vert[0]);
479
480 f32 dot = contactNormal.Dot(tri.vert[1]);
481
482 if (dot < triMin)
483 {
484 triMin = dot;
485 minVert = 1;
486 }
487 else if (dot > triMax)
488 {
489 triMax = dot;
490 maxVert = 1;
491 }
492 dot = contactNormal.Dot(tri.vert[2]);
493
494 if (dot < triMin)
495 {
496 triMin = dot;
497 minVert = 2;
498 }
499 else if (dot > triMax)
500 {
501 triMax = dot;
502 maxVert = 2;
503 }
504 f32 p = trans->pos.Dot(contactNormal);
505 f32 boxMin = p - convex->as.box.boxSize[whichFace];
506 f32 boxMax = p + convex->as.box.boxSize[whichFace];
507
508 if (triMin >= boxMax)
509 return false;
510
511 if (triMax <= boxMin)
512 return false;
513
514 f32 d1 = boxMax - triMin;
515 f32 d2 = triMax - boxMin;
516
517 f32 penetrated;
518 neV3 contactPoint;
519 bool reverse = false;
520
521 if (d1 < d2)
522 {
523 penetrated = d1;
524 contactNormal *= -1.0f;
525 contactPoint = tri.vert[minVert];
526 reverse = true;
527 }
528 else
529 {
530 penetrated = d2;
531 contactPoint = tri.vert[maxVert];
532 }
533 if (penetrated < result.depth)
534 {
535 s32 otherAxis1 = (whichFace + 1) % 3;
536
537 s32 otherAxis2 = (whichFace + 2) % 3;
538
539 result.depth = penetrated;
540 result.contactA = contactPoint;
541 result.contactB = contactPoint + contactNormal * penetrated;
542 result.valid = true;
543 result.contactNormal = contactNormal;
544 if (reverse)
545 result.contactX = trans->rot[otherAxis1] * -1.0f;
546 else
547 result.contactX = trans->rot[otherAxis1];
548
549 result.contactY = trans->rot[otherAxis2];
550 }
551 return true;
552 }
553
MeasureBoxEdgeTriangleEdgePenetration(ConvexTestResult & result,TriangleParam & tri,s32 dim1,s32 dim2)554 bool BoxTestParam::MeasureBoxEdgeTriangleEdgePenetration(ConvexTestResult & result, TriangleParam & tri, s32 dim1, s32 dim2)
555 {
556 neV3 edgeNormal = tri.edges[dim2];
557
558 edgeNormal.Normalize();
559
560 if (edgeNormal.IsConsiderZero())
561 return true;
562
563 neV3 contactNormal = trans->rot[dim1].Cross(edgeNormal);
564
565 if (contactNormal.IsConsiderZero())
566 return true;
567
568 contactNormal.Normalize(); // do we need this?
569
570 if (contactNormal.IsConsiderZero())
571 return true;
572
573 neV3 contactPoint = trans->pos;
574
575 s32 otherAxis1 = (dim1 + 1) % 3;
576 s32 otherAxis2 = (dim1 + 2) % 3;
577
578 f32 p = contactNormal.Dot(contactPoint);
579
580 f32 dot1,dot2;
581 f32 sign1, sign2;
582
583 dot1 = contactNormal.Dot(radii[otherAxis1]);
584 dot2 = contactNormal.Dot(radii[otherAxis2]);
585
586 f32 boxMin, boxMax;
587
588 sign1 = dot1 < 0.0f ? -1.0f : 1.0f;
589 sign2 = dot2 < 0.0f ? -1.0f : 1.0f;
590
591 boxMax = p + dot1 * sign1;
592 boxMax += dot2 * sign2;
593
594 boxMin = p - dot1 * sign1;
595 boxMin -= dot2 * sign2;
596
597 f32 triMin;
598 f32 triMax;
599
600 f32 q = contactNormal.Dot(tri.vert[dim2]);
601 f32 r = contactNormal.Dot(tri.vert[(dim2+2)%3]);
602
603 if (q < r)
604 {
605 triMin = q;
606 triMax = r;
607 }
608 else
609 {
610 triMin = r;
611 triMax = q;
612 }
613
614 if (triMin >= boxMax || triMax <= boxMin)
615 return false;
616
617 f32 penetrated;
618
619 if (triMin == q)
620 {
621 contactNormal = contactNormal * -1.0f;
622 penetrated = boxMax - triMin;
623
624 contactPoint += (radii[otherAxis1] * sign1);
625 contactPoint += (radii[otherAxis2] * sign2);
626 }
627 else
628 {
629 penetrated = triMax - boxMin;
630
631 contactPoint -= (radii[otherAxis1] * sign1);
632 contactPoint -= (radii[otherAxis2] * sign2);
633 }
634 if (penetrated < result.depth)
635 {
636 result.depth = penetrated;
637 result.contactA = contactPoint;
638 result.contactB = contactPoint;
639 result.valid = true;
640 result.contactNormal = contactNormal;
641 //ChooseAxis(result.contactX, result.contactY, contactNormal);
642
643 result.edgeA[0] = contactPoint + radii[dim1];
644 result.edgeA[1] = contactPoint - radii[dim1];
645
646 result.edgeB[0] = tri.vert[dim2];
647 result.edgeB[1] = tri.vert[(dim2+1)%3];
648 }
649 return true;
650 }
651
652 /****************************************************************************
653 *
654 * Box2TerrainTest
655 *
656 ****************************************************************************/
657
658 //static s32 callCnt = 0;
659
Box2TerrainTest(neCollisionResult & result,TConvex & convexA,neT3 & transA,TConvex & convexB)660 void Box2TerrainTest(neCollisionResult & result, TConvex & convexA, neT3 & transA, TConvex & convexB)
661 {
662 // Convex2TerrainTest(result, convexA, transA, convexB);
663
664 // return;
665
666 neSimpleArray<s32> & _triIndex = *convexB.as.terrain.triIndex;
667
668 s32 triangleCount = _triIndex.GetUsedCount();
669
670 neArray<neTriangle_> & triangleArray = *convexB.as.terrain.triangles;
671
672 ConvexTestResult res[2];
673
674 BoxTestParam boxParamA;
675
676 boxParamA.convex = &convexA;
677 boxParamA.trans = &transA;
678 boxParamA.radii[0] = transA.rot[0] * convexA.as.box.boxSize[0];
679 boxParamA.radii[1] = transA.rot[1] * convexA.as.box.boxSize[1];
680 boxParamA.radii[2] = transA.rot[2] * convexA.as.box.boxSize[2];
681
682 s32 finalTriIndex = -1;
683 s32 currentRes = 1;
684 s32 testRes = 0;
685
686 res[currentRes].depth = -1.0e6f;
687 res[currentRes].valid = false;
688 res[testRes].depth = 1.0e6f;
689
690 s32 terrainMatID = 0;
691
692 u32 userData = 0;
693 /*
694 callCnt++;
695
696 if (callCnt == 21)
697 ASSERT(0);
698 */
699 for (s32 i = 0; i < triangleCount; i++)
700 {
701 s32 test = _triIndex[i];
702
703 neTriangle_ * t = &triangleArray[_triIndex[i]];
704
705 TriangleParam triParam;
706
707 triParam.vert[0] = convexB.vertices[t->indices[0]];
708 triParam.vert[1] = convexB.vertices[t->indices[1]];
709 triParam.vert[2] = convexB.vertices[t->indices[2]];
710
711 triParam.edges[0] = triParam.vert[1] - triParam.vert[0];
712 triParam.edges[1] = triParam.vert[2] - triParam.vert[1];
713 triParam.edges[2] = triParam.vert[0] - triParam.vert[2];
714 triParam.normal = triParam.edges[0].Cross(triParam.edges[1]);
715 triParam.normal.Normalize();
716 triParam.d = triParam.normal.Dot(triParam.vert[0]);
717
718 if (t->flag == neTriangle::NE_TRI_TRIANGLE)
719 {
720 if (boxParamA.TriTest(res[testRes], triParam))
721 {
722 if (res[testRes].depth > res[currentRes].depth)
723 {
724 s32 tmp = testRes;
725
726 testRes = currentRes;
727
728 currentRes = tmp;
729
730 terrainMatID = t->materialID;
731
732 finalTriIndex = _triIndex[i];
733
734 userData = t->userData;
735 }
736 }
737 }
738 else if (t->flag == neTriangle::NE_TRI_HEIGHT_MAP)
739 {
740 if (boxParamA.TriHeightTest(res[testRes], triParam))
741 {
742 if (res[testRes].depth > res[currentRes].depth)
743 {
744 s32 tmp = testRes;
745
746 testRes = currentRes;
747
748 currentRes = tmp;
749
750 terrainMatID = t->materialID;
751
752 finalTriIndex = _triIndex[i];
753
754 userData = t->userData;
755 }
756 }
757 }
758 else
759 {
760 ASSERT(0);
761 }
762 }
763 if (res[currentRes].valid)
764 {
765 /* {
766 neV3 points[4];
767 neV3 red;
768
769 neTriangle_ * t = &triangleArray[finalTriIndex];
770
771 points[0] = convexB.vertices[t->indices[0]];
772 points[1] = convexB.vertices[t->indices[1]];
773 points[2] = convexB.vertices[t->indices[2]];
774 points[3] = convexB.vertices[t->indices[0]];
775
776 DrawLine(red, points, 4);
777 }
778 */ result.penetrate = true;
779
780 result.depth = res[currentRes].depth;
781
782 result.convexB = (TConvex*)userData;
783
784 //result.collisionFrame[0] = res[currentRes].contactX;
785 //result.collisionFrame[1] = res[currentRes].contactY;
786 result.collisionFrame[2] = res[currentRes].contactNormal;
787
788 result.materialIdB = terrainMatID;
789
790 //if (res[currentRes].isEdgeEdge)
791 {
792 result.contactA = res[currentRes].contactA;
793
794 result.contactB = res[currentRes].contactB;
795 }
796 //else
797 //{
798 // result.contactA = res[currentRes].contactA;
799 //
800 // result.contactB = res[currentRes].contactB;
801 //}
802 }
803 else
804 {
805 result.penetrate = false;
806 }
807 }
808
809