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 "constraint.h"
20 #include "rigidbody.h"
21 #include "scenery.h"
22 #include "stack.h"
23 #include "simulator.h"
24 #include "message.h"
25 #include "dcd.h"
26
27 const s32 BOX_NUM_FACES = 6;
28
29 const s32 BOX_NUM_VERTS = 8;
30
31 const s32 BOX_NUM_EDGES = 12;
32
33 const s32 TRI_NUM_FACES = 2;
34
35 const s32 TRI_NUM_VERTS = 3;
36
37 const s32 TRI_NUM_EDGES = 3;
38
39
40 s32 _num_edge_test;
41
42 s32 _num_face_test;
43
44 static neByte _boxNeighbourFaces[][4] = {{2,3,4,5},{2,3,4,5},{0,1,4,5},{0,1,4,5},{0,1,2,3},{0,1,2,3}};
45 static neByte _boxNeighbourVerts[][4] = {{2,3,6,7},{0,1,4,5},{4,5,6,7},{0,1,2,3},{1,3,5,7},{0,2,4,6}};
46 static neByte _boxNeighbourEdges[][4] = {{0,1,2,3},{4,5,6,7},{0,4,8,9},{1,5,10,11},{2,8,6,10},{3,7,9,11}};
47 static neByte _boxVertNeighbourEdges[][4] = {{5,7,11,0xff},{5,6,10,0xff},{1,3,11,0xff},{1,2,10,0xff},{4,7,9,0xff},{4,6,8,0xff},{0,3,9,0xff},{0,2,8,0xff}};
48 static neV3 _boxNormals[BOX_NUM_FACES] = {{0,1,0,0},{0,-1,0,0},{1,0,0,0},{-1,0,0,0},{0,0,1,0},{0,0,-1,0}};
49 static neV3 _boxVertexPos0[BOX_NUM_VERTS] = {{-1,-1,-1,0},{-1,-1,1,0},{-1,1,-1,0},{-1,1,1,0},{1,-1,-1,0},{1,-1,1,0},{1,1,-1,0},{1,1,1,0}};
50 static neV3 _boxVertexPosP[BOX_NUM_VERTS];
51 static neV3 _boxVertexPosQ[BOX_NUM_VERTS];
52 static neBool _visited[100];
53
54 DCDFace BoxFaces[BOX_NUM_FACES] =
55 {
56 {_boxNeighbourFaces[0],_boxNeighbourVerts[0],_boxNeighbourEdges[0]}, //0
57 {_boxNeighbourFaces[1],_boxNeighbourVerts[1],_boxNeighbourEdges[1]}, //1
58 {_boxNeighbourFaces[2],_boxNeighbourVerts[2],_boxNeighbourEdges[2]}, //2
59 {_boxNeighbourFaces[3],_boxNeighbourVerts[3],_boxNeighbourEdges[3]}, //3
60 {_boxNeighbourFaces[4],_boxNeighbourVerts[4],_boxNeighbourEdges[4]}, //4
61 {_boxNeighbourFaces[5],_boxNeighbourVerts[5],_boxNeighbourEdges[5]}, //5
62 };
63
64 DCDVert BoxVertices[BOX_NUM_VERTS] =
65 {
66 {_boxVertNeighbourEdges[0],}, //0
67 {_boxVertNeighbourEdges[1],}, //1
68 {_boxVertNeighbourEdges[2],}, //2
69 {_boxVertNeighbourEdges[3],}, //3
70 {_boxVertNeighbourEdges[4],}, //4
71 {_boxVertNeighbourEdges[5],}, //5
72 {_boxVertNeighbourEdges[6],}, //6
73 {_boxVertNeighbourEdges[7],}, //7
74 };
75
76 DCDEdge BoxEdges[BOX_NUM_EDGES] =
77 {
78 {0,2,6,7}, //0
79 {0,3,2,3}, //1
80 {0,4,3,7}, //2
81 {0,5,2,6}, //3
82 {1,2,4,5}, //4
83 {1,3,0,1}, //5
84 {1,4,1,5}, //6
85 {1,5,0,4}, //7
86 {2,4,5,7}, //8
87 {2,5,4,6}, //9
88 {3,4,1,3}, //10
89 {3,5,0,2}, //11
90 };
91
92 static neByte _triNeigbhourFaces[TRI_NUM_FACES][1] = {{0}, {1}};
93 static neByte _triNeighbourVerts[TRI_NUM_FACES][3] = {{0,1,2},{0,1,2}};
94 static neByte _triNeighbourEdges[TRI_NUM_FACES][3] = {{0,1,2},{0,1,2}};
95 static neByte _triVertNeighbourEdges[TRI_NUM_VERTS][3] = {{0,2, 0xff},{0, 1, 0xff},{1, 2, 0xff}};
96
97 static neV3 _triNormals[TRI_NUM_FACES];
98 static neV3 _triVertexPos[3];
99
100 DCDFace TriFaces[TRI_NUM_FACES] =
101 {
102 {_triNeigbhourFaces[0],_triNeighbourVerts[0],_triNeighbourEdges[0]},
103 {_triNeigbhourFaces[1],_triNeighbourVerts[1],_triNeighbourEdges[1]},
104 };
105
106 DCDVert TriVertices[TRI_NUM_VERTS] =
107 {
108 {_triVertNeighbourEdges[0]},
109 {_triVertNeighbourEdges[1]},
110 {_triVertNeighbourEdges[2]},
111 };
112
113 DCDEdge TriEdges[TRI_NUM_EDGES] =
114 {
115 {0,1,0,1},
116 {0,1,1,2},
117 {0,1,2,0},
118 };
119
120 neV3 TriEdgeDir[TRI_NUM_EDGES];
121
SetConvex(const TConvex & convex,neV3 * vertArray)122 void DCDMesh::SetConvex(const TConvex & convex, neV3 * vertArray)
123 {
124 if (convex.type == TConvex::BOX)
125 {
126 numFaces = BOX_NUM_FACES;
127 numVerts = BOX_NUM_VERTS;
128 //numEdges = BOX_NUM_EDGES;
129 normals = _boxNormals;
130 faces = BoxFaces;
131 verts = BoxVertices;
132 edges = BoxEdges;
133 if (vertArray)
134 vertices = vertArray;
135
136 numNeighbour = 4;
137 }
138 else if (convex.type == TConvex::CONVEXDCD)
139 {
140 numFaces = *(int*)convex.as.convexDCD.convexData;
141 numVerts = *((int*)convex.as.convexDCD.convexData+1);
142 //numEdges = *((int*)convex.as.convexDCD.convexData+2);
143
144 f32 * np = (f32 *)(convex.as.convexDCD.convexData + 4 * sizeof(int));
145 normals = (neV3*)np;
146
147 vertices = (neV3*)(np + 4 * numFaces);
148 faces = (DCDFace*)(vertices + numVerts);
149 verts = (DCDVert*)((neByte*)faces + sizeof(DCDFace) * numFaces);
150 edges = (DCDEdge*)((neByte*)verts + sizeof(DCDVert) * numVerts);
151
152 numNeighbour = 3;
153 }
154 else if (convex.type == TConvex::TRIANGLE)
155 {
156 numFaces = TRI_NUM_FACES;
157 numVerts = TRI_NUM_VERTS;
158 normals = _triNormals;
159 vertices = _triVertexPos;
160 faces = TriFaces;
161 verts = TriVertices;
162 edges = TriEdges;
163 numNeighbour = 3;
164 }
165 }
GetVertOnFace(s32 faceIndex,s32 vertIndex)166 neV3 DCDMesh::GetVertOnFace(s32 faceIndex, s32 vertIndex)
167 {
168 return vertices[faces[faceIndex].neighbourVerts[vertIndex]];
169 }
GetVert(s32 vertIndex)170 neV3 DCDMesh::GetVert(s32 vertIndex)
171 {
172 return vertices[vertIndex];
173 }
GetNormal(s32 faceIndex)174 neV3 DCDMesh::GetNormal(s32 faceIndex)
175 {
176 return normals[faceIndex];
177 }
178 /*neByte DCDMesh::FaceGetNumFaceNeighbour(s32 faceIndex)
179 {
180 return faces[faceIndex].numberFaceNeighbour;
181 }
FaceGetFaceNeighbour(s32 faceIndex,s32 neighbourIndex)182 */neByte DCDMesh::FaceGetFaceNeighbour(s32 faceIndex, s32 neighbourIndex)
183 {
184 return faces[faceIndex].neighbourFaces[neighbourIndex];
185 }
FaceGetEdgeNeighbour(s32 faceIndex,s32 neighbourIndex)186 neByte DCDMesh::FaceGetEdgeNeighbour(s32 faceIndex, s32 neighbourIndex)
187 {
188 return faces[faceIndex].neighbourEdges[neighbourIndex];
189 }
190 /*neByte DCDMesh::VertGetNumEdgeNeighbour(s32 vertIndex)
191 {
192 return verts[vertIndex].numberEdgeNeighbour;
193 }
VertGetEdgeNeighbour(s32 vertIndex,s32 neighbourIndex)194 */neByte DCDMesh::VertGetEdgeNeighbour(s32 vertIndex, s32 neighbourIndex)
195 {
196 return verts[vertIndex].neighbourEdges[neighbourIndex];
197 }
EdgeGetVert1(s32 edgeIndex)198 neByte DCDMesh::EdgeGetVert1(s32 edgeIndex)
199 {
200 return edges[edgeIndex].v1;
201 }
EdgeGetVert2(s32 edgeIndex)202 neByte DCDMesh::EdgeGetVert2(s32 edgeIndex)
203 {
204 return edges[edgeIndex].v2;
205 }
206
207 const s32 NUM_STACK_SIZE = 200;
208
209 bool CalcContactEE(const neV3 & edgeA0,
210 const neV3 & edgeA1,
211 const neV3 & edgeB0,
212 const neV3 & edgeB1, neV3 & contactA, neV3 & contactB);
213
214 struct EdgeStackRecord
215 {
216 s32 edgeP;
217 s32 edgeQ;
218 };
219
220 class EdgeStack
221 {
222 public:
Init()223 void Init()
224 {
225 tos = 0;
226 }
Push(s32 edgeP,s32 edgeQ)227 void Push(s32 edgeP, s32 edgeQ)
228 {
229 ASSERT(tos < NUM_STACK_SIZE);
230
231 for (s32 i = 0; i < tos; i++)
232 {
233 if ((eStack[i].edgeP == edgeP && eStack[i].edgeQ == edgeQ) ||
234 (eStack[i].edgeP == edgeQ && eStack[i].edgeQ == edgeP))
235 return;
236 }
237 eStack[tos].edgeP = edgeP;
238 eStack[tos].edgeQ = edgeQ;
239 tos++;
240 }
241
Pop(s32 & edgeP,s32 & edgeQ)242 bool Pop(s32 & edgeP, s32 & edgeQ)
243 {
244 ASSERT(tos > 0);
245
246 tos--;
247 edgeP = eStack[tos].edgeP;
248 edgeQ = eStack[tos].edgeQ;
249 return true;
250 }
IsEmpty()251 neBool IsEmpty()
252 {
253 return tos == 0;
254 }
255 private:
256 s32 tos;
257 EdgeStackRecord eStack[NUM_STACK_SIZE];
258 };
259
260 EdgeStack gEdgeStack;
261
262 neV3 BigC;
263
264 f32 BigCLength;
265
266 class Face
267 {
268 public:
269 // face is defined as normal.Dot(p) = k
270 neV3 normal;
271 f32 k;
272 };
273
274 class DCDObj
275 {
276 public:
277 //TConvex * convex;
278 neBool isBox;
279
280 DCDMesh mesh;
281
282 neT3 * trans;
283
GetFace(s32 faceIndex)284 Face GetFace(s32 faceIndex)
285 {
286 Face face0;
287
288 face0.normal = trans->rot * mesh.normals[faceIndex];
289
290 neV3 tmp1 = (*trans) * mesh.GetVertOnFace(faceIndex, 0);
291
292 //face0.k = face0.normal.Dot(trans->pos) + mesh.normals[faceIndex].v[3] * -1.0f;
293
294 face0.k = face0.normal.Dot(tmp1);
295
296 return face0;
297 }
GetVertWorld(s32 vertIndex)298 neV3 GetVertWorld(s32 vertIndex)
299 {
300 neV3 vert = (*trans) * mesh.vertices[vertIndex];
301
302 return vert;
303 }
GetNegVertWorld(s32 vertIndex)304 neV3 GetNegVertWorld(s32 vertIndex)
305 {
306 neV3 vert = GetVertWorld(vertIndex) * -1.0f;
307
308 return vert;
309 }
GetWorldNormalByEdge1(s32 edgeIndex)310 NEINLINE neV3 GetWorldNormalByEdge1(s32 edgeIndex)
311 {
312 neV3 ret; ret = trans->rot * mesh.normals[mesh.edges[edgeIndex].f1];
313
314 return ret;
315 }
GetWorldNormalByEdge2(s32 edgeIndex)316 NEINLINE neV3 GetWorldNormalByEdge2(s32 edgeIndex)
317 {
318 neV3 ret; ret = trans->rot * mesh.normals[mesh.edges[edgeIndex].f2];
319
320 return ret;
321 }
GetSupportPoint(const neV3 & norm)322 NEINLINE s32 GetSupportPoint(const neV3 & norm)
323 {
324 if (isBox)
325 return GetSupportPointBox(norm);
326
327 else
328 return GetSupportPointMesh(norm);
329
330 return 0;
331 }
GetWorldEdgeVerts(s32 edgeIndex,neV3 & av,neV3 & bv)332 NEINLINE void GetWorldEdgeVerts(s32 edgeIndex, neV3 & av, neV3 & bv)
333 {
334 neV3 tmp;
335
336 tmp = mesh.vertices[mesh.edges[edgeIndex].v1];
337
338 av = (*trans) * tmp;
339
340 tmp = mesh.vertices[mesh.edges[edgeIndex].v2];
341
342 bv = (*trans) * tmp;
343 }
FaceGetWorldNormal(s32 faceIndex)344 neV3 FaceGetWorldNormal(s32 faceIndex)
345 {
346 return trans->rot * mesh.GetNormal(faceIndex);
347 }
348 private:
GetSupportPointBox(const neV3 & norm)349 s32 GetSupportPointBox(const neV3 & norm)
350 {
351 neV3 localNorm = trans->rot.TransposeMulV3(norm);
352
353 localNorm *= -1.0f;
354
355 s32 ret = 0;
356
357 if (localNorm[0] >= 0.0f)
358 {
359 if (localNorm[1] >= 0.0f)
360 {
361 if (localNorm[2] >= 0.0f)
362 ret = 7;
363 else
364 ret = 6;
365 }
366 else
367 {
368 if (localNorm[2] >= 0.0f)
369 ret = 5;
370 else
371 ret = 4;
372 }
373 }
374 else
375 {
376 if (localNorm[1] >= 0.0f)
377 {
378 if (localNorm[2] >= 0.0f)
379 ret = 3;
380 else
381 ret = 2;
382 }
383 else
384 {
385 if (localNorm[2] >= 0.0f)
386 ret = 1;
387 else
388 ret = 0;
389 }
390 }
391 return ret;
392 }
GetSupportPointMesh(const neV3 & norm)393 s32 GetSupportPointMesh(const neV3 & norm)
394 {
395 neV3 localNorm = trans->rot.TransposeMulV3(norm);
396
397 localNorm *= -1.0f;
398
399 s32 ret = 0;
400
401 f32 maxd = -1.0e6f;
402
403 neByte neighbourEdge;
404
405 neBool moving;
406
407 do {
408 moving = false;
409
410 s32 i = 0;
411
412 //while (i < mesh.verts[ret].numberEdgeNeighbour)
413 do
414 {
415 s32 currentVert;
416
417 neighbourEdge = mesh.verts[ret].neighbourEdges[i];
418
419 if (neighbourEdge == 0xff)
420 break;
421
422 if (mesh.edges[neighbourEdge].v1 == ret)
423 currentVert = mesh.edges[neighbourEdge].v2;
424 else
425 currentVert = mesh.edges[neighbourEdge].v1;
426
427 //if (currentVert > 10)
428 // ASSERT(0);
429
430 f32 dot = mesh.vertices[currentVert].Dot(localNorm);
431
432 if (dot > maxd)
433 {
434 maxd = dot;
435
436 ret = currentVert;
437
438 moving = 1;
439
440 break;
441 }
442
443 i++;
444 } while (true);
445 } while(moving);
446
447 return ret;
448 /*
449 for (s32 i = 0; i < mesh.numVerts; i++)
450 {
451 f32 dot = mesh.vertices[i].Dot(localNorm);
452
453 if (dot > maxd)
454 {
455 maxd = dot;
456
457 ret = i;
458 }
459 }
460 return ret;
461 */
462 }
463
464 };
465
funcD(const Face & face)466 f32 funcD(const Face & face)
467 {
468 f32 k = face.k;
469
470 neV3 N;
471
472 N = face.normal;
473
474 if (face.k < 0.0f)
475 {
476 k = -face.k;
477 N *= -1.0f;
478 }
479
480 f32 den = k - N.Dot(BigC);
481
482 den = BigCLength * den;
483
484 ASSERT(!neIsConsiderZero(den));
485
486 f32 ret = -k / den;
487
488 return ret;
489 }
490
SignedDistance(const Face & faceP,const neV3 & vertQ,Face & faceM)491 f32 SignedDistance(const Face & faceP, const neV3 & vertQ , Face & faceM)
492 {
493 faceM = faceP;
494
495 f32 dot = faceP.normal.Dot(vertQ);
496
497 faceM.k += (dot);
498
499 return funcD(faceM);
500 }
501
502 class SearchResult
503 {
504 public:
505 enum Type
506 {
507 FACE,
508 VERTEX,
509 EDGE,
510 } ;
511
SearchResult(const TConvex & convexA,neT3 * transA,const TConvex & convexB,neT3 * transB,neV3 * vertArrayA,neV3 * vertArrayB)512 SearchResult(const TConvex & convexA, neT3 * transA, const TConvex & convexB, neT3 * transB, neV3 * vertArrayA, neV3 * vertArrayB)
513 {
514 objA.mesh.SetConvex(convexA, vertArrayA);
515
516 objA.isBox = (convexA.type == TConvex::BOX);
517
518 objA.trans = transA;
519
520 objB.mesh.SetConvex(convexB, vertArrayB);
521
522 objB.isBox = (convexB.type == TConvex::BOX);
523
524 objB.trans = transB;
525
526 dMax = -1.0e6f;
527 }
TestFace(s32 face0Index,neBool & assigned)528 neBool TestFace(s32 face0Index, neBool & assigned)
529 {
530 assigned = false;
531
532 _visited[face0Index] = true;
533
534 Face face0 = objA.GetFace(face0Index);
535
536 neByte _indexB = objB.GetSupportPoint(face0.normal);
537
538 neV3 vertB;
539
540 vertB = objB.GetNegVertWorld(_indexB);
541
542 Face newFace;
543
544 f32 d = SignedDistance(face0, vertB, newFace);
545
546 if (d >= 0.0f)
547 return false;
548
549 if (d <= dMax)
550 return true;
551
552 dMax = d;
553
554 typeA = SearchResult::FACE;
555
556 typeB = SearchResult::VERTEX;
557
558 indexA = face0Index;
559
560 indexB = _indexB;
561
562 face = newFace;
563
564 assigned = true;
565
566 _num_face_test++;
567
568 return true;
569 }
570 neBool SearchFV(s32 initialFace, neBool & assigned);
571
572 neBool SearchEE(s32 flag /*0 or 1*/, s32 aIndex, s32 bIndex, neBool & assigned);
573
574 neBool SearchEETri(s32 flag /*0 or 1*/, s32 aIndex, s32 bIndex, neBool & assigned);
575
576 DCDObj objA;
577
578 Type typeA;
579
580 s32 indexA;
581
582 DCDObj objB;
583
584 Type typeB;
585
586 s32 indexB;
587
588 Face face;
589
590 f32 dMax;
591 };
592
SearchFV(s32 initialFace,neBool & assigned)593 neBool SearchResult::SearchFV(s32 initialFace, neBool & assigned)
594 {
595 for (s32 i = 0; i < objA.mesh.numFaces; i++)
596 {
597 _visited[i] = false;
598
599 if (objA.isBox)
600 {
601 objA.mesh.normals[i].v[3] = objA.mesh.vertices[objA.mesh.faces[i].neighbourVerts[0]].Dot(objA.mesh.normals[i]) * -1.0f;
602 }
603 }
604
605 if (!TestFace(initialFace, assigned))
606 return false;
607
608 //ASSERT(assigned);
609
610 neBool found = true;
611
612 s32 currentFace = initialFace;
613
614 while (found)
615 {
616 found = false;
617
618 for (s32 ii = 0; ii < objA.mesh.numNeighbour; ii++)
619 {
620 s32 i = objA.mesh.FaceGetFaceNeighbour(currentFace, ii);
621
622 if (_visited[i])
623 continue;
624
625 neBool _assigned;
626
627 if (!TestFace(i, _assigned))
628 return false;
629
630 if (_assigned)
631 found = true;
632 }
633 if (found)
634 {
635 currentFace = indexA;
636 }
637 }
638 return true;
639 }
640
Determinant(const neV3 & a,const neV3 & b,const neV3 & c)641 f32 Determinant(const neV3 & a, const neV3 & b, const neV3 & c)
642 {
643 f32 t1 = a[0] * b[1] * c[2];
644
645 f32 t2 = a[1] * b[0] * c[2];
646
647 f32 t3 = a[0] * b[2] * c[1];
648
649 f32 t4 = a[2] * b[1] * c[0];
650
651 f32 t5 = a[1] * b[2] * c[0];
652
653 f32 t6 = a[2] * b[0] * c[1];
654
655 f32 ret = t1 - t2 - t3 - t4 + t5 + t6;
656
657 return ret;
658 }
659
SearchEE(s32 flag,s32 aIndex,s32 bIndex,neBool & assigned)660 neBool SearchResult::SearchEE(s32 flag, s32 aIndex, s32 bIndex, neBool & assigned)
661 {
662 assigned = false;
663
664 gEdgeStack.Init();
665
666 neByte edgeIndex;
667
668 if (flag == 0) //fv
669 {
670 for (s32 i = 0; i < objA.mesh.numNeighbour; i++)
671 {
672 int j = 0;
673
674 while ((edgeIndex = objB.mesh.VertGetEdgeNeighbour(bIndex, j)) != 0xff)
675 {
676 gEdgeStack.Push(objA.mesh.FaceGetEdgeNeighbour(aIndex, i),
677 objB.mesh.VertGetEdgeNeighbour(bIndex, j));
678
679 j++;
680 }
681 }
682 }
683 else //vf
684 {
685 s32 i = 0;
686
687 while ((edgeIndex = objA.mesh.VertGetEdgeNeighbour(aIndex, i)) != 0xff)
688 {
689 for (s32 j = 0; j < objB.mesh.numNeighbour; j++)
690 {
691 gEdgeStack.Push(objA.mesh.VertGetEdgeNeighbour(aIndex, i),
692 objB.mesh.FaceGetEdgeNeighbour(bIndex, j));
693 }
694 i++;
695 }
696 }
697 while (!gEdgeStack.IsEmpty())
698 {
699 _num_edge_test++;
700
701 s32 edgeP, edgeQ;
702
703 gEdgeStack.Pop(edgeP, edgeQ);
704
705 // does the edge form a face
706 neV3 a = objA.GetWorldNormalByEdge1(edgeP);
707
708 neV3 b = objA.GetWorldNormalByEdge2(edgeP);
709
710 neV3 c = objB.GetWorldNormalByEdge1(edgeQ) * -1.0f;
711
712 neV3 d = objB.GetWorldNormalByEdge2(edgeQ) * -1.0f;
713
714 f32 cba = Determinant(c,b,a);
715
716 f32 dba = Determinant(d,b,a);
717
718 f32 prod0 = cba * dba;
719
720 if (prod0 >= 0.0f/*-1.0e-6f*/)
721 {
722 continue;
723 }
724
725 f32 adc = Determinant(a,d,c);
726
727 f32 bdc = Determinant(b,d,c);
728
729 f32 prod1 = adc * bdc;
730
731 if (prod1 >= 0.0f/*-1.0e-6f*/)
732 {
733 continue;
734 }
735 f32 prod2 = cba * bdc;
736
737 if (prod2 <= 0.0f/*1.0e-6f*/)
738 {
739 continue;
740 }
741 neV3 ai, bi;
742 neV3 naj, nbj;
743
744 objA.GetWorldEdgeVerts(edgeP, ai, bi);
745
746 objB.GetWorldEdgeVerts(edgeQ, naj, nbj);
747
748 naj *= -1.0f; nbj *= -1.0f;
749
750 neV3 ainaj = ai + naj;
751 neV3 ainbj = ai + nbj;
752 neV3 binaj = bi + naj;
753 //neV3 binbj = bi + nbj;
754
755 neV3 diff1 = ainaj - ainbj;
756 neV3 diff2 = ainaj - binaj ;
757
758 Face testFace;
759
760 testFace.normal = diff1.Cross(diff2);
761
762 f32 len = testFace.normal.Length();
763
764 if (neIsConsiderZero(len))
765 {
766 continue;
767 }
768 testFace.normal *= (1.0f / len);
769
770 testFace.k = testFace.normal.Dot(ainaj);
771
772 f32 testD = funcD(testFace);
773
774 if (testD >= 0)
775 return false;
776
777
778 if (testD <= dMax)
779 continue;
780
781 assigned = true;
782 dMax = testD;
783 face = testFace;
784 indexA = edgeP;
785 indexB = edgeQ;
786 typeA = SearchResult::EDGE;
787 typeB = SearchResult::EDGE;
788
789 // push
790 s32 i, j;
791
792 s32 vindex;
793
794 vindex = objB.mesh.EdgeGetVert1(edgeQ);
795
796 i = 0;
797
798 while ((j = objB.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
799 {
800 if (j != edgeQ)
801 gEdgeStack.Push(edgeP, j);
802
803 i++;
804 }
805
806 vindex = objB.mesh.EdgeGetVert2(edgeQ);
807
808 i = 0;
809
810 while ((j = objB.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
811 {
812 if (j != edgeQ)
813 gEdgeStack.Push(edgeP, j);
814
815 i++;
816 }
817
818 vindex = objA.mesh.EdgeGetVert1(edgeP);
819
820 i = 0;
821
822 while((j = objA.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
823 {
824 if (j != edgeP)
825 gEdgeStack.Push(j, edgeQ);
826
827 i++;
828 }
829
830 vindex = objA.mesh.EdgeGetVert2(edgeP);
831
832 //for (i = 0; i < objA.mesh.VertGetNumEdgeNeighbour(vindex); i++)
833 i = 0;
834
835 while ((j = objA.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
836 {
837 if (j != edgeP)
838 gEdgeStack.Push(j, edgeQ);
839
840 i++;
841 }
842 /*
843 if (testD <= dMax)
844 continue;
845
846 assigned = true;
847 dMax = testD;
848 face = testFace;
849 indexA = edgeP;
850 indexB = edgeQ;
851 typeA = SearchResult::EDGE;
852 typeB = SearchResult::EDGE;
853 */ }
854 return true;
855 }
856
SearchEETri(s32 flag,s32 aIndex,s32 bIndex,neBool & assigned)857 neBool SearchResult::SearchEETri(s32 flag, s32 aIndex, s32 bIndex, neBool & assigned)
858 {
859 assigned = false;
860
861 gEdgeStack.Init();
862
863 neByte edgeIndex;
864
865 if (flag == 0) //fv
866 {
867 // face of convex A
868 // vertex of triangle B
869 for (s32 i = 0; i < objA.mesh.numNeighbour; i++) // for each edge neighbour of Face aIndex
870 {
871 int j = 0;
872
873 while ((edgeIndex = objB.mesh.VertGetEdgeNeighbour(bIndex, j)) != 0xff)
874 {
875 gEdgeStack.Push(objA.mesh.FaceGetEdgeNeighbour(aIndex, i),
876 objB.mesh.VertGetEdgeNeighbour(bIndex, j));
877
878 j++;
879 }
880 }
881 }
882 else //vf
883 {
884 //vertex of convex A
885 //face of triangle B
886 s32 i = 0;
887
888 //for each edge neighbour incident to Vertex aIndex
889
890 while ((edgeIndex = objA.mesh.VertGetEdgeNeighbour(aIndex, i)) != 0xff)
891 {
892 for (s32 j = 0; j < objB.mesh.numNeighbour; j++)
893 {
894 gEdgeStack.Push(objA.mesh.VertGetEdgeNeighbour(aIndex, i),
895 objB.mesh.FaceGetEdgeNeighbour(bIndex, j));
896 }
897 i++;
898 }
899 }
900 while (!gEdgeStack.IsEmpty())
901 {
902 _num_edge_test++;
903
904 s32 edgeP, edgeQ;
905
906 gEdgeStack.Pop(edgeP, edgeQ);
907
908 // does the edge form a face
909 neV3 a = objA.GetWorldNormalByEdge1(edgeP);
910
911 neV3 b = objA.GetWorldNormalByEdge2(edgeP);
912
913 neV3 c = objB.GetWorldNormalByEdge1(edgeQ) * -1.0f;
914
915 neV3 d = objB.GetWorldNormalByEdge2(edgeQ) * -1.0f;
916
917 c += (TriEdgeDir[edgeQ] * 0.01f);
918
919 d += (TriEdgeDir[edgeQ] * 0.01f);
920
921 c.Normalize();
922
923 d.Normalize();
924
925 f32 cba = Determinant(c,b,a);
926
927 f32 dba = Determinant(d,b,a);
928
929 f32 prod0 = cba * dba;
930
931 if (prod0 >= -1.0e-6f)
932 {
933 continue;
934 }
935
936 f32 adc = Determinant(a,d,c);
937
938 f32 bdc = Determinant(b,d,c);
939
940 f32 prod1 = adc * bdc;
941
942 if (prod1 >= -1.0e-6f)
943 {
944 continue;
945 }
946 f32 prod2 = cba * bdc;
947
948 if (prod2 <= 1.0e-6f)
949 {
950 continue;
951 }
952
953
954 neV3 ai, bi;
955 neV3 naj, nbj;
956
957 objA.GetWorldEdgeVerts(edgeP, ai, bi);
958
959 objB.GetWorldEdgeVerts(edgeQ, naj, nbj);
960
961 naj *= -1.0f; nbj *= -1.0f;
962
963 neV3 ainaj = ai + naj;
964 neV3 ainbj = ai + nbj;
965 neV3 binaj = bi + naj;
966 //neV3 binbj = bi + nbj;
967
968 neV3 diff1 = ainaj - ainbj;
969 neV3 diff2 = ainaj - binaj ;
970
971 Face testFace;
972
973 testFace.normal = diff1.Cross(diff2);
974
975 f32 len = testFace.normal.Length();
976
977 if (neIsConsiderZero(len))
978 {
979 continue;
980 }
981 testFace.normal *= (1.0f / len);
982
983 testFace.k = testFace.normal.Dot(ainaj);
984
985 f32 testD = funcD(testFace);
986
987 if (testD >= 0)
988 return false;
989
990 if (testD <= dMax)
991 continue;
992
993 assigned = true;
994 dMax = testD;
995 face = testFace;
996 indexA = edgeP;
997 indexB = edgeQ;
998 typeA = SearchResult::EDGE;
999 typeB = SearchResult::EDGE;
1000
1001 // push
1002 s32 i, j;
1003
1004 s32 vindex;
1005
1006 vindex = objB.mesh.EdgeGetVert1(edgeQ);
1007
1008 i = 0;
1009
1010 while ((j = objB.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
1011 {
1012 if (j != edgeQ)
1013 gEdgeStack.Push(edgeP, j);
1014
1015 i++;
1016 }
1017
1018 vindex = objB.mesh.EdgeGetVert2(edgeQ);
1019
1020 i = 0;
1021
1022 while ((j = objB.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
1023 {
1024 if (j != edgeQ)
1025 gEdgeStack.Push(edgeP, j);
1026
1027 i++;
1028 }
1029
1030 vindex = objA.mesh.EdgeGetVert1(edgeP);
1031
1032 i = 0;
1033
1034 while((j = objA.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
1035 {
1036 if (j != edgeP)
1037 gEdgeStack.Push(j, edgeQ);
1038
1039 i++;
1040 }
1041
1042 vindex = objA.mesh.EdgeGetVert2(edgeP);
1043
1044 //for (i = 0; i < objA.mesh.VertGetNumEdgeNeighbour(vindex); i++)
1045 i = 0;
1046
1047 while ((j = objA.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
1048 {
1049 if (j != edgeP)
1050 gEdgeStack.Push(j, edgeQ);
1051
1052 i++;
1053 }
1054 }
1055 return true;
1056 }
1057
TestDCD(neCollisionResult & result,TConvex & convexA,neT3 & transA,TConvex & convexB,neT3 & transB,const neV3 & backupVector)1058 bool TestDCD(neCollisionResult & result, TConvex & convexA, neT3 & transA, TConvex & convexB, neT3 & transB, const neV3 & backupVector)
1059 {
1060 _num_edge_test = 0;
1061
1062 _num_face_test = 0;
1063
1064 result.penetrate = false;
1065
1066 neV3 aPoint = transA.pos;
1067
1068 neV3 av; av.Set(0.1f);
1069
1070 aPoint += av;
1071
1072 neV3 bPoint = transB.pos;
1073
1074 av.Set(0.2f);
1075
1076 bPoint += av;
1077
1078 BigC = aPoint - bPoint;
1079
1080 BigCLength = BigC.Length();
1081
1082 neV3 * aVertArray, * bVertArray;
1083
1084 if (convexA.type == TConvex::BOX)
1085 {
1086 for (s32 i = 0; i < BOX_NUM_VERTS; i++)
1087 {
1088 _boxVertexPosP[i] = _boxVertexPos0[i] * convexA.as.box.boxSize;
1089 }
1090 aVertArray = _boxVertexPosP;
1091 }
1092 if (convexB.type == TConvex::BOX)
1093 {
1094 for (s32 i = 0; i < BOX_NUM_VERTS; i++)
1095 {
1096 _boxVertexPosQ[i] = _boxVertexPos0[i] * convexB.as.box.boxSize;
1097 }
1098 bVertArray = _boxVertexPosQ;
1099 }
1100
1101 SearchResult srFV(convexA, &transA, convexB, &transB, aVertArray, bVertArray);
1102
1103 neBool showDebug = 0;
1104
1105 neBool showDebug2 = (srFV.objA.mesh.numVerts > 8 && srFV.objB.mesh.numVerts > 8);
1106
1107 neBool assigned;
1108
1109 neBool res = srFV.SearchFV(0, assigned);
1110
1111 if (!res)
1112 {
1113 if (showDebug)
1114 {TOKAMAK_OUTPUT_2("%d, %d \n", _num_face_test, _num_edge_test);}
1115
1116 return false;
1117 }
1118 SearchResult srVF(convexB, &transB, convexA, &transA, bVertArray, aVertArray);
1119
1120 srVF.dMax = srFV.dMax;
1121
1122 BigC *= -1.0f;
1123
1124 s32 whichF = srFV.objB.mesh.edges[srFV.objB.mesh.verts[srFV.indexB].neighbourEdges[0]].f1;
1125
1126 res = srVF.SearchFV(whichF, assigned);
1127
1128 if (!res)
1129 {
1130 if (showDebug)
1131 {TOKAMAK_OUTPUT_2("%d, %d \n", _num_face_test, _num_edge_test);}
1132
1133 return false;
1134 }
1135
1136 bool need2Swap = false;
1137
1138 SearchResult srEE(convexA, &transA, convexB, &transB, aVertArray, bVertArray);
1139
1140 s32 eeflag = 0;
1141
1142 s32 pindex, qindex;
1143
1144 if (srVF.dMax > srFV.dMax)
1145 {
1146 need2Swap = true;
1147
1148 srEE.dMax = srVF.dMax;
1149
1150 eeflag = 1;
1151
1152 pindex = srVF.indexB;
1153
1154 qindex = srVF.indexA;
1155 }
1156 else
1157 {
1158 srEE.dMax = srFV.dMax;
1159
1160 pindex = srFV.indexA;
1161
1162 qindex = srFV.indexB;
1163 }
1164 BigC *= -1.0f;
1165
1166 if (!srEE.SearchEE(eeflag, pindex, qindex, assigned))
1167 {
1168 if (showDebug)
1169 {TOKAMAK_OUTPUT_2("%d, %d \n", _num_face_test, _num_edge_test);}
1170
1171 return false;
1172 }
1173 if (showDebug2)
1174 {
1175 TOKAMAK_OUTPUT_2("%d, %d \n", _num_face_test, _num_edge_test);
1176 }
1177 if (!assigned)
1178 {
1179 if (!need2Swap)
1180 {
1181 ASSERT(srFV.typeA == SearchResult::FACE && srFV.typeB == SearchResult::VERTEX);
1182
1183 result.penetrate = true;
1184
1185 result.collisionFrame[2] = srFV.face.normal * -1.0f;
1186
1187 result.depth = srFV.face.k;
1188
1189 result.contactB = srFV.objB.GetVertWorld(srFV.indexB);
1190
1191 result.contactA = result.contactB + srFV.face.normal * srFV.face.k;
1192 }
1193 else
1194 {
1195 ASSERT(srVF.typeA == SearchResult::FACE && srVF.typeB == SearchResult::VERTEX);
1196
1197 result.penetrate = true;
1198
1199 result.collisionFrame[2] = srVF.face.normal;
1200
1201 result.depth = srVF.face.k;
1202
1203 result.contactA = srVF.objB.GetVertWorld(srVF.indexB);
1204
1205 result.contactB = result.contactA + srVF.face.normal * srVF.face.k;
1206 }
1207 }
1208 else
1209 {
1210 ASSERT(srEE.typeA == SearchResult::EDGE &&
1211 srEE.typeB == SearchResult::EDGE);
1212
1213 neV3 edgeA[2];
1214 neV3 edgeB[2];
1215
1216 srEE.objA.GetWorldEdgeVerts(srEE.indexA, edgeA[0], edgeA[1]);
1217
1218 srEE.objB.GetWorldEdgeVerts(srEE.indexB, edgeB[0], edgeB[1]);
1219
1220 bool r = CalcContactEE(edgeA[0], edgeA[1], edgeB[0], edgeB[1], result.contactA, result.contactB);
1221
1222 if (r)
1223 {
1224 if (srEE.face.k > 0.0f)
1225 {
1226 result.collisionFrame[2] = srEE.face.normal * -1.0f;
1227
1228 result.depth = srEE.face.k;
1229 }
1230 else
1231 {
1232 result.collisionFrame[2] = srEE.face.normal;
1233
1234 result.depth = srEE.face.k * -1.0f;
1235 }
1236
1237 result.penetrate = true;
1238 }
1239 else
1240 {
1241 return false;
1242 }
1243 }
1244
1245 return true;
1246 }
1247
TestDCDTri(ConvexTestResult & res,TConvex & convexA,neT3 & transA,const neV3 & insidePoint)1248 bool TestDCDTri(ConvexTestResult & res, TConvex & convexA, neT3 & transA, const neV3 & insidePoint)
1249 {
1250 res.valid = false;
1251
1252 neV3 aPoint = transA.pos;
1253
1254 neV3 av; av.Set(0.1f);
1255
1256 aPoint += av;
1257
1258 neV3 bPoint = insidePoint;
1259
1260 BigC = aPoint - bPoint;
1261
1262 BigCLength = BigC.Length();
1263
1264 neV3 * aVertArray = NULL, * bVertArray = NULL;
1265
1266 TConvex dummyB; dummyB.type = TConvex::TRIANGLE;
1267
1268 if (convexA.type == TConvex::BOX)
1269 {
1270 for (s32 i = 0; i < BOX_NUM_VERTS; i++)
1271 {
1272 _boxVertexPosP[i] = _boxVertexPos0[i] * convexA.as.box.boxSize;
1273 }
1274 aVertArray = _boxVertexPosP;
1275 }
1276 neT3 transB; transB.SetIdentity();
1277
1278 SearchResult srBoxFaceTriVert(convexA, &transA, dummyB, &transB, aVertArray, bVertArray);
1279
1280 neBool assigned;
1281
1282 neBool r = srBoxFaceTriVert.SearchFV(0, assigned);
1283
1284 if (!r)
1285 {return false;}
1286
1287 /* bPoint = insidePoint;// + _triNormals[1];
1288 BigC = bPoint - aPoint;
1289 BigCLength = BigC.Length();
1290 */
1291 BigC *= -1.0f;
1292
1293 SearchResult srBoxVertTriFace(dummyB, &transB, convexA, &transA, bVertArray, aVertArray);
1294
1295 if (!(r = srBoxVertTriFace.TestFace(0, assigned)))
1296 return false;
1297
1298 //BigC *= -1.0f;
1299 /*
1300 bPoint = insidePoint + _triNormals[0];
1301 BigC = bPoint - aPoint;
1302 BigCLength = BigC.Length();
1303 */
1304 neBool assigned2;
1305
1306 if (!(r = srBoxVertTriFace.TestFace(1, assigned2)))
1307 return false;
1308
1309 assigned |= assigned2;
1310
1311 BigC *= -1.0f;
1312 /*
1313 bPoint = insidePoint;
1314 BigC = aPoint - bPoint;
1315 BigCLength = BigC.Length();
1316 */
1317
1318 neBool need2Swap = false;
1319
1320 SearchResult srBoxTriEE(convexA, &transA, dummyB, &transB, aVertArray, bVertArray);
1321
1322 s32 eeflag = 0;
1323
1324 s32 pindex, qindex;
1325
1326 if (srBoxVertTriFace.dMax > srBoxFaceTriVert.dMax)
1327 {
1328 need2Swap = true;
1329
1330 srBoxTriEE.dMax = srBoxVertTriFace.dMax;
1331
1332 eeflag = 1;
1333
1334 pindex = srBoxVertTriFace.indexB; // vertex of Convex
1335
1336 qindex = srBoxVertTriFace.indexA; // face of Triangle
1337 }
1338 else
1339 {
1340 srBoxTriEE.dMax = srBoxFaceTriVert.dMax;
1341
1342 pindex = srBoxFaceTriVert.indexA; // face of Convex
1343
1344 qindex = srBoxFaceTriVert.indexB; // vertex of Triangle
1345 }
1346 //BigC *= -1.0f;
1347
1348 if (!srBoxTriEE.SearchEETri(eeflag, pindex, qindex, assigned))
1349 {
1350 return false;
1351 }
1352 if (!assigned)
1353 {
1354 FV_Backup:
1355 if (!need2Swap)
1356 {
1357 ASSERT(srBoxFaceTriVert.typeA == SearchResult::FACE && srBoxFaceTriVert.typeB == SearchResult::VERTEX);
1358
1359 res.valid = true;
1360
1361 res.contactNormal = srBoxFaceTriVert.face.normal * -1.0f;
1362
1363 res.depth = srBoxFaceTriVert.face.k;
1364
1365 res.contactB = srBoxFaceTriVert.objB.GetVertWorld(srBoxFaceTriVert.indexB);
1366
1367 res.contactA = res.contactB + srBoxFaceTriVert.face.normal * srBoxFaceTriVert.face.k;
1368 }
1369 else
1370 {
1371 ASSERT(srBoxVertTriFace.typeA == SearchResult::FACE && srBoxVertTriFace.typeB == SearchResult::VERTEX);
1372
1373 res.valid = true;
1374
1375 res.contactNormal = srBoxVertTriFace.face.normal;
1376
1377 res.depth = srBoxVertTriFace.face.k;
1378
1379 res.contactA = srBoxVertTriFace.objB.GetVertWorld(srBoxVertTriFace.indexB);
1380
1381 res.contactB = res.contactA + srBoxVertTriFace.face.normal * srBoxVertTriFace.face.k;
1382 }
1383 }
1384 else
1385 {
1386 ASSERT(srBoxTriEE.typeA == SearchResult::EDGE &&
1387 srBoxTriEE.typeB == SearchResult::EDGE);
1388
1389 neV3 edgeA[2];
1390 neV3 edgeB[2];
1391
1392 srBoxTriEE.objA.GetWorldEdgeVerts(srBoxTriEE.indexA, edgeA[0], edgeA[1]);
1393
1394 srBoxTriEE.objB.GetWorldEdgeVerts(srBoxTriEE.indexB, edgeB[0], edgeB[1]);
1395
1396 bool r = CalcContactEE(edgeA[0], edgeA[1], edgeB[0], edgeB[1], res.contactA, res.contactB);
1397
1398 if (r)
1399 {
1400 if (srBoxTriEE.face.k > 0.0f)
1401 {
1402 res.contactNormal = srBoxTriEE.face.normal * -1.0f;
1403
1404 res.depth = srBoxTriEE.face.k;
1405 }
1406 else
1407 {
1408 res.contactNormal = srBoxTriEE.face.normal;
1409
1410 res.depth = srBoxTriEE.face.k * -1.0f;
1411 }
1412
1413 res.valid = true;
1414 }
1415 else
1416 {
1417 //return false;
1418 goto FV_Backup;
1419 }
1420 }
1421 return true;
1422 }
1423
Convex2TerrainTest(neCollisionResult & result,TConvex & convexA,neT3 & transA,TConvex & convexB)1424 void Convex2TerrainTest(neCollisionResult & result, TConvex & convexA, neT3 & transA, TConvex & convexB)
1425 {
1426 neSimpleArray<s32> & _triIndex = *convexB.as.terrain.triIndex;
1427
1428 s32 triangleCount = _triIndex.GetUsedCount();
1429
1430 neArray<neTriangle_> & triangleArray = *convexB.as.terrain.triangles;
1431
1432 ConvexTestResult res[2];
1433
1434 s32 finalTriIndex = -1;
1435 s32 currentRes = 1;
1436 s32 testRes = 0;
1437
1438 res[currentRes].depth = -1.0e6f;
1439 res[currentRes].valid = false;
1440 res[testRes].depth = 1.0e6f;
1441
1442 s32 terrainMatID = 0;
1443
1444 neBool found = false;
1445 #if 0
1446 for (s32 j = 0/*triangleCount-1*/; j < triangleCount; j++)
1447 //int j = 12;
1448 {
1449 neV3 points[4];
1450 neV3 red;red.Set(1.0f);
1451
1452 neTriangle_ * t = &triangleArray[_triIndex[j]];
1453
1454 points[0] = convexB.vertices[t->indices[0]];
1455 points[1] = convexB.vertices[t->indices[1]];
1456 points[2] = convexB.vertices[t->indices[2]];
1457 points[3] = convexB.vertices[t->indices[0]];
1458 extern void DrawLine(const neV3 & colour, neV3 * startpoint, s32 count);
1459 DrawLine(red, points, 4);
1460 }
1461 #endif
1462 for (s32 i = 0; i < triangleCount; i++)
1463 {
1464 s32 test = _triIndex[i];
1465
1466 neTriangle_ * t = &triangleArray[_triIndex[i]];
1467
1468 _triVertexPos[0] = convexB.vertices[t->indices[0]];
1469 _triVertexPos[1] = convexB.vertices[t->indices[1]];
1470 _triVertexPos[2] = convexB.vertices[t->indices[2]];
1471
1472 neV3 diff1 = _triVertexPos[1] - _triVertexPos[0];
1473 neV3 diff2 = _triVertexPos[2] - _triVertexPos[1];
1474 neV3 diff3 = _triVertexPos[0] - _triVertexPos[2];
1475
1476 _triNormals[0] = diff1.Cross(diff2);
1477
1478 _triNormals[0].Normalize();
1479
1480 _triNormals[1] = -_triNormals[0];
1481
1482 _triNormals[0].v[3] = _triNormals[0].Dot(_triVertexPos[0]);
1483
1484 _triNormals[1].v[3] = -_triNormals[0].v[3];
1485
1486 TriEdgeDir[0] = _triNormals[0].Cross(diff1);
1487 TriEdgeDir[1] = _triNormals[0].Cross(diff2);
1488 TriEdgeDir[2] = _triNormals[0].Cross(diff3);
1489 TriEdgeDir[0].Normalize();
1490 TriEdgeDir[1].Normalize();
1491 TriEdgeDir[2].Normalize();
1492
1493 neV3 insidePoint = _triVertexPos[0] + _triVertexPos[1] + _triVertexPos[2];
1494
1495 insidePoint *= (1.0f / 3.0f);
1496
1497 //insidePoint += (_triNormals[1] * 0.1f);
1498
1499 if (TestDCDTri(res[testRes], convexA, transA, insidePoint))
1500 {
1501 if (res[testRes].depth > res[currentRes].depth)
1502 {
1503 s32 tmp = testRes;
1504
1505 testRes = currentRes;
1506
1507 currentRes = tmp;
1508
1509 terrainMatID = t->materialID;
1510
1511 finalTriIndex = _triIndex[i];
1512
1513 found = true;
1514 }
1515 }
1516 }
1517 if (found)
1518 {
1519 result.penetrate = true;
1520
1521 result.depth = res[currentRes].depth;
1522
1523 result.collisionFrame[2] = res[currentRes].contactNormal;
1524
1525 result.materialIdB = terrainMatID;
1526
1527 result.contactA = res[currentRes].contactA;
1528
1529 result.contactB = res[currentRes].contactB;
1530 }
1531 else
1532 {
1533 result.penetrate = false;
1534 }
1535 }
1536
CalcContactEE(const neV3 & edgeA0,const neV3 & edgeA1,const neV3 & edgeB0,const neV3 & edgeB1,neV3 & contactA,neV3 & contactB)1537 bool CalcContactEE(const neV3 & edgeA0,
1538 const neV3 & edgeA1,
1539 const neV3 & edgeB0,
1540 const neV3 & edgeB1, neV3 & contactA, neV3 & contactB)
1541 {
1542 f32 d1343, d4321, d1321, d4343, d2121;
1543 f32 numer, denom, au, bu;
1544
1545 neV3 p13;
1546 neV3 p43;
1547 neV3 p21;
1548
1549 p13 = (edgeA0) - (edgeB0);
1550 p43 = (edgeB1) - (edgeB0);
1551
1552 if ( p43.IsConsiderZero() )
1553 {
1554 goto CalcContactEE_Exit;
1555 }
1556
1557 p21 = (edgeA1) - (edgeA0);
1558
1559 if ( p21.IsConsiderZero() )
1560 {
1561 goto CalcContactEE_Exit;
1562 }
1563
1564 d1343 = p13.Dot(p43);
1565 d4321 = p43.Dot(p21);
1566 d1321 = p13.Dot(p21);
1567 d4343 = p43.Dot(p43);
1568 d2121 = p21.Dot(p21);
1569
1570 denom = d2121 * d4343 - d4321 * d4321;
1571
1572 if (neAbs(denom) < NE_ZERO)
1573 goto CalcContactEE_Exit;
1574
1575 numer = d1343 * d4321 - d1321 * d4343;
1576 au = numer / denom;
1577 bu = (d1343 + d4321 * (au)) / d4343;
1578
1579 if (au < 0.0f || au >= 1.0f)
1580 goto CalcContactEE_Exit;
1581
1582 if (bu < 0.0f || bu >= 1.0f)
1583 goto CalcContactEE_Exit;
1584
1585 {
1586 neV3 tmpv;
1587
1588 tmpv = p21 * au;
1589 contactA = (edgeA0) + tmpv;
1590
1591 tmpv = p43 * bu;
1592 contactB = (edgeB0) + tmpv;
1593 }
1594
1595 return true;
1596
1597 CalcContactEE_Exit:
1598
1599 return false;
1600 }