1 /* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 *
11 * 1. The origin of this software must not be misrepresented; you must not
12 * claim that you wrote the original software. If you use this software
13 * in a product, an acknowledgment in the product documentation would be
14 * appreciated but is not required.
15 *
16 * 2. Altered source versions must be plainly marked as such, and must not be
17 * misrepresented as being the original software.
18 *
19 * 3. This notice may not be removed or altered from any source distribution.
20 */
21
22 #include "dgTypes.h"
23 #include "dgHeap.h"
24 #include "dgStack.h"
25 #include "dgSphere.h"
26 #include "dgPolyhedra.h"
27 #include "dgConvexHull3d.h"
28 #include "dgSmallDeterminant.h"
29 #include <string.h>
30
31
32 #pragma warning(disable:4100)
33 //#define DG_MIN_EDGE_ASPECT_RATIO hacd::HaF64 (0.02f)
34
35 class dgDiagonalEdge
36 {
37 public:
dgDiagonalEdge(dgEdge * const edge)38 dgDiagonalEdge (dgEdge* const edge)
39 :m_i0(edge->m_incidentVertex), m_i1(edge->m_twin->m_incidentVertex)
40 {
41 }
42 hacd::HaI32 m_i0;
43 hacd::HaI32 m_i1;
44 };
45
46
47 struct dgEdgeCollapseEdgeHandle
48 {
dgEdgeCollapseEdgeHandledgEdgeCollapseEdgeHandle49 dgEdgeCollapseEdgeHandle (dgEdge* const newEdge)
50 :m_inList(false), m_edge(newEdge)
51 {
52 }
53
dgEdgeCollapseEdgeHandledgEdgeCollapseEdgeHandle54 dgEdgeCollapseEdgeHandle (const dgEdgeCollapseEdgeHandle &dataHandle)
55 :m_inList(true), m_edge(dataHandle.m_edge)
56 {
57 dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *)IntToPointer (m_edge->m_userData);
58 if (handle) {
59 HACD_ASSERT (handle != this);
60 handle->m_edge = NULL;
61 }
62 m_edge->m_userData = hacd::HaU64 (PointerToInt(this));
63 }
64
~dgEdgeCollapseEdgeHandledgEdgeCollapseEdgeHandle65 ~dgEdgeCollapseEdgeHandle ()
66 {
67 if (m_inList) {
68 if (m_edge) {
69 dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *)IntToPointer (m_edge->m_userData);
70 if (handle == this) {
71 m_edge->m_userData = PointerToInt (NULL);
72 }
73 }
74 }
75 m_edge = NULL;
76 }
77
78 hacd::HaU32 m_inList;
79 dgEdge* m_edge;
80 };
81
82
83 class dgVertexCollapseVertexMetric
84 {
85 public:
86 hacd::HaF64 elem[10];
87
dgVertexCollapseVertexMetric(const dgBigPlane & plane)88 dgVertexCollapseVertexMetric (const dgBigPlane &plane)
89 {
90 elem[0] = plane.m_x * plane.m_x;
91 elem[1] = plane.m_y * plane.m_y;
92 elem[2] = plane.m_z * plane.m_z;
93 elem[3] = plane.m_w * plane.m_w;
94 elem[4] = hacd::HaF64 (2.0) * plane.m_x * plane.m_y;
95 elem[5] = hacd::HaF64 (2.0) * plane.m_x * plane.m_z;
96 elem[6] = hacd::HaF64 (2.0) * plane.m_x * plane.m_w;
97 elem[7] = hacd::HaF64 (2.0) * plane.m_y * plane.m_z;
98 elem[8] = hacd::HaF64 (2.0) * plane.m_y * plane.m_w;
99 elem[9] = hacd::HaF64 (2.0) * plane.m_z * plane.m_w;
100 }
101
Clear()102 void Clear ()
103 {
104 memset (elem, 0, 10 * sizeof (hacd::HaF64));
105 }
106
Accumulate(const dgVertexCollapseVertexMetric & p)107 void Accumulate (const dgVertexCollapseVertexMetric& p)
108 {
109 elem[0] += p.elem[0];
110 elem[1] += p.elem[1];
111 elem[2] += p.elem[2];
112 elem[3] += p.elem[3];
113 elem[4] += p.elem[4];
114 elem[5] += p.elem[5];
115 elem[6] += p.elem[6];
116 elem[7] += p.elem[7];
117 elem[8] += p.elem[8];
118 elem[9] += p.elem[9];
119 }
120
Accumulate(const dgBigPlane & plane)121 void Accumulate (const dgBigPlane& plane)
122 {
123 elem[0] += plane.m_x * plane.m_x;
124 elem[1] += plane.m_y * plane.m_y;
125 elem[2] += plane.m_z * plane.m_z;
126 elem[3] += plane.m_w * plane.m_w;
127
128 elem[4] += hacd::HaF64 (2.0f) * plane.m_x * plane.m_y;
129 elem[5] += hacd::HaF64 (2.0f) * plane.m_x * plane.m_z;
130 elem[7] += hacd::HaF64 (2.0f) * plane.m_y * plane.m_z;
131
132 elem[6] += hacd::HaF64 (2.0f) * plane.m_x * plane.m_w;
133 elem[8] += hacd::HaF64 (2.0f) * plane.m_y * plane.m_w;
134 elem[9] += hacd::HaF64 (2.0f) * plane.m_z * plane.m_w;
135 }
136
137
Evalue(const dgVector & p) const138 hacd::HaF64 Evalue (const dgVector &p) const
139 {
140 hacd::HaF64 acc = elem[0] * p.m_x * p.m_x + elem[1] * p.m_y * p.m_y + elem[2] * p.m_z * p.m_z +
141 elem[4] * p.m_x * p.m_y + elem[5] * p.m_x * p.m_z + elem[7] * p.m_y * p.m_z +
142 elem[6] * p.m_x + elem[8] * p.m_y + elem[9] * p.m_z + elem[3];
143 return fabs (acc);
144 }
145 };
146
147
148
149 #if 0
150 namespace InternalPolyhedra
151 {
152
153
154 struct VertexCache: public dgList<dgEdge*>
155 {
156 hacd::HaI32 size;
157
158 VertexCache (hacd::HaI32 t, dgMemoryAllocator* const allocator)
159 :dgList<dgEdge*>(allocator)
160 {
161 size = t;
162 }
163
164
165 hacd::HaI32 IsInCache (dgEdge *edge) const
166 {
167 hacd::HaI32 score;
168 dgEdge *ptr;
169
170 score = GetCount() + 2;
171 Iterator iter (*this);
172 for (iter.End(); iter; iter --) {
173 ptr = *iter;
174 if (ptr->m_incidentVertex == edge->m_incidentVertex) {
175 return score;
176 }
177 score --;
178 }
179 return 0;
180 }
181
182 hacd::HaI32 AddEdge (dgEdge *edge)
183 {
184 if (IsInCache (edge) == 0) {
185 Addtop (edge);
186 if (GetCount() > size) {
187 Remove(GetLast());
188 }
189 return 1;
190 }
191 return 0;
192 }
193
194
195 dgEdge *GetEdge (hacd::HaI32 mark) const
196 {
197 dgEdge *ptr;
198 dgEdge *edge;
199
200 if (GetCount()) {
201 Iterator iter (*this);
202 for (iter.End(); iter; iter --) {
203 ptr = *iter;
204 edge = ptr;
205 do {
206 if (edge->m_incidentFace > 0) {
207 if (edge->m_mark != mark) {
208 return edge;
209 }
210 }
211 edge = edge->m_twin->m_next;
212 } while (ptr != edge);
213 }
214 }
215 return NULL;
216 }
217 };
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234 /*
235 static bool CheckIfCoplanar (
236 const dgBigPlane& plane,
237 dgEdge *face,
238 const hacd::HaF32* const pool,
239 hacd::HaI32 stride)
240 {
241 dgEdge* ptr;
242 hacd::HaF64 dist;
243
244 ptr = face;
245 do {
246 dgBigVector p (&pool[ptr->m_incidentVertex * stride]);
247 dist = fabs (plane.Evalue (p));
248 if (dist > hacd::HaF64(0.08)) {
249 return false;
250 }
251 ptr = ptr->m_next;
252 } while (ptr != face);
253
254 return true;
255 }
256 */
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272 static void GetAdjacentCoplanarFacesPerimeter (
273 dgPolyhedra& perimeter,
274 const dgPolyhedra& polyhedra,
275 dgEdge* const face,
276 const hacd::HaF32* const pool,
277 hacd::HaI32 strideInBytes,
278 dgEdge** const stack,
279 hacd::HaI32* const faceIndex)
280 {
281 const hacd::HaF32 normalDeviation = hacd::HaF32 (0.9999f);
282 dgStack<hacd::HaI32>facesIndex (4096);
283
284 HACD_ASSERT (face->m_incidentFace > 0);
285
286 polyhedra.IncLRU();
287 hacd::HaI32 faceMark = polyhedra.IncLRU();
288
289 dgVector normal (polyhedra.FaceNormal (face, pool, strideInBytes));
290 hacd::HaF32 dot = normal % normal;
291 if (dot < hacd::HaF32 (1.0e-12f)) {
292 dgEdge* ptr = face;
293 hacd::HaI32 faceIndexCount = 0;
294 do {
295 faceIndex[faceIndexCount] = ptr->m_incidentVertex;
296 faceIndexCount ++;
297 ptr->m_mark = faceMark;
298 ptr = ptr->m_next;
299 } while (ptr != face);
300 perimeter.AddFace (faceIndexCount, faceIndex);
301 return;
302 }
303 normal = normal.Scale (hacd::HaF32 (1.0f) / dgSqrt (dot));
304
305 stack[0] = face;
306 hacd::HaI32 index = 1;
307 perimeter.BeginFace();
308 while (index) {
309 index --;
310 dgEdge* edge = stack[index];
311
312 if (edge->m_mark == faceMark) {
313 continue;
314 }
315
316 dgVector normal1 (polyhedra.FaceNormal (edge, pool, strideInBytes));
317 dot = normal1 % normal1;
318 if (dot < hacd::HaF32 (1.0e-12f)) {
319 dot = hacd::HaF32 (1.0e-12f);
320 }
321 normal1 = normal1.Scale (hacd::HaF32 (1.0f) / dgSqrt (dot));
322
323 dot = normal1 % normal;
324 if (dot >= normalDeviation) {
325 dgEdge* ptr = edge;
326 hacd::HaI32 faceIndexCount = 0;
327 do {
328 faceIndex[faceIndexCount] = ptr->m_incidentVertex;
329 faceIndexCount ++;
330 ptr->m_mark = faceMark;
331 if ((ptr->m_twin->m_incidentFace > 0) && (ptr->m_twin->m_mark != faceMark)) {
332 stack[index] = ptr->m_twin;
333 index ++;
334 HACD_ASSERT (index < polyhedra.GetCount() / 2);
335 }
336 ptr = ptr->m_next;
337 } while (ptr != edge);
338 perimeter.AddFace (faceIndexCount, faceIndex);
339 }
340 }
341 perimeter.EndFace();
342
343 dgPolyhedra::Iterator iter (perimeter);
344 for (iter.Begin(); iter; ) {
345 dgEdge* edge = &(*iter);
346 iter ++;
347 if ((edge->m_incidentFace > 0) && (edge->m_twin->m_incidentFace > 0)) {
348 if ( perimeter.GetNodeFromInfo (*edge->m_twin) == iter.GetNode()) {
349 iter ++;
350 }
351 perimeter.DeleteEdge (edge);
352 }
353 }
354 }
355 }
356
357
358 dgPolyhedraDescriptor::dgPolyhedraDescriptor (const dgPolyhedra& Polyhedra)
359 :m_unboundedLoops ()
360 {
361 Update (Polyhedra);
362 }
363
364 dgPolyhedraDescriptor::~dgPolyhedraDescriptor ()
365 {
366 }
367
368 void dgPolyhedraDescriptor::Update (const dgPolyhedra& srcPolyhedra)
369 {
370 hacd::HaI32 saveMark;
371 hacd::HaI32 faceCountLocal;
372 hacd::HaI32 edgeCountLocal;
373 hacd::HaI32 vertexCountLocal;
374 hacd::HaI32 maxVertexIndexLocal;
375 dgEdge *ptr;
376 dgEdge *edge;
377 dgPolyhedra* polyhedra;
378
379 polyhedra = (dgPolyhedra*) &srcPolyhedra;
380
381 faceCountLocal = 0;
382 edgeCountLocal = 0;
383 vertexCountLocal = 0;
384 maxVertexIndexLocal = -1;
385
386 saveMark = polyhedra->m_edgeMark;
387 if (saveMark < 8) {
388 saveMark = 8;
389 }
390 polyhedra->m_edgeMark = 8;
391 dgPolyhedra::Iterator iter(*polyhedra);
392 for (iter.Begin(); iter; iter ++) {
393 edge = &(*iter);
394 edge->m_mark = 0;
395 edgeCountLocal ++;
396 if (edge->m_incidentVertex > maxVertexIndexLocal) {
397 maxVertexIndexLocal = edge->m_incidentVertex;
398 }
399 }
400
401 m_unboundedLoops.RemoveAll();
402 for (iter.Begin(); iter; iter ++) {
403 edge = &(*iter);
404
405 if (edge->m_incidentFace < 0) {
406 if (~edge->m_mark & 1) {
407 m_unboundedLoops.Append (edge);
408 ptr = edge;
409 do {
410 ptr->m_mark |= 1;
411 ptr = ptr->m_next;
412 } while (ptr != edge);
413 }
414 }
415
416 if (~edge->m_mark & 2) {
417 vertexCountLocal ++;
418 ptr = edge;
419 do {
420 ptr->m_mark |= 2;
421 ptr = ptr->m_twin->m_next;
422 } while (ptr != edge);
423 }
424
425 if (~edge->m_mark & 4) {
426 ptr = edge;
427 faceCountLocal ++;
428 do {
429 ptr->m_mark |= 4;
430 ptr = ptr->m_next;
431 } while (ptr != edge);
432 }
433 }
434
435 m_edgeCount = edgeCountLocal;
436 m_faceCount = faceCountLocal;
437 m_vertexCount = vertexCountLocal;
438 m_maxVertexIndex = maxVertexIndexLocal + 1;
439
440 polyhedra->m_edgeMark = saveMark;
441 }
442
443
444
445
446
447
448
449 void dgPolyhedra::DeleteAllFace()
450 {
451 m_baseMark = 0;
452 m_edgeMark = 0;
453 m_faceSecuence = 0;
454 RemoveAll();
455 }
456
457
458 bool dgPolyhedra::SanityCheck () const
459 {
460 //HACD_ASSERT (0);
461 return true;
462 /*
463 hacd::HaI32 i;
464 hacd::HaI32 coorCount;
465 dgEdge *ptr;
466 dgEdge *edge;
467 dgTreeNode *node;
468 dgStack<hacd::HaI32> coor(65536);
469
470 Iterator iter (*this);
471 for (iter.Begin(); iter; iter ++) {
472 node = iter.GetNode();
473 if (!node->IsAlive()) {
474 return false;
475 }
476
477 edge = &(*iter);
478
479 if ((edge->m_incidentFace < 0) && (edge->m_twin->m_incidentFace < 0)) {
480 return false;
481 }
482
483
484 if (edge->m_mark > m_edgeMark) {
485 return false;
486 }
487
488 node = iter.GetNode();
489 dgPairKey key (edge->m_incidentVertex, edge->m_twin->m_incidentVertex);
490 if (key.GetVal() != node->GetKey()) {
491 return false;
492 }
493
494 ptr = edge;
495 do {
496 if (ptr->m_incidentVertex != edge->m_incidentVertex) {
497 return false;
498 }
499 ptr = ptr->m_twin->m_next;
500 } while (ptr != edge);
501
502 ptr = edge;
503 coorCount = 0;
504 do {
505 if (coorCount * sizeof (hacd::HaI32) > coor.GetSizeInBytes()) {
506 return false;
507 }
508 if (ptr->m_incidentFace != edge->m_incidentFace) {
509 return false;
510 }
511 coor [coorCount] = ptr->m_incidentVertex;
512 coorCount ++;
513
514 ptr = ptr->m_next;
515 } while (ptr != edge);
516
517 ptr = edge->m_prev;
518 i = 0;
519 do {
520 if (i * sizeof (hacd::HaI32) > coor.GetSizeInBytes()) {
521 return false;
522 }
523 if (ptr->m_incidentFace != edge->m_incidentFace) {
524 return false;
525 }
526
527 if (coor [coorCount - i - 1] != ptr->m_incidentVertex) {
528 return false;
529 }
530
531 i ++;
532 ptr = ptr->m_prev;
533 } while (ptr != edge->m_prev);
534
535 if (edge->m_twin->m_twin != edge) {
536 return false;
537 }
538
539 if (edge->m_prev->m_next != edge) {
540 return false;
541 }
542
543 if (edge->m_next->m_prev != edge) {
544 return false;
545 }
546 }
547
548 return dgTree <dgEdge, hacd::HaI64>::SanityCheck();
549 */
550 }
551
552
553
554
555
556
557
558
559
560
561
562 dgEdge *dgPolyhedra::FindVertexNode (hacd::HaI32 v) const
563 {
564 dgEdge *edge;
565 dgTreeNode *node;
566
567 dgPairKey key (0, v);
568 node = FindGreaterEqual (key.GetVal());
569 edge = NULL;
570 if (node) {
571 dgEdge *ptr;
572 ptr = node->GetInfo().m_twin;
573 if (ptr->m_incidentVertex == v) {
574 edge = ptr;
575 }
576 }
577
578 return edge;
579 }
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597 dgEdge *dgPolyhedra::SpliteEdgeAndTriangulate (hacd::HaI32 newIndex, dgEdge* srcEdge)
598 {
599 dgEdge* ankle = srcEdge->m_next;
600 dgEdge* edge = SpliteEdge (newIndex, srcEdge);
601 HACD_ASSERT (edge == ankle->m_prev);
602 edge = ankle->m_prev;
603 ankle = edge;
604
605 do {
606 dgEdge* const faceEdge = edge->m_twin;
607 if (faceEdge->m_incidentFace > 0) {
608 if (faceEdge->m_next->m_next->m_next != faceEdge) {
609 dgEdge* const edge0 = AddHalfEdge (newIndex, faceEdge->m_prev->m_incidentVertex);
610 dgEdge* const twin0 = AddHalfEdge (faceEdge->m_prev->m_incidentVertex, newIndex);
611
612 twin0->m_incidentFace = faceEdge->m_incidentFace;
613 faceEdge->m_prev->m_incidentFace = m_faceSecuence;
614 faceEdge->m_incidentFace = m_faceSecuence;
615 edge0->m_incidentFace = m_faceSecuence;
616 m_faceSecuence ++;
617
618 edge0->m_twin = twin0;
619 twin0->m_twin = edge0;
620
621 twin0->m_next = faceEdge->m_next;
622 faceEdge->m_next->m_prev = twin0;
623
624 twin0->m_prev = faceEdge->m_prev->m_prev;
625 faceEdge->m_prev->m_prev->m_next = twin0;
626
627 edge0->m_prev = faceEdge;
628 faceEdge->m_next = edge0;
629
630 edge0->m_next = faceEdge->m_prev;
631 faceEdge->m_prev->m_prev = edge0;
632 }
633 }
634
635 edge = edge->m_twin->m_next;
636 } while (edge != ankle);
637
638 #ifdef __ENABLE_SANITY_CHECK
639 HACD_ASSERT (SanityCheck ());
640 #endif
641
642 return ankle;
643 }
644
645
646
647
648 hacd::HaI32 dgPolyhedra::GetMaxIndex() const
649 {
650 hacd::HaI32 maxIndex;
651 dgEdge *edge;
652
653 maxIndex = -1;
654
655 dgPolyhedra::Iterator iter(*this);
656 for (iter.Begin(); iter; iter ++) {
657 edge = &(*iter);
658 if (edge->m_incidentVertex > maxIndex) {
659 maxIndex = edge->m_incidentVertex;
660 }
661 }
662 return (maxIndex + 1);
663 }
664
665
666
667 hacd::HaI32 dgPolyhedra::GetUnboundedFaceCount () const
668 {
669 hacd::HaI32 count;
670 hacd::HaI32 mark;
671 dgEdge *ptr;
672 dgEdge *edge;
673 Iterator iter (*this);
674
675 count = 0;
676 mark = IncLRU();
677 for (iter.Begin(); iter; iter ++) {
678 edge = &(*iter);
679 if (edge->m_mark == mark) {
680 continue;
681 }
682
683 if (edge->m_incidentFace > 0) {
684 continue;
685 }
686
687 count ++;
688 ptr = edge;
689 do {
690 ptr->m_mark = mark;
691 ptr = ptr->m_next;
692 } while (ptr != edge);
693 }
694 return count;
695 }
696
697
698
699 hacd::HaI32 dgPolyhedra::PackVertex (hacd::HaF32* const destArray, const hacd::HaF32* const unpackArray, hacd::HaI32 strideInBytes)
700 {
701 HACD_ASSERT (0);
702 return 0;
703 /*
704 hacd::HaI32 i;
705 hacd::HaI32 mark;
706 hacd::HaI32 stride;
707 hacd::HaI32 maxCount;
708 hacd::HaI32 edgeCount;
709 hacd::HaI32 vertexCount;
710 dgEdge *ptr;
711 dgEdge *edge;
712 dgTreeNode *node;
713 dgTreeNode **tree;
714
715 mark = IncLRU();
716
717 stride = strideInBytes / sizeof (hacd::HaF32);
718
719 maxCount = GetCount();
720 dgStack<dgTreeNode *> treePool(GetCount());
721 tree = &treePool[0];
722
723 edgeCount = 0;
724 vertexCount = 0;
725 Iterator iter (*this);
726 for (iter.Begin(); iter; ) {
727 node = iter.GetNode();
728 iter ++;
729
730 tree[edgeCount] = node;
731
732 HACD_ASSERT (edgeCount < maxCount);
733 edgeCount ++;
734
735 edge = &node->GetInfo();
736 if (edge->m_mark != mark) {
737 hacd::HaI32 index;
738 ptr = edge;
739 index = edge->m_incidentVertex;
740 memcpy (&destArray[vertexCount * stride], &unpackArray[index * stride], stride * sizeof (hacd::HaF32));
741 do {
742 ptr->m_mark = mark;
743 ptr->m_incidentVertex = vertexCount;
744 ptr = ptr->m_twin->m_next;
745
746 } while (ptr != edge);
747 vertexCount ++;
748 }
749 }
750
751 RemoveAll ();
752 for (i = 0; i < edgeCount; i ++) {
753 node = tree[i];
754 node->Unkill();
755 edge = &node->GetInfo();
756 dgPairKey key(edge->m_incidentVertex, edge->m_twin->m_incidentVertex);
757 Insert (node, key.GetVal());
758 node->Release();
759 }
760
761 return vertexCount;
762 */
763 }
764
765
766
767
768
769 void dgPolyhedra::GetBadEdges (dgList<dgEdge*>& faceList, const hacd::HaF32* const pool, hacd::HaI32 strideInBytes) const
770 {
771 dgStack<char> memPool ((4096 + 256) * (sizeof (hacd::HaF32) + sizeof(dgEdge)));
772 dgDownHeap<dgEdge*, hacd::HaF32> heap(&memPool[0], memPool.GetSizeInBytes());
773
774 dgPolyhedra tmp (*this);
775 hacd::HaI32 stride = hacd::HaI32 (strideInBytes / sizeof (hacd::HaF32));
776
777 hacd::HaI32 mark = tmp.IncLRU();
778 Iterator iter (tmp);
779 for (iter.Begin(); iter; iter ++) {
780 dgEdge* const edge = &(*iter);
781
782 if (edge->m_mark == mark) {
783 continue;
784 }
785 if (edge->m_incidentFace < 0) {
786 continue;
787 }
788
789 hacd::HaI32 count = 0;
790 dgEdge* ptr = edge;
791 do {
792 count ++;
793 ptr->m_mark = mark;
794 ptr = ptr->m_next;
795 } while (ptr != edge);
796
797 if (count > 3) {
798 dgEdge* const badEdge = InternalPolyhedra::TriangulateFace (tmp, edge, pool, stride, heap, NULL);
799 if (badEdge) {
800 dgEdge* const edge = FindEdge (badEdge->m_incidentVertex, badEdge->m_twin->m_incidentVertex);
801 dgEdge* ptr = edge;
802 do {
803 ptr->m_mark = mark;
804 ptr = ptr->m_next;
805 } while (ptr != edge);
806
807 HACD_ASSERT (edge);
808 HACD_ASSERT(0);
809 faceList.Append(edge);
810 }
811 }
812 }
813 }
814
815
816
817
818 /*
819 void dgPolyhedra::CollapseDegenerateFaces (
820 dgPolyhedraDescriptor &desc,
821 const hacd::HaF32* const pool,
822 hacd::HaI32 strideInBytes,
823 hacd::HaF32 area)
824 {
825 hacd::HaI32 i0;
826 hacd::HaI32 i1;
827 hacd::HaI32 i2;
828 hacd::HaI32 mark;
829 hacd::HaI32 stride;
830 hacd::HaF32 cost;
831 hacd::HaF32 area2;
832 hacd::HaF32 e10Len;
833 hacd::HaF32 e21Len;
834 hacd::HaF32 e02Len;
835 hacd::HaF32 faceArea;
836 bool someChanges;
837 dgEdge *ptr;
838 dgEdge *face;
839 dgEdge *edge;
840
841
842 #ifdef __ENABLE_SANITY_CHECK
843 HACD_ASSERT (SanityCheck ());
844 #endif
845
846 stride = strideInBytes / sizeof (hacd::HaF32);
847 area2 = area * area;
848 dgStack<char> heapPool (desc.m_faceCount * (sizeof (hacd::HaF32) + sizeof (dgPairKey) + sizeof (hacd::HaI32)));
849 HACD_ASSERT (0);
850 dgDownHeap<dgPairKey, hacd::HaF32> bigHeapArray(&heapPool[0], heapPool.GetSizeInBytes());
851
852 Iterator iter (*this);
853 do {
854 someChanges = false;
855 mark = IncLRU();
856 for (iter.Begin(); iter; iter ++) {
857 edge = &(*iter);
858
859 if ((edge->m_mark != mark) && (edge->m_incidentFace > 0)) {
860 HACD_ASSERT (edge->m_next->m_next->m_next == edge);
861
862 edge->m_mark = mark;
863 edge->m_next->m_mark = mark;
864 edge->m_prev->m_mark = mark;
865
866 i0 = edge->m_incidentVertex * stride;
867 i1 = edge->m_next->m_incidentVertex * stride;
868 i2 = edge->m_prev->m_incidentVertex * stride;
869
870 dgVector p0 (&pool[i0]);
871 dgVector p1 (&pool[i1]);
872 dgVector p2 (&pool[i2]);
873
874 dgVector normal ((p2 - p0) * (p1 - p0));
875 faceArea = normal % normal;
876 if (faceArea < area2) {
877 someChanges = true;
878 dgPairKey key (edge->m_incidentVertex, edge->m_twin->m_incidentVertex);
879 bigHeapArray.Push (key, area2 - faceArea);
880 }
881 }
882 }
883
884 while (bigHeapArray.GetCount()) {
885 // collapse this edge
886 cost = area2 - bigHeapArray.Value();
887 dgPairKey key (bigHeapArray[0]);
888 bigHeapArray.Pop();
889
890 // face = FindEdge (key.i0, key.i1);
891 face = FindEdge (key.GetLowKey(), key.GetHighKey());
892 if (face) {
893 i0 = face->m_incidentVertex * stride;
894 i1 = face->m_next->m_incidentVertex * stride;
895 i2 = face->m_prev->m_incidentVertex * stride;
896
897 dgVector p0 (&pool[i0]);
898 dgVector p1 (&pool[i1]);
899 dgVector p2 (&pool[i2]);
900
901 dgVector e10 (p1 - p0);
902 dgVector e21 (p2 - p1);
903 dgVector e02 (p0 - p2);
904
905 e10Len = e10 % e10;
906 e21Len = e21 % e21;
907 e02Len = e02 % e02;
908
909
910 edge = face;
911 if ((e21Len < e10Len) && (e21Len < e02Len)){
912 edge = face->m_next;
913 }
914 if ((e02Len < e10Len) && (e02Len < e21Len)){
915 edge = face->m_prev;
916 }
917 ptr = CollapseEdge(edge);
918 if (!ptr) {
919 ptr = CollapseEdge(edge->m_twin);
920 if (!ptr) {
921 ptr = CollapseEdge(edge->m_next);
922 if (!ptr) {
923 ptr = CollapseEdge(edge->m_next->m_twin);
924 if (!ptr) {
925 ptr = CollapseEdge(edge->m_prev->m_next);
926 if (!ptr) {
927 ptr = CollapseEdge(edge->m_prev->m_twin);
928 if (!ptr) {
929 DeleteFace (edge);
930 }
931 }
932 }
933 }
934 }
935 }
936
937 #ifdef __ENABLE_SANITY_CHECK
938 HACD_ASSERT (SanityCheck ());
939 #endif
940
941 }
942 }
943 } while (someChanges);
944
945 desc.Update(*this);
946 }
947 */
948
949 /*
950 void dgPolyhedra::GetCoplanarFaces (dgList<dgEdge*>& faceList, dgEdge *startFace, const hacd::HaF32* const pool, hacd::HaI32 strideInBytes, hacd::HaF32 normalDeviation) const
951 {
952 hacd::HaI32 mark;
953 hacd::HaI32 index;
954 // hacd::HaF64 dot;
955
956 if (!GetCount()) {
957 return;
958 }
959
960 dgStack<dgEdge*> stackPool (GetCount() / 2);
961 dgEdge **const stack = &stackPool[0];
962
963 if (startFace->m_incidentFace < 0) {
964 startFace = startFace->m_twin;
965 }
966
967 HACD_ASSERT (startFace->m_incidentFace > 0);
968 mark = IncLRU();
969
970 dgBigVector normal (FaceNormal (startFace, pool, strideInBytes));
971 hacd::HaF64 dot = normal % normal;
972 if (dot < hacd::HaF64 (1.0e-12f)) {
973 dgEdge* ptr = startFace;
974 do {
975 ptr->m_mark = mark;
976 ptr = ptr->m_next;
977 } while (ptr != startFace);
978
979 HACD_ASSERT (0);
980 faceList.Append (startFace);
981 return;
982 }
983 normal = normal.Scale (hacd::HaF64 (1.0) / sqrt (dot));
984
985
986 stack[0] = startFace;
987 index = 1;
988 while (index) {
989 index --;
990 dgEdge* const edge = stack[index];
991
992 if (edge->m_mark == mark) {
993 HACD_ASSERT (0u);
994 continue;
995 }
996
997 dgBigVector normal1 (FaceNormal (edge, pool, strideInBytes));
998 dot = normal1 % normal1;
999 if (dot > hacd::HaF64 (1.0e-12f)) {
1000 normal1 = normal1.Scale (hacd::HaF64 (1.0) / sqrt (dot));
1001 }
1002
1003 dot = normal1 % normal;
1004 if (dot >= normalDeviation) {
1005 dgEdge* ptr = edge;
1006 do {
1007 ptr->m_mark = mark;
1008
1009 if ((ptr->m_twin->m_incidentFace > 0) && (ptr->m_twin->m_mark != mark)) {
1010 stack[index] = ptr->m_twin;
1011 index ++;
1012 HACD_ASSERT (index < GetCount() / 2);
1013 }
1014 ptr = ptr->m_next;
1015 } while (ptr != edge);
1016
1017 HACD_ASSERT (0);
1018 faceList.Append (edge);
1019 }
1020 }
1021 }
1022
1023
1024
1025 void dgPolyhedra::DeleteOverlapingFaces (
1026 const hacd::HaF32* const pool,
1027 hacd::HaI32 strideInBytes,
1028 hacd::HaF32 distTol__)
1029 {
1030 hacd::HaI32 i;
1031 hacd::HaI32 mark;
1032 hacd::HaI32 perimeterCount;
1033 dgEdge *edge;
1034 dgPolyhedra *perimeters;
1035
1036 if (!GetCount()) {
1037 return;
1038 }
1039
1040 dgStack<hacd::HaI32>faceIndexPool (4096);
1041 dgStack<dgEdge*> stackPool (GetCount() / 2 + 100);
1042 dgStack<dgPolyhedra> flatPerimetersPool (GetCount() / 3 + 100);
1043
1044 perimeterCount = 0;
1045 perimeters = &flatPerimetersPool[0];
1046
1047 mark = IncLRU();
1048 Iterator iter (*this);
1049 for (iter.Begin(); iter; iter++) {
1050 edge = &(*iter);
1051 if (edge->m_incidentFace < 0) {
1052 continue;
1053 }
1054
1055 if (edge->m_mark >= mark) {
1056 continue;
1057 }
1058
1059 dgPolyhedra dommy;
1060 perimeters[perimeterCount] = dommy;
1061 InternalPolyhedra::GetAdjacentCoplanarFacesPerimeter (perimeters[perimeterCount], *this, edge, pool, strideInBytes, &stackPool[0], &faceIndexPool[0]);
1062 perimeterCount ++;
1063 }
1064
1065 // write code here
1066
1067
1068 for (i = 0 ; i < perimeterCount; i ++) {
1069 perimeters[i].DeleteAllFace();
1070 }
1071 }
1072
1073
1074 void dgPolyhedra::InvertWinding ()
1075 {
1076 dgStack<hacd::HaI32> vertexData(1024 * 4);
1077 dgStack<hacd::HaI64> userData(1024 * 4);
1078
1079 dgPolyhedra tmp (*this);
1080
1081 RemoveAll();
1082
1083 BeginFace();
1084 hacd::HaI32 mark = tmp.IncLRU();
1085 Iterator iter (tmp);
1086 for (iter.Begin(); iter; iter ++) {
1087 dgEdge* const edge = &(*iter);
1088
1089 if (edge->m_incidentFace < 0) {
1090 continue;
1091 }
1092 if (edge->m_mark == mark) {
1093 continue;
1094 }
1095
1096 hacd::HaI32 count = 0;
1097 dgEdge* ptr = edge;
1098 do {
1099 userData[count] = hacd::HaI32 (ptr->m_userData);
1100 vertexData[count] = ptr->m_incidentVertex;
1101 count ++;
1102 HACD_ASSERT (count < 1024 * 4);
1103
1104 ptr->m_mark = mark;
1105 ptr = ptr->m_prev;
1106 } while (ptr != edge);
1107
1108 AddFace(count, &vertexData[0], &userData[0]);
1109 }
1110 EndFace();
1111
1112 HACD_ASSERT (SanityCheck());
1113
1114 }
1115 */
1116
1117 /*
1118
1119 void dgPolyhedra::Quadrangulate (const hacd::HaF32* const vertex, hacd::HaI32 strideInBytes)
1120 {
1121 hacd::HaI32 mark;
1122 hacd::HaI32 stride;
1123 hacd::HaU32 cost;
1124 hacd::HaU32 maxCost;
1125 dgTree<dgEdge*, hacd::HaI64> essensialEdge;
1126
1127 Iterator iter (*this);
1128 for (iter.Begin(); iter; iter ++) {
1129 dgEdge *edge;
1130 edge = &(*iter);
1131 dgPairKey code (edge->m_incidentVertex, edge->m_twin->m_incidentVertex);
1132 essensialEdge.Insert (edge, code.GetVal());
1133 }
1134
1135 Triangulate (vertex, strideInBytes);
1136
1137 stride = strideInBytes / sizeof (hacd::HaF32);
1138 dgHeap<dgEdge*, hacd::HaU32> heapCost (GetCount(), 0xffffffff);
1139 maxCost = 1<<30;
1140 mark = IncLRU();
1141 for (iter.Begin(); iter; iter ++) {
1142 dgEdge *edge;
1143 dgEdge *twin;
1144
1145 hacd::HaU32 edgeCost;
1146 hacd::HaU32 twinCost;
1147 hacd::HaU32 shapeCost;
1148 hacd::HaU32 normalCost;
1149 hacd::HaF32 normalDot;
1150 hacd::HaF32 edgeAngle0;
1151 hacd::HaF32 edgeAngle1;
1152 hacd::HaF32 twinAngle0;
1153 hacd::HaF32 twinAngle1;
1154 hacd::HaF32 shapeFactor;
1155 hacd::HaF32 medianAngle;
1156
1157 dgTree<dgEdge*, hacd::HaI64>::dgTreeNode *essencial;
1158
1159 edge = &(*iter);
1160 twin = edge->m_twin;
1161
1162 if (edge->m_mark == mark) {
1163 continue;
1164 }
1165
1166 if ((edge->m_incidentFace < 0) || (twin->m_incidentFace < 0)) {
1167 continue;
1168 }
1169
1170 edge->m_mark = mark;
1171 twin->m_mark = mark;
1172
1173 dgVector edgeNormal (FaceNormal (edge, vertex, strideInBytes));
1174 if ((edgeNormal % edgeNormal) < 1.0e-8) {
1175 continue;
1176 }
1177 dgVector twinNormal (FaceNormal (twin, vertex, strideInBytes));
1178
1179 if ((twinNormal % twinNormal) < 1.0e-8) {
1180 continue;
1181 }
1182
1183 edgeNormal = edgeNormal.Scale (dgRsqrt (edgeNormal % edgeNormal));
1184 twinNormal = twinNormal.Scale (dgRsqrt (twinNormal % twinNormal));
1185
1186 normalDot = edgeNormal % twinNormal;
1187 normalCost = ClampValue (2048 - (hacd::HaI32)(2048.0f * normalDot), 0, 2048);
1188 if (normalCost > 600) {
1189 normalCost = 4000000;
1190 }
1191
1192 dgVector p0 (&vertex[edge->m_incidentVertex * stride]);
1193 dgVector p1 (&vertex[edge->m_twin->m_incidentVertex * stride]);
1194 dgVector p2 (&vertex[edge->m_prev->m_incidentVertex * stride]);
1195 dgVector p3 (&vertex[twin->m_prev->m_incidentVertex * stride]);
1196
1197 dgVector e10 (p1 - p0);
1198 dgVector e20 (p2 - p0);
1199 dgVector e30 (p3 - p0);
1200
1201 e10 = e10.Scale (dgRsqrt ((e10 % e10) + 1.e-10f));
1202 e20 = e20.Scale (dgRsqrt ((e20 % e20) + 1.e-10f));
1203 e30 = e30.Scale (dgRsqrt ((e30 % e30) + 1.e-10f));
1204
1205 edgeAngle0 = dgRAD2DEG * dgAtan2 (edgeNormal % (e10 * e20), e20 % e10);
1206 edgeAngle1 = dgRAD2DEG * dgAtan2 (twinNormal % (e30 * e10), e10 % e30);
1207
1208 if ((edgeAngle0 + edgeAngle1) < 160.0f) {
1209 HACD_ASSERT ((edgeAngle0 + edgeAngle1) > 0.0f);
1210 medianAngle = 4.0f * edgeAngle0 * edgeAngle1 / (edgeAngle0 + edgeAngle1);
1211
1212 HACD_ASSERT (medianAngle > 0.0f);
1213 HACD_ASSERT (medianAngle < 360.0f);
1214 edgeCost = abs (ClampValue (90 - (hacd::HaI32)medianAngle, -90, 90));
1215 } else {
1216 edgeCost = 4000000;
1217 }
1218
1219 dgVector t10 (p0 - p1);
1220 dgVector t20 (p3 - p1);
1221 dgVector t30 (p2 - p1);
1222
1223 t10 = t10.Scale (dgRsqrt ((t10 % t10) + 1.e-10f));
1224 t20 = t20.Scale (dgRsqrt ((t20 % t20) + 1.e-10f));
1225 t30 = t30.Scale (dgRsqrt ((t30 % t30) + 1.e-10f));
1226
1227 twinAngle0 = dgRAD2DEG * dgAtan2 (twinNormal % (t10 * t20), t20 % t10);
1228 twinAngle1 = dgRAD2DEG * dgAtan2 (edgeNormal % (t30 * t10), t10 % t30);
1229
1230 if ((twinAngle0 + twinAngle1) < 160.0f) {
1231 HACD_ASSERT ((twinAngle0 + twinAngle1) > 0.0f);
1232 medianAngle = 4.0f * twinAngle0 * twinAngle1 / (twinAngle0 + twinAngle1);
1233
1234 HACD_ASSERT (medianAngle > 0.0f);
1235 HACD_ASSERT (medianAngle < 360.0f);
1236 twinCost = abs (ClampValue (90 - (hacd::HaI32)medianAngle, -90, 90));
1237 } else {
1238 twinCost = 4000000;
1239 }
1240
1241
1242 hacd::HaF32 a0;
1243 hacd::HaF32 a1;
1244 hacd::HaF32 a2;
1245 hacd::HaF32 a3;
1246 hacd::HaF32 angleSum;
1247 hacd::HaF32 angleSum2;
1248
1249 a0 = edgeAngle0 + edgeAngle1;
1250 a1 = twinAngle0 + twinAngle1;
1251
1252 dgVector oe10 (p0 - p2);
1253 dgVector oe20 (p1 - p2);
1254
1255 oe10 = oe10.Scale (dgRsqrt ((oe10 % oe10) + 1.e-10f));
1256 oe20 = oe20.Scale (dgRsqrt ((oe20 % oe20) + 1.e-10f));
1257 a2 = dgRAD2DEG * dgAtan2 (edgeNormal % (oe10 * oe20), oe20 % oe10);
1258
1259 dgVector ot10 (p1 - p3);
1260 dgVector ot20 (p0 - p3);
1261
1262 ot10 = ot10.Scale (dgRsqrt ((ot10 % ot10) + 1.e-10f));
1263 ot20 = ot20.Scale (dgRsqrt ((ot20 % ot20) + 1.e-10f));
1264 a3 = dgRAD2DEG * dgAtan2 (twinNormal % (ot10 * ot20), ot20 % ot10);
1265
1266 angleSum = (a0 + a1 + a2 + a3) * 0.25f;
1267 angleSum2 = (a0 * a0 + a1 * a1 + a2 * a2 + a3 * a3) * 0.25f;
1268 shapeFactor = powf (dgSqrt (angleSum2 - angleSum * angleSum), 1.25f);
1269
1270 shapeCost = ClampValue ((hacd::HaI32)(shapeFactor * 4.0f), 0, 4096);
1271
1272 cost = normalCost + edgeCost + twinCost + shapeCost;
1273 dgPairKey code (edge->m_incidentVertex, edge->m_twin->m_incidentVertex);
1274 essencial = essensialEdge.Find(code.GetVal());
1275 if (essencial) {
1276 cost += 1000;
1277 }
1278 heapCost.Push (edge, maxCost - cost);
1279 }
1280
1281
1282 mark = IncLRU();
1283 while (heapCost.GetCount ()) {
1284 hacd::HaU32 cost;
1285 dgEdge *edge;
1286
1287 edge = heapCost[0];
1288 cost = maxCost - heapCost.Value ();
1289 if (cost > 100000) {
1290 break;
1291 }
1292
1293 heapCost.Pop();
1294
1295 if (edge->m_mark != mark) {
1296 edge->m_mark = mark;
1297 edge->m_twin->m_mark = mark;
1298
1299 edge->m_next->m_mark = mark;
1300 edge->m_next->m_twin->m_mark = mark;
1301
1302 edge->m_prev->m_mark = mark;
1303 edge->m_prev->m_twin->m_mark = mark;
1304
1305 edge->m_twin->m_next->m_mark = mark;
1306 edge->m_twin->m_next->m_twin->m_mark = mark;
1307
1308 edge->m_twin->m_prev->m_mark = mark;
1309 edge->m_twin->m_prev->m_twin->m_mark = mark;
1310
1311 DeleteEdge (edge);
1312 }
1313
1314 }
1315
1316 //#ifdef _DEBUG
1317 //mark = IncLRU();
1318 //for (iter.Begin(); iter; iter ++) {
1319 // dgEdge *edge;
1320 // edge = &(*iter);
1321 //
1322 // if (edge->m_incidentFace > 0) {
1323 // if (edge->m_mark != mark) {
1324 // dgEdge *ptr;
1325 // ptr = edge;
1326 // do {
1327 // ptr->m_mark = mark;
1328 // dgTrace (("%d ", ptr->m_incidentVertex));
1329 // ptr = ptr->m_next;
1330 // } while(ptr != edge);
1331 // dgTrace (("\n"));
1332 //
1333 // }
1334 // }
1335 //}
1336 //HACD_ASSERT (0);
1337 //#endif
1338
1339 }
1340
1341 void dgPolyhedra::OptimalTriangulation (const hacd::HaF32* const vertex, hacd::HaI32 strideInBytes)
1342 {
1343 hacd::HaI32 color;
1344 dgEdge *edge;
1345 dgList<dgEdge*> edgeStart;
1346
1347 Quadrangulate (vertex, strideInBytes);
1348
1349 color = IncLRU();
1350 dgTree<dgEdge*, hacd::HaI64> faceList;
1351 Iterator iter (*this);
1352 for (iter.Begin(); iter; iter ++) {
1353 hacd::HaI32 min;
1354 hacd::HaI32 count;
1355 dgEdge *ptr;
1356 dgEdge *start;
1357
1358 edge = &(*iter);
1359 if (edge->m_mark == color) {
1360 continue;
1361 }
1362 if (edge->m_incidentFace < 0) {
1363 edge->m_mark = color + 1;
1364 continue;
1365 }
1366
1367 count = 0;
1368 min = 0x7fffffff;
1369 start = edge;
1370 ptr = edge;
1371 do {
1372 count ++;
1373 ptr->m_mark = color;
1374 if (ptr->m_incidentVertex < min) {
1375 start = ptr;
1376 min = ptr->m_incidentVertex;
1377 }
1378 ptr = ptr->m_next;
1379 } while (ptr != edge);
1380 if (count == 4) {
1381 dgPairKey key (start->m_incidentVertex, start->m_twin->m_incidentVertex);
1382 faceList.Insert (start, key.GetVal());
1383 }
1384 }
1385
1386 color = IncLRU();
1387 for (edge = InternalPolyhedra::FindQuadStart(faceList, color); edge; edge = InternalPolyhedra::FindQuadStart(faceList, color)) {
1388 InternalPolyhedra::TriangleQuadStrips (*this, faceList, edge, color);
1389 }
1390
1391 #ifdef _DEBUG
1392 for (iter.Begin(); iter; iter ++) {
1393 edge = &(*iter);
1394 if (edge->m_incidentFace > 0)
1395 HACD_ASSERT (edge->m_next->m_next->m_next == edge);
1396 }
1397 #endif
1398 }
1399
1400
1401 hacd::HaI32 dgPolyhedra::TriangleStrips (
1402 hacd::HaU32 outputBuffer[],
1403 hacd::HaI32 maxBufferSize,
1404 hacd::HaI32 vertexCacheSize) const
1405 {
1406 hacd::HaI32 setMark;
1407 hacd::HaI32 indexCount;
1408 hacd::HaI32 stripsIndex;
1409 hacd::HaI32 faceColorMark;
1410 hacd::HaI32 debugFaceCount;
1411 hacd::HaI32 debugIndexCount;
1412
1413 dgEdge *edge;
1414 InternalPolyhedra::VertexCache vertexCache(vertexCacheSize);
1415
1416 dgPolyhedra tmp(*this);
1417
1418 tmp.IncLRU();
1419 faceColorMark = tmp.IncLRU();
1420 tmp.IncLRU();
1421 setMark = tmp.IncLRU();
1422
1423 debugFaceCount = 0;
1424 debugIndexCount = 0;
1425
1426 indexCount = 0;
1427
1428 for (edge = InternalPolyhedra::FindStripStart(tmp, faceColorMark, vertexCache); edge; edge = InternalPolyhedra::FindStripStart(tmp, faceColorMark, vertexCache)) {
1429 stripsIndex = InternalPolyhedra::TriangleStrips (edge, setMark, &outputBuffer[indexCount + 1], vertexCache);
1430
1431 debugFaceCount += stripsIndex - 2;
1432 debugIndexCount += stripsIndex;
1433
1434 if (stripsIndex > 0) {
1435 outputBuffer[indexCount] = stripsIndex;
1436 indexCount += stripsIndex + 1;
1437 if (indexCount > maxBufferSize) {
1438 break;
1439 }
1440 }
1441 }
1442
1443 dgTrace (("index per triangles %f\n", hacd::HaF32 (debugIndexCount) / hacd::HaF32 (debugFaceCount)));
1444
1445 return indexCount;
1446 }
1447 */
1448
1449 /*
1450 hacd::HaI32 dgPolyhedra::TriangleList (
1451 hacd::HaU32 outputBuffer[],
1452 hacd::HaI32 maxSize,
1453 hacd::HaI32 vertexCacheSize) const
1454 {
1455 hacd::HaI32 mark;
1456 hacd::HaI32 cost;
1457 hacd::HaI32 count;
1458 hacd::HaI32 vertex;
1459 hacd::HaI32 cacheHit;
1460 hacd::HaI32 tmpVertex;
1461 hacd::HaI32 cacheMiss;
1462 hacd::HaI32 twinVertex;
1463 hacd::HaI32 lowestPrize;
1464 hacd::HaI64 key;
1465 dgEdge *ptr;
1466 dgEdge *edge;
1467 dgList<hacd::HaI32> vertexCache;
1468 dgTree<dgEdge*, hacd::HaI64> edgeList;
1469 dgTree<dgEdge*, hacd::HaI64>::dgTreeNode *node;
1470 dgTree<dgEdge*, hacd::HaI64>::dgTreeNode *bestEdge;
1471
1472
1473 cacheHit = 0;
1474 cacheMiss = 0;
1475
1476 Iterator iter (*this);
1477 for (iter.Begin(); iter; iter ++) {
1478 edge = &(*iter);
1479 if (edge->m_incidentFace > 0) {
1480 edgeList.Insert (edge, iter.GetNode()->GetKey());
1481 }
1482 }
1483
1484 count = 0;
1485 mark = IncLRU();
1486 while (edgeList) {
1487
1488 node = edgeList.Minimum();
1489 edge = node->GetInfo();
1490 ptr = edge;
1491 do {
1492 ptr->m_mark = mark;
1493
1494 vertex = ptr->m_incidentVertex;
1495 if (count < maxSize) {
1496 outputBuffer[count] = vertex;
1497 count ++;
1498 }
1499 cacheMiss ++;
1500 vertexCache.Append (vertex);
1501 if (vertexCache.GetCount() > vertexCacheSize) {
1502 vertexCache.Remove (vertexCache.GetFirst());
1503 }
1504 edgeList.Remove(GetNodeFromInfo(*ptr)->GetKey());
1505
1506 ptr = ptr->m_next;
1507 } while (ptr != edge);
1508
1509 dgList<hacd::HaI32>::Iterator vertexIter(vertexCache);
1510 for (vertexIter.Begin(); vertexIter; ) {
1511
1512 vertex = *vertexIter;
1513 vertexIter ++;
1514
1515 key = vertex;
1516 key <<= 32;
1517
1518 bestEdge = NULL;
1519 lowestPrize = 100000;
1520
1521 node = edgeList.FindGreaterEqual (key);
1522 dgTree<dgEdge *, hacd::HaI64>::Iterator iter(edgeList);
1523 for (iter.Set(node); iter; iter ++) {
1524 node = iter.GetNode();
1525 key = node->GetKey();
1526 key >>= 32;
1527 if (key > vertex) {
1528 break;
1529 }
1530
1531 ptr = node->GetInfo();
1532
1533 HACD_ASSERT (ptr->m_mark != mark);
1534 HACD_ASSERT (ptr->m_twin->m_incidentVertex == vertex);
1535
1536
1537 twinVertex = ptr->m_twin->m_incidentVertex;
1538 dgList<hacd::HaI32>::Iterator vertexIter(vertexCache);
1539 cost = 0;
1540 for (vertexIter.Begin(); vertexIter; vertexIter ++) {
1541 tmpVertex = *vertexIter;
1542 if (twinVertex == tmpVertex) {
1543 break;
1544 };
1545 cost ++;
1546 }
1547 if (cost < lowestPrize) {
1548 lowestPrize = cost;
1549 bestEdge = node;
1550 }
1551 }
1552
1553 if (bestEdge) {
1554 edge = bestEdge->GetInfo();
1555
1556 ptr = edge;
1557 do {
1558 ptr->m_mark = mark;
1559 vertex = ptr->m_incidentVertex;
1560 if (count < maxSize) {
1561 outputBuffer[count] = vertex;
1562 count ++;
1563 }
1564
1565 edgeList.Remove(GetNodeFromInfo(*ptr)->GetKey());
1566
1567 dgList<hacd::HaI32>::Iterator vertexIter(vertexCache);
1568 for (vertexIter.Begin(); vertexIter; vertexIter++) {
1569 tmpVertex = *vertexIter;
1570 if (vertex == tmpVertex) {
1571 cacheHit ++;
1572 break;
1573 }
1574 }
1575
1576 if (!vertexIter) {
1577 cacheMiss ++;
1578 vertexCache.Append (vertex);
1579 if (vertexCache.GetCount() > vertexCacheSize) {
1580 vertexCache.Remove (vertexCache.GetFirst());
1581 }
1582 }
1583
1584 ptr = ptr->m_next;
1585 } while (ptr != edge);
1586
1587 vertexIter.Begin();
1588 }
1589 }
1590 }
1591
1592 // dgTrace ("cacheHit = %d, cacheMiss = %d, total = %d\n", cacheHit, cacheMiss, cacheMiss + cacheHit);
1593
1594 return count;
1595 }
1596
1597 */
1598
1599
1600 hacd::HaI32 dgPolyhedra::TriangleList (hacd::HaU32 outputBuffer[], hacd::HaI32 maxSize__, hacd::HaI32 vertexCacheSize__) const
1601 {
1602 hacd::HaI32 mark;
1603 hacd::HaI32 count;
1604 hacd::HaI32 cacheMiss;
1605 hacd::HaI32 score;
1606 hacd::HaI32 bestScore;
1607 dgEdge *ptr;
1608 dgEdge *edge;
1609 dgEdge *face;
1610 dgTree<dgEdge*, hacd::HaI32> vertexIndex;
1611 InternalPolyhedra::VertexCache vertexCache (16);
1612
1613
1614 Iterator iter (*this);
1615 for (iter.Begin(); iter; iter ++) {
1616 edge = &(*iter);
1617 vertexIndex.Insert (edge, edge->m_incidentVertex);
1618 }
1619 count = 0;
1620 cacheMiss = 0;;
1621
1622 mark = IncLRU();
1623 while (vertexIndex.GetCount()) {
1624 edge = vertexCache.GetEdge(mark);
1625 if (!edge) {
1626 dgTree<dgEdge*, hacd::HaI32>::dgTreeNode *node;
1627 dgTree<dgEdge*, hacd::HaI32>::Iterator iter (vertexIndex);
1628 for (iter.Begin (); iter; ) {
1629 node = iter.GetNode();
1630 iter ++;
1631 ptr = node->GetInfo();;
1632 edge = ptr;
1633
1634 do {
1635 if (edge->m_incidentFace > 0) {
1636 if (edge->m_mark != mark) {
1637 goto newEdge;
1638 }
1639 }
1640 edge = edge->m_twin->m_next;
1641 } while (edge != ptr);
1642 vertexIndex.Remove (node);
1643 }
1644 edge = NULL;
1645 }
1646 newEdge:
1647
1648 face = NULL;
1649 bestScore = -1;
1650 if (edge) {
1651 ptr = edge;
1652 do {
1653 if (ptr->m_incidentFace > 0) {
1654 if (ptr->m_mark != mark) {
1655 score = vertexCache.IsInCache (ptr->m_next) + vertexCache.IsInCache(ptr->m_prev);
1656 if (score > bestScore) {
1657 bestScore = score;
1658 face = ptr;
1659 }
1660 }
1661 }
1662
1663 ptr = ptr->m_twin->m_next;
1664 } while (ptr != edge);
1665
1666 HACD_ASSERT (face);
1667 ptr = face;
1668 do {
1669 outputBuffer[count] = hacd::HaU32 (ptr->m_incidentVertex);
1670 count ++;
1671 cacheMiss += vertexCache.AddEdge (ptr);
1672 ptr->m_mark = mark;
1673 ptr = ptr->m_next;
1674 } while (ptr != face);
1675 }
1676 }
1677
1678 // add all legacy faces
1679 for (iter.Begin(); iter; iter ++) {
1680 edge = &(*iter);
1681 if (edge->m_mark != mark) {
1682 if (edge->m_incidentFace > 0) {
1683 ptr = edge;
1684 do {
1685 outputBuffer[count] = hacd::HaU32 (ptr->m_incidentVertex);
1686 count ++;
1687 cacheMiss ++;
1688 ptr->m_mark = mark;
1689 ptr = ptr->m_next;
1690 } while (ptr != edge);
1691 }
1692 }
1693 }
1694
1695 dgTrace (("fifo efficiency %f\n", hacd::HaF32 (cacheMiss) * 3.0f / hacd::HaF32 (count)));
1696
1697 return count;
1698 }
1699
1700
1701
1702 void dgPolyhedra::SwapInfo (dgPolyhedra& source)
1703 {
1704 dgTree<dgEdge, dgEdgeKey>::SwapInfo((dgTree<dgEdge, dgEdgeKey>&)source);
1705
1706 Swap (m_baseMark, source.m_baseMark);
1707 Swap (m_edgeMark, source.m_edgeMark);
1708 Swap (m_faceSecuence, source.m_faceSecuence);
1709
1710 }
1711
1712 void dgPolyhedra::GetOpenFaces (dgList<dgEdge*>& faceList) const
1713 {
1714 hacd::HaI32 mark = IncLRU();
1715 // dgList<dgEdge*> openfaces;
1716 Iterator iter (*this);
1717 for (iter.Begin(); iter; iter ++) {
1718 dgEdge* edge = &(*iter);
1719 if ((edge->m_mark != mark) && (edge->m_incidentFace < 0)) {
1720 dgEdge* ptr = edge;
1721 faceList.Append(edge);
1722 do {
1723 ptr->m_mark = mark;
1724
1725 ptr = ptr->m_next;
1726 } while (ptr != edge);
1727 }
1728 }
1729 }
1730
1731
1732 /*
1733 bool dgPolyhedra::TriangulateFace (dgEdge* face, const hacd::HaF32* const vertex, hacd::HaI32 strideInBytes, dgVector& normal)
1734 {
1735 hacd::HaI32 memPool [2048];
1736 dgDownHeap<dgEdge*, hacd::HaF32> heap(&memPool[0], sizeof (memPool));
1737
1738
1739 hacd::HaI32 stride = hacd::HaI32 (strideInBytes / sizeof (hacd::HaF32));
1740 return InternalPolyhedra::TriangulateFace (*this, face, vertex, stride, heap, &normal) ? false : true;
1741 }
1742 */
1743
1744
1745
1746
1747
1748 #endif
1749
1750
1751
dgPolyhedra(void)1752 dgPolyhedra::dgPolyhedra (void)
1753 :dgTree <dgEdge, hacd::HaI64>()
1754 ,m_baseMark(0)
1755 ,m_edgeMark(0)
1756 ,m_faceSecuence(0)
1757 {
1758 }
1759
dgPolyhedra(const dgPolyhedra & polyhedra)1760 dgPolyhedra::dgPolyhedra (const dgPolyhedra &polyhedra)
1761 :dgTree <dgEdge, hacd::HaI64>()
1762 ,m_baseMark(0)
1763 ,m_edgeMark(0)
1764 ,m_faceSecuence(0)
1765 {
1766 dgStack<hacd::HaI32> indexPool (1024 * 16);
1767 dgStack<hacd::HaU64> userPool (1024 * 16);
1768 hacd::HaI32* const index = &indexPool[0];
1769 hacd::HaU64* const user = &userPool[0];
1770
1771 BeginFace ();
1772 Iterator iter(polyhedra);
1773 for (iter.Begin(); iter; iter ++) {
1774 dgEdge* const edge = &(*iter);
1775 if (edge->m_incidentFace < 0) {
1776 continue;
1777 }
1778
1779 if (!FindEdge(edge->m_incidentVertex, edge->m_twin->m_incidentVertex)) {
1780 hacd::HaI32 indexCount = 0;
1781 dgEdge* ptr = edge;
1782 do {
1783 user[indexCount] = ptr->m_userData;
1784 index[indexCount] = ptr->m_incidentVertex;
1785 indexCount ++;
1786 ptr = ptr->m_next;
1787 } while (ptr != edge);
1788
1789 dgEdge* const face = AddFace (indexCount, index, (hacd::HaI64*) user);
1790 ptr = face;
1791 do {
1792 ptr->m_incidentFace = edge->m_incidentFace;
1793 ptr = ptr->m_next;
1794 } while (ptr != face);
1795 }
1796 }
1797 EndFace();
1798
1799 m_faceSecuence = polyhedra.m_faceSecuence;
1800
1801 #ifdef __ENABLE_SANITY_CHECK
1802 HACD_ASSERT (SanityCheck());
1803 #endif
1804 }
1805
~dgPolyhedra()1806 dgPolyhedra::~dgPolyhedra ()
1807 {
1808 }
1809
1810
GetFaceCount() const1811 hacd::HaI32 dgPolyhedra::GetFaceCount() const
1812 {
1813 Iterator iter (*this);
1814 hacd::HaI32 count = 0;
1815 hacd::HaI32 mark = IncLRU();
1816 for (iter.Begin(); iter; iter ++) {
1817 dgEdge* const edge = &(*iter);
1818 if (edge->m_mark == mark) {
1819 continue;
1820 }
1821
1822 if (edge->m_incidentFace < 0) {
1823 continue;
1824 }
1825
1826 count ++;
1827 dgEdge* ptr = edge;
1828 do {
1829 ptr->m_mark = mark;
1830 ptr = ptr->m_next;
1831 } while (ptr != edge);
1832 }
1833 return count;
1834 }
1835
1836
1837
AddFace(hacd::HaI32 count,const hacd::HaI32 * const index,const hacd::HaI64 * const userdata)1838 dgEdge* dgPolyhedra::AddFace ( hacd::HaI32 count, const hacd::HaI32* const index, const hacd::HaI64* const userdata)
1839 {
1840 class IntersectionFilter
1841 {
1842 public:
1843 IntersectionFilter ()
1844 {
1845 m_count = 0;
1846 }
1847
1848 bool Insert (hacd::HaI32 dummy, hacd::HaI64 value)
1849 {
1850 hacd::HaI32 i;
1851 for (i = 0 ; i < m_count; i ++) {
1852 if (m_array[i] == value) {
1853 return false;
1854 }
1855 }
1856 m_array[i] = value;
1857 m_count ++;
1858 return true;
1859 }
1860
1861 hacd::HaI32 m_count;
1862 hacd::HaI64 m_array[2048];
1863 };
1864
1865 IntersectionFilter selfIntersectingFaceFilter;
1866
1867 hacd::HaI32 dummyValues = 0;
1868 hacd::HaI32 i0 = index[count-1];
1869 for (hacd::HaI32 i = 0; i < count; i ++) {
1870 hacd::HaI32 i1 = index[i];
1871 dgPairKey code0 (i0, i1);
1872
1873 if (!selfIntersectingFaceFilter.Insert (dummyValues, code0.GetVal())) {
1874 return NULL;
1875 }
1876
1877 dgPairKey code1 (i1, i0);
1878 if (!selfIntersectingFaceFilter.Insert (dummyValues, code1.GetVal())) {
1879 return NULL;
1880 }
1881
1882
1883 if (i0 == i1) {
1884 return NULL;
1885 }
1886 if (FindEdge (i0, i1)) {
1887 return NULL;
1888 }
1889 i0 = i1;
1890 }
1891
1892 m_faceSecuence ++;
1893
1894 i0 = index[count-1];
1895 hacd::HaI32 i1 = index[0];
1896 hacd::HaU64 udata0 = 0;
1897 hacd::HaU64 udata1 = 0;
1898 if (userdata) {
1899 udata0 = hacd::HaU64 (userdata[count-1]);
1900 udata1 = hacd::HaU64 (userdata[0]);
1901 }
1902
1903 bool state;
1904 dgPairKey code (i0, i1);
1905 dgEdge tmpEdge (i0, m_faceSecuence, udata0);
1906 dgTreeNode* node = Insert (tmpEdge, code.GetVal(), state);
1907 HACD_ASSERT (!state);
1908 dgEdge* edge0 = &node->GetInfo();
1909 dgEdge* const first = edge0;
1910
1911 for (hacd::HaI32 i = 1; i < count; i ++) {
1912 i0 = i1;
1913 i1 = index[i];
1914 udata0 = udata1;
1915 udata1 = hacd::HaU64 (userdata ? userdata[i] : 0);
1916
1917 dgPairKey code (i0, i1);
1918 dgEdge tmpEdge (i0, m_faceSecuence, udata0);
1919 node = Insert (tmpEdge, code.GetVal(), state);
1920 HACD_ASSERT (!state);
1921
1922 dgEdge* const edge1 = &node->GetInfo();
1923 edge0->m_next = edge1;
1924 edge1->m_prev = edge0;
1925 edge0 = edge1;
1926 }
1927
1928 first->m_prev = edge0;
1929 edge0->m_next = first;
1930
1931 return first->m_next;
1932 }
1933
1934
EndFace()1935 void dgPolyhedra::EndFace ()
1936 {
1937 dgPolyhedra::Iterator iter (*this);
1938
1939 // Connect all twin edge
1940 for (iter.Begin(); iter; iter ++) {
1941 dgEdge* const edge = &(*iter);
1942 if (!edge->m_twin) {
1943 edge->m_twin = FindEdge (edge->m_next->m_incidentVertex, edge->m_incidentVertex);
1944 if (edge->m_twin) {
1945 edge->m_twin->m_twin = edge;
1946 }
1947 }
1948 }
1949
1950 #ifdef __ENABLE_SANITY_CHECK
1951 HACD_ASSERT (SanityCheck());
1952 #endif
1953 dgStack<dgEdge*> edgeArrayPool(GetCount() * 2 + 256);
1954
1955 hacd::HaI32 edgeCount = 0;
1956 dgEdge** const edgeArray = &edgeArrayPool[0];
1957 for (iter.Begin(); iter; iter ++) {
1958 dgEdge* const edge = &(*iter);
1959 if (!edge->m_twin) {
1960 bool state;
1961 dgPolyhedra::dgPairKey code (edge->m_next->m_incidentVertex, edge->m_incidentVertex);
1962 dgEdge tmpEdge (edge->m_next->m_incidentVertex, -1);
1963 tmpEdge.m_incidentFace = -1;
1964 dgPolyhedra::dgTreeNode* const node = Insert (tmpEdge, code.GetVal(), state);
1965 HACD_ASSERT (!state);
1966 edge->m_twin = &node->GetInfo();
1967 edge->m_twin->m_twin = edge;
1968 edgeArray[edgeCount] = edge->m_twin;
1969 edgeCount ++;
1970 }
1971 }
1972
1973 for (hacd::HaI32 i = 0; i < edgeCount; i ++) {
1974 dgEdge* const edge = edgeArray[i];
1975 HACD_ASSERT (!edge->m_prev);
1976 dgEdge *ptr = edge->m_twin;
1977 for (; ptr->m_next; ptr = ptr->m_next->m_twin){}
1978 ptr->m_next = edge;
1979 edge->m_prev = ptr;
1980 }
1981
1982 #ifdef __ENABLE_SANITY_CHECK
1983 HACD_ASSERT (SanityCheck ());
1984 #endif
1985 }
1986
1987
DeleteFace(dgEdge * const face)1988 void dgPolyhedra::DeleteFace(dgEdge* const face)
1989 {
1990 dgEdge* edgeList[1024 * 16];
1991
1992 if (face->m_incidentFace > 0) {
1993 hacd::HaI32 count = 0;
1994 dgEdge* ptr = face;
1995 do {
1996 ptr->m_incidentFace = -1;
1997 hacd::HaI32 i = 0;
1998 for (; i < count; i ++) {
1999 if ((edgeList[i] == ptr) || (edgeList[i]->m_twin == ptr)) {
2000 break;
2001 }
2002 }
2003 if (i == count) {
2004 edgeList[count] = ptr;
2005 count ++;
2006 }
2007 ptr = ptr->m_next;
2008 } while (ptr != face);
2009
2010
2011 for (hacd::HaI32 i = 0; i < count; i ++) {
2012 dgEdge* const ptr = edgeList[i];
2013 if (ptr->m_twin->m_incidentFace < 0) {
2014 DeleteEdge (ptr);
2015 }
2016 }
2017 }
2018 }
2019
2020
2021
FaceNormal(dgEdge * const face,const hacd::HaF64 * const pool,hacd::HaI32 strideInBytes) const2022 dgBigVector dgPolyhedra::FaceNormal (dgEdge* const face, const hacd::HaF64* const pool, hacd::HaI32 strideInBytes) const
2023 {
2024 hacd::HaI32 stride = strideInBytes / sizeof (hacd::HaF64);
2025 dgEdge* edge = face;
2026 dgBigVector p0 (&pool[edge->m_incidentVertex * stride]);
2027 edge = edge->m_next;
2028 dgBigVector p1 (&pool[edge->m_incidentVertex * stride]);
2029 dgBigVector e1 (p1 - p0);
2030
2031 dgBigVector normal (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
2032 for (edge = edge->m_next; edge != face; edge = edge->m_next) {
2033 dgBigVector p2 (&pool[edge->m_incidentVertex * stride]);
2034 dgBigVector e2 (p2 - p0);
2035 normal += e1 * e2;
2036 e1 = e2;
2037 }
2038 return normal;
2039 }
2040
2041
AddHalfEdge(hacd::HaI32 v0,hacd::HaI32 v1)2042 dgEdge* dgPolyhedra::AddHalfEdge (hacd::HaI32 v0, hacd::HaI32 v1)
2043 {
2044 if (v0 != v1) {
2045 dgPairKey pairKey (v0, v1);
2046 dgEdge tmpEdge (v0, -1);
2047
2048 dgTreeNode* node = Insert (tmpEdge, pairKey.GetVal());
2049 return node ? &node->GetInfo() : NULL;
2050 } else {
2051 return NULL;
2052 }
2053 }
2054
2055
DeleteEdge(dgEdge * const edge)2056 void dgPolyhedra::DeleteEdge (dgEdge* const edge)
2057 {
2058 dgEdge *const twin = edge->m_twin;
2059
2060 edge->m_prev->m_next = twin->m_next;
2061 twin->m_next->m_prev = edge->m_prev;
2062 edge->m_next->m_prev = twin->m_prev;
2063 twin->m_prev->m_next = edge->m_next;
2064
2065 dgTreeNode *const nodeA = GetNodeFromInfo (*edge);
2066 dgTreeNode *const nodeB = GetNodeFromInfo (*twin);
2067
2068 HACD_ASSERT (&nodeA->GetInfo() == edge);
2069 HACD_ASSERT (&nodeB->GetInfo() == twin);
2070
2071 Remove (nodeA);
2072 Remove (nodeB);
2073 }
2074
2075
SpliteEdge(hacd::HaI32 newIndex,dgEdge * const edge)2076 dgEdge* dgPolyhedra::SpliteEdge (hacd::HaI32 newIndex, dgEdge* const edge)
2077 {
2078 dgEdge* const edge00 = edge->m_prev;
2079 dgEdge* const edge01 = edge->m_next;
2080 dgEdge* const twin00 = edge->m_twin->m_next;
2081 dgEdge* const twin01 = edge->m_twin->m_prev;
2082
2083 hacd::HaI32 i0 = edge->m_incidentVertex;
2084 hacd::HaI32 i1 = edge->m_twin->m_incidentVertex;
2085
2086 hacd::HaI32 f0 = edge->m_incidentFace;
2087 hacd::HaI32 f1 = edge->m_twin->m_incidentFace;
2088
2089 DeleteEdge (edge);
2090
2091 dgEdge* const edge0 = AddHalfEdge (i0, newIndex);
2092 dgEdge* const edge1 = AddHalfEdge (newIndex, i1);
2093
2094 dgEdge* const twin0 = AddHalfEdge (newIndex, i0);
2095 dgEdge* const twin1 = AddHalfEdge (i1, newIndex);
2096 HACD_ASSERT (edge0);
2097 HACD_ASSERT (edge1);
2098 HACD_ASSERT (twin0);
2099 HACD_ASSERT (twin1);
2100
2101 edge0->m_twin = twin0;
2102 twin0->m_twin = edge0;
2103
2104 edge1->m_twin = twin1;
2105 twin1->m_twin = edge1;
2106
2107 edge0->m_next = edge1;
2108 edge1->m_prev = edge0;
2109
2110 twin1->m_next = twin0;
2111 twin0->m_prev = twin1;
2112
2113 edge0->m_prev = edge00;
2114 edge00 ->m_next = edge0;
2115
2116 edge1->m_next = edge01;
2117 edge01->m_prev = edge1;
2118
2119 twin0->m_next = twin00;
2120 twin00->m_prev = twin0;
2121
2122 twin1->m_prev = twin01;
2123 twin01->m_next = twin1;
2124
2125 edge0->m_incidentFace = f0;
2126 edge1->m_incidentFace = f0;
2127
2128 twin0->m_incidentFace = f1;
2129 twin1->m_incidentFace = f1;
2130
2131 #ifdef __ENABLE_SANITY_CHECK
2132 // HACD_ASSERT (SanityCheck ());
2133 #endif
2134
2135 return edge0;
2136 }
2137
2138
2139
FlipEdge(dgEdge * const edge)2140 bool dgPolyhedra::FlipEdge (dgEdge* const edge)
2141 {
2142 // dgTreeNode *node;
2143 if (edge->m_next->m_next->m_next != edge) {
2144 return false;
2145 }
2146
2147 if (edge->m_twin->m_next->m_next->m_next != edge->m_twin) {
2148 return false;
2149 }
2150
2151 if (FindEdge(edge->m_prev->m_incidentVertex, edge->m_twin->m_prev->m_incidentVertex)) {
2152 return false;
2153 }
2154
2155 dgEdge *const prevEdge = edge->m_prev;
2156 dgEdge *const prevTwin = edge->m_twin->m_prev;
2157
2158 dgPairKey edgeKey (prevTwin->m_incidentVertex, prevEdge->m_incidentVertex);
2159 dgPairKey twinKey (prevEdge->m_incidentVertex, prevTwin->m_incidentVertex);
2160
2161 ReplaceKey (GetNodeFromInfo (*edge), edgeKey.GetVal());
2162 // HACD_ASSERT (node);
2163
2164 ReplaceKey (GetNodeFromInfo (*edge->m_twin), twinKey.GetVal());
2165 // HACD_ASSERT (node);
2166
2167 edge->m_incidentVertex = prevTwin->m_incidentVertex;
2168 edge->m_twin->m_incidentVertex = prevEdge->m_incidentVertex;
2169
2170 edge->m_userData = prevTwin->m_userData;
2171 edge->m_twin->m_userData = prevEdge->m_userData;
2172
2173 prevEdge->m_next = edge->m_twin->m_next;
2174 prevTwin->m_prev->m_prev = edge->m_prev;
2175
2176 prevTwin->m_next = edge->m_next;
2177 prevEdge->m_prev->m_prev = edge->m_twin->m_prev;
2178
2179 edge->m_prev = prevTwin->m_prev;
2180 edge->m_next = prevEdge;
2181
2182 edge->m_twin->m_prev = prevEdge->m_prev;
2183 edge->m_twin->m_next = prevTwin;
2184
2185 prevTwin->m_prev->m_next = edge;
2186 prevTwin->m_prev = edge->m_twin;
2187
2188 prevEdge->m_prev->m_next = edge->m_twin;
2189 prevEdge->m_prev = edge;
2190
2191 edge->m_next->m_incidentFace = edge->m_incidentFace;
2192 edge->m_prev->m_incidentFace = edge->m_incidentFace;
2193
2194 edge->m_twin->m_next->m_incidentFace = edge->m_twin->m_incidentFace;
2195 edge->m_twin->m_prev->m_incidentFace = edge->m_twin->m_incidentFace;
2196
2197
2198 #ifdef __ENABLE_SANITY_CHECK
2199 HACD_ASSERT (SanityCheck ());
2200 #endif
2201
2202 return true;
2203 }
2204
2205
2206
GetConectedSurface(dgPolyhedra & polyhedra) const2207 bool dgPolyhedra::GetConectedSurface (dgPolyhedra &polyhedra) const
2208 {
2209 if (!GetCount()) {
2210 return false;
2211 }
2212
2213 dgEdge* edge = NULL;
2214 Iterator iter(*this);
2215 for (iter.Begin (); iter; iter ++) {
2216 edge = &(*iter);
2217 if ((edge->m_mark < m_baseMark) && (edge->m_incidentFace > 0)) {
2218 break;
2219 }
2220 }
2221
2222 if (!iter) {
2223 return false;
2224 }
2225
2226 hacd::HaI32 faceIndex[4096];
2227 hacd::HaI64 faceDataIndex[4096];
2228 dgStack<dgEdge*> stackPool (GetCount());
2229 dgEdge** const stack = &stackPool[0];
2230
2231 hacd::HaI32 mark = IncLRU();
2232
2233 polyhedra.BeginFace ();
2234 stack[0] = edge;
2235 hacd::HaI32 index = 1;
2236 while (index) {
2237 index --;
2238 dgEdge* const edge = stack[index];
2239
2240 if (edge->m_mark == mark) {
2241 continue;
2242 }
2243
2244 hacd::HaI32 count = 0;
2245 dgEdge* ptr = edge;
2246 do {
2247 ptr->m_mark = mark;
2248 faceIndex[count] = ptr->m_incidentVertex;
2249 faceDataIndex[count] = hacd::HaI64 (ptr->m_userData);
2250 count ++;
2251 HACD_ASSERT (count < hacd::HaI32 ((sizeof (faceIndex)/sizeof(faceIndex[0]))));
2252
2253 if ((ptr->m_twin->m_incidentFace > 0) && (ptr->m_twin->m_mark != mark)) {
2254 stack[index] = ptr->m_twin;
2255 index ++;
2256 HACD_ASSERT (index < GetCount());
2257 }
2258
2259 ptr = ptr->m_next;
2260 } while (ptr != edge);
2261
2262 polyhedra.AddFace (count, &faceIndex[0], &faceDataIndex[0]);
2263 }
2264
2265 polyhedra.EndFace ();
2266
2267 return true;
2268 }
2269
2270
ChangeEdgeIncidentVertex(dgEdge * const edge,hacd::HaI32 newIndex)2271 void dgPolyhedra::ChangeEdgeIncidentVertex (dgEdge* const edge, hacd::HaI32 newIndex)
2272 {
2273 dgEdge* ptr = edge;
2274 do {
2275 dgTreeNode* node = GetNodeFromInfo(*ptr);
2276 dgPairKey Key0 (newIndex, ptr->m_twin->m_incidentVertex);
2277 ReplaceKey (node, Key0.GetVal());
2278
2279 node = GetNodeFromInfo(*ptr->m_twin);
2280 dgPairKey Key1 (ptr->m_twin->m_incidentVertex, newIndex);
2281 ReplaceKey (node, Key1.GetVal());
2282
2283 ptr->m_incidentVertex = newIndex;
2284
2285 ptr = ptr->m_twin->m_next;
2286 } while (ptr != edge);
2287 }
2288
2289
DeleteDegenerateFaces(const hacd::HaF64 * const pool,hacd::HaI32 strideInBytes,hacd::HaF64 area)2290 void dgPolyhedra::DeleteDegenerateFaces (const hacd::HaF64* const pool, hacd::HaI32 strideInBytes, hacd::HaF64 area)
2291 {
2292 if (!GetCount()) {
2293 return;
2294 }
2295
2296 #ifdef __ENABLE_SANITY_CHECK
2297 HACD_ASSERT (SanityCheck ());
2298 #endif
2299 dgStack <dgPolyhedra::dgTreeNode*> faceArrayPool(GetCount() / 2 + 100);
2300
2301 hacd::HaI32 count = 0;
2302 dgPolyhedra::dgTreeNode** const faceArray = &faceArrayPool[0];
2303 hacd::HaI32 mark = IncLRU();
2304 Iterator iter (*this);
2305 for (iter.Begin(); iter; iter ++) {
2306 dgEdge* const edge = &(*iter);
2307
2308 if ((edge->m_mark != mark) && (edge->m_incidentFace > 0)) {
2309 faceArray[count] = iter.GetNode();
2310 count ++;
2311 dgEdge* ptr = edge;
2312 do {
2313 ptr->m_mark = mark;
2314 ptr = ptr->m_next;
2315 } while (ptr != edge);
2316 }
2317 }
2318
2319 hacd::HaF64 area2 = area * area;
2320 area2 *= hacd::HaF64 (4.0f);
2321
2322 for (hacd::HaI32 i = 0; i < count; i ++) {
2323 dgPolyhedra::dgTreeNode* const faceNode = faceArray[i];
2324 dgEdge* const edge = &faceNode->GetInfo();
2325
2326 dgBigVector normal (FaceNormal (edge, pool, strideInBytes));
2327
2328 hacd::HaF64 faceArea = normal % normal;
2329 if (faceArea < area2) {
2330 DeleteFace (edge);
2331 }
2332 }
2333
2334 #ifdef __ENABLE_SANITY_CHECK
2335 mark = IncLRU();
2336 for (iter.Begin(); iter; iter ++) {
2337 dgEdge* const edge = &(*iter);
2338 if ((edge->m_mark != mark) && (edge->m_incidentFace > 0)) {
2339 //HACD_ASSERT (edge->m_next->m_next->m_next == edge);
2340 dgEdge* ptr = edge;
2341 do {
2342 ptr->m_mark = mark;
2343 ptr = ptr->m_next;
2344 } while (ptr != edge);
2345
2346 dgBigVector normal (FaceNormal (edge, pool, strideInBytes));
2347
2348 hacd::HaF64 faceArea = normal % normal;
2349 HACD_ASSERT (faceArea >= area2);
2350 }
2351 }
2352 HACD_ASSERT (SanityCheck ());
2353 #endif
2354 }
2355
2356
NormalizeVertex(hacd::HaI32 count,dgBigVector * const dst,const hacd::HaF64 * const src,hacd::HaI32 stride)2357 static void NormalizeVertex (hacd::HaI32 count, dgBigVector* const dst, const hacd::HaF64* const src, hacd::HaI32 stride)
2358 {
2359 // dgBigVector min;
2360 // dgBigVector max;
2361 // GetMinMax (min, max, src, count, hacd::HaI32 (stride * sizeof (hacd::HaF64)));
2362 // dgBigVector centre (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
2363 for (hacd::HaI32 i = 0; i < count; i ++) {
2364 // dst[i].m_x = centre.m_x + src[i * stride + 0];
2365 // dst[i].m_y = centre.m_y + src[i * stride + 1];
2366 // dst[i].m_z = centre.m_z + src[i * stride + 2];
2367 dst[i].m_x = src[i * stride + 0];
2368 dst[i].m_y = src[i * stride + 1];
2369 dst[i].m_z = src[i * stride + 2];
2370 dst[i].m_w= hacd::HaF64 (0.0f);
2371 }
2372 }
2373
EdgePlane(hacd::HaI32 i0,hacd::HaI32 i1,hacd::HaI32 i2,const dgBigVector * const pool)2374 static dgBigPlane EdgePlane (hacd::HaI32 i0, hacd::HaI32 i1, hacd::HaI32 i2, const dgBigVector* const pool)
2375 {
2376 const dgBigVector& p0 = pool[i0];
2377 const dgBigVector& p1 = pool[i1];
2378 const dgBigVector& p2 = pool[i2];
2379
2380 dgBigPlane plane (p0, p1, p2);
2381 hacd::HaF64 mag = sqrt (plane % plane);
2382 if (mag < hacd::HaF64 (1.0e-12f)) {
2383 mag = hacd::HaF64 (1.0e-12f);
2384 }
2385 mag = hacd::HaF64 (1.0f) / mag;
2386
2387 plane.m_x *= mag;
2388 plane.m_y *= mag;
2389 plane.m_z *= mag;
2390 plane.m_w *= mag;
2391
2392 return plane;
2393 }
2394
2395
UnboundedLoopPlane(hacd::HaI32 i0,hacd::HaI32 i1,hacd::HaI32 i2,const dgBigVector * const pool)2396 static dgBigPlane UnboundedLoopPlane (hacd::HaI32 i0, hacd::HaI32 i1, hacd::HaI32 i2, const dgBigVector* const pool)
2397 {
2398 const dgBigVector p0 = pool[i0];
2399 const dgBigVector p1 = pool[i1];
2400 const dgBigVector p2 = pool[i2];
2401 dgBigVector E0 (p1 - p0);
2402 dgBigVector E1 (p2 - p0);
2403
2404 dgBigVector N ((E0 * E1) * E0);
2405 hacd::HaF64 dist = - (N % p0);
2406 dgBigPlane plane (N, dist);
2407
2408 hacd::HaF64 mag = sqrt (plane % plane);
2409 if (mag < hacd::HaF64 (1.0e-12f)) {
2410 mag = hacd::HaF64 (1.0e-12f);
2411 }
2412 mag = hacd::HaF64 (10.0f) / mag;
2413
2414 plane.m_x *= mag;
2415 plane.m_y *= mag;
2416 plane.m_z *= mag;
2417 plane.m_w *= mag;
2418
2419 return plane;
2420 }
2421
2422
CalculateAllMetrics(const dgPolyhedra * const polyhedra,dgVertexCollapseVertexMetric * const table,const dgBigVector * const pool)2423 static void CalculateAllMetrics (const dgPolyhedra* const polyhedra, dgVertexCollapseVertexMetric* const table, const dgBigVector* const pool)
2424 {
2425 hacd::HaI32 edgeMark = polyhedra->IncLRU();
2426 dgPolyhedra::Iterator iter (*polyhedra);
2427 for (iter.Begin(); iter; iter ++) {
2428 dgEdge* const edge = &(*iter);
2429
2430 HACD_ASSERT (edge);
2431 if (edge->m_mark != edgeMark) {
2432
2433 if (edge->m_incidentFace > 0) {
2434 hacd::HaI32 i0 = edge->m_incidentVertex;
2435 hacd::HaI32 i1 = edge->m_next->m_incidentVertex;
2436 hacd::HaI32 i2 = edge->m_prev->m_incidentVertex;
2437
2438 dgBigPlane constrainPlane (EdgePlane (i0, i1, i2, pool));
2439 dgVertexCollapseVertexMetric tmp (constrainPlane);
2440
2441 dgEdge* ptr = edge;
2442 do {
2443 ptr->m_mark = edgeMark;
2444 i0 = ptr->m_incidentVertex;
2445 table[i0].Accumulate(tmp);
2446
2447 ptr = ptr->m_next;
2448 } while (ptr != edge);
2449
2450 } else {
2451 HACD_ASSERT (edge->m_twin->m_incidentFace > 0);
2452 hacd::HaI32 i0 = edge->m_twin->m_incidentVertex;
2453 hacd::HaI32 i1 = edge->m_twin->m_next->m_incidentVertex;
2454 hacd::HaI32 i2 = edge->m_twin->m_prev->m_incidentVertex;
2455
2456 edge->m_mark = edgeMark;
2457 dgBigPlane constrainPlane (UnboundedLoopPlane (i0, i1, i2, pool));
2458 dgVertexCollapseVertexMetric tmp (constrainPlane);
2459
2460 i0 = edge->m_incidentVertex;
2461 table[i0].Accumulate(tmp);
2462
2463 i0 = edge->m_twin->m_incidentVertex;
2464 table[i0].Accumulate(tmp);
2465 }
2466 }
2467 }
2468 }
2469
2470
EdgePenalty(const dgBigVector * const pool,dgEdge * const edge) const2471 hacd::HaF64 dgPolyhedra::EdgePenalty (const dgBigVector* const pool, dgEdge* const edge) const
2472 {
2473 hacd::HaI32 i0 = edge->m_incidentVertex;
2474 hacd::HaI32 i1 = edge->m_next->m_incidentVertex;
2475
2476 const dgBigVector& p0 = pool[i0];
2477 const dgBigVector& p1 = pool[i1];
2478 dgBigVector dp (p1 - p0);
2479
2480 hacd::HaF64 dot = dp % dp;
2481 if (dot < hacd::HaF64(1.0e-6f)) {
2482 return hacd::HaF64 (-1.0f);
2483 }
2484
2485 if ((edge->m_incidentFace > 0) && (edge->m_twin->m_incidentFace > 0)) {
2486 dgBigVector edgeNormal (FaceNormal (edge, &pool[0].m_x, sizeof (dgBigVector)));
2487 dgBigVector twinNormal (FaceNormal (edge->m_twin, &pool[0].m_x, sizeof (dgBigVector)));
2488
2489 hacd::HaF64 mag0 = edgeNormal % edgeNormal;
2490 hacd::HaF64 mag1 = twinNormal % twinNormal;
2491 if ((mag0 < hacd::HaF64 (1.0e-24f)) || (mag1 < hacd::HaF64 (1.0e-24f))) {
2492 return hacd::HaF64 (-1.0f);
2493 }
2494
2495 edgeNormal = edgeNormal.Scale (hacd::HaF64 (1.0f) / sqrt(mag0));
2496 twinNormal = twinNormal.Scale (hacd::HaF64 (1.0f) / sqrt(mag1));
2497
2498 dot = edgeNormal % twinNormal;
2499 if (dot < hacd::HaF64 (-0.9f)) {
2500 return hacd::HaF32 (-1.0f);
2501 }
2502
2503 dgEdge* ptr = edge;
2504 do {
2505 if ((ptr->m_incidentFace <= 0) || (ptr->m_twin->m_incidentFace <= 0)){
2506 dgEdge* const adj = edge->m_twin;
2507 ptr = edge;
2508 do {
2509 if ((ptr->m_incidentFace <= 0) || (ptr->m_twin->m_incidentFace <= 0)){
2510 return hacd::HaF32 (-1.0);
2511 }
2512 ptr = ptr->m_twin->m_next;
2513 } while (ptr != adj);
2514 }
2515 ptr = ptr->m_twin->m_next;
2516 } while (ptr != edge);
2517 }
2518
2519 hacd::HaI32 faceA = edge->m_incidentFace;
2520 hacd::HaI32 faceB = edge->m_twin->m_incidentFace;
2521
2522 i0 = edge->m_twin->m_incidentVertex;
2523 dgBigVector p (pool[i0].m_x, pool[i0].m_y, pool[i0].m_z, hacd::HaF32 (0.0f));
2524
2525 bool penalty = false;
2526 dgEdge* ptr = edge;
2527 do {
2528 dgEdge* const adj = ptr->m_twin;
2529
2530 hacd::HaI32 face = adj->m_incidentFace;
2531 if ((face != faceB) && (face != faceA) && (face >= 0) && (adj->m_next->m_incidentFace == face) && (adj->m_prev->m_incidentFace == face)){
2532
2533 hacd::HaI32 i0 = adj->m_next->m_incidentVertex;
2534 const dgBigVector& p0 = pool[i0];
2535
2536 hacd::HaI32 i1 = adj->m_incidentVertex;
2537 const dgBigVector& p1 = pool[i1];
2538
2539 hacd::HaI32 i2 = adj->m_prev->m_incidentVertex;
2540 const dgBigVector& p2 = pool[i2];
2541
2542 dgBigVector n0 ((p1 - p0) * (p2 - p0));
2543 dgBigVector n1 ((p1 - p) * (p2 - p));
2544
2545 // hacd::HaF64 mag0 = n0 % n0;
2546 // HACD_ASSERT (mag0 > hacd::HaF64(1.0e-16f));
2547 // mag0 = sqrt (mag0);
2548
2549 // hacd::HaF64 mag1 = n1 % n1;
2550 // mag1 = sqrt (mag1);
2551
2552 hacd::HaF64 dot = n0 % n1;
2553 if (dot < hacd::HaF64 (0.0f)) {
2554 // if (dot <= (mag0 * mag1 * hacd::HaF32 (0.707f)) || (mag0 > (hacd::HaF64(16.0f) * mag1))) {
2555 penalty = true;
2556 break;
2557 }
2558 }
2559
2560 ptr = ptr->m_twin->m_next;
2561 } while (ptr != edge);
2562
2563 hacd::HaF64 aspect = hacd::HaF32 (-1.0f);
2564 if (!penalty) {
2565 hacd::HaI32 i0 = edge->m_twin->m_incidentVertex;
2566 dgBigVector p0 (pool[i0]);
2567
2568 aspect = hacd::HaF32 (1.0f);
2569 for (dgEdge* ptr = edge->m_twin->m_next->m_twin->m_next; ptr != edge; ptr = ptr->m_twin->m_next) {
2570 if (ptr->m_incidentFace > 0) {
2571 hacd::HaI32 i0 = ptr->m_next->m_incidentVertex;
2572 const dgBigVector& p1 = pool[i0];
2573
2574 hacd::HaI32 i1 = ptr->m_prev->m_incidentVertex;
2575 const dgBigVector& p2 = pool[i1];
2576
2577 dgBigVector e0 (p1 - p0);
2578 dgBigVector e1 (p2 - p1);
2579 dgBigVector e2 (p0 - p2);
2580
2581 hacd::HaF64 mag0 = e0 % e0;
2582 hacd::HaF64 mag1 = e1 % e1;
2583 hacd::HaF64 mag2 = e2 % e2;
2584 hacd::HaF64 maxMag = GetMax (mag0, mag1, mag2);
2585 hacd::HaF64 minMag = GetMin (mag0, mag1, mag2);
2586 hacd::HaF64 ratio = minMag / maxMag;
2587
2588 if (ratio < aspect) {
2589 aspect = ratio;
2590 }
2591 }
2592 }
2593 aspect = sqrt (aspect);
2594 //aspect = 1.0f;
2595 }
2596
2597 return aspect;
2598 }
2599
CalculateVertexMetrics(dgVertexCollapseVertexMetric table[],const dgBigVector * const pool,dgEdge * const edge)2600 static void CalculateVertexMetrics (dgVertexCollapseVertexMetric table[], const dgBigVector* const pool, dgEdge* const edge)
2601 {
2602 hacd::HaI32 i0 = edge->m_incidentVertex;
2603
2604 // const dgBigVector& p0 = pool[i0];
2605 table[i0].Clear ();
2606 dgEdge* ptr = edge;
2607 do {
2608
2609 if (ptr->m_incidentFace > 0) {
2610 hacd::HaI32 i1 = ptr->m_next->m_incidentVertex;
2611 hacd::HaI32 i2 = ptr->m_prev->m_incidentVertex;
2612 dgBigPlane constrainPlane (EdgePlane (i0, i1, i2, pool));
2613 table[i0].Accumulate (constrainPlane);
2614
2615 } else {
2616 hacd::HaI32 i1 = ptr->m_twin->m_incidentVertex;
2617 hacd::HaI32 i2 = ptr->m_twin->m_prev->m_incidentVertex;
2618 dgBigPlane constrainPlane (UnboundedLoopPlane (i0, i1, i2, pool));
2619 table[i0].Accumulate (constrainPlane);
2620
2621 i1 = ptr->m_prev->m_incidentVertex;
2622 i2 = ptr->m_prev->m_twin->m_prev->m_incidentVertex;
2623 constrainPlane = UnboundedLoopPlane (i0, i1, i2, pool);
2624 table[i0].Accumulate (constrainPlane);
2625 }
2626
2627 ptr = ptr->m_twin->m_next;
2628 } while (ptr != edge);
2629 }
2630
RemoveHalfEdge(dgPolyhedra * const polyhedra,dgEdge * const edge)2631 static void RemoveHalfEdge (dgPolyhedra* const polyhedra, dgEdge* const edge)
2632 {
2633 dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle *) IntToPointer (edge->m_userData);
2634 if (handle) {
2635 handle->m_edge = NULL;
2636 }
2637
2638 dgPolyhedra::dgTreeNode* const node = polyhedra->GetNodeFromInfo(*edge);
2639 HACD_ASSERT (node);
2640 polyhedra->Remove (node);
2641 }
2642
2643
CollapseEdge(dgPolyhedra * const polyhedra,dgEdge * const edge)2644 static dgEdge* CollapseEdge(dgPolyhedra* const polyhedra, dgEdge* const edge)
2645 {
2646 hacd::HaI32 v0 = edge->m_incidentVertex;
2647 hacd::HaI32 v1 = edge->m_twin->m_incidentVertex;
2648
2649 #ifdef __ENABLE_SANITY_CHECK
2650 dgPolyhedra::dgPairKey TwinKey (v1, v0);
2651 dgPolyhedra::dgTreeNode* const node = polyhedra->Find (TwinKey.GetVal());
2652 dgEdge* const twin1 = node ? &node->GetInfo() : NULL;
2653 HACD_ASSERT (twin1);
2654 HACD_ASSERT (edge->m_twin == twin1);
2655 HACD_ASSERT (twin1->m_twin == edge);
2656 HACD_ASSERT (edge->m_incidentFace != 0);
2657 HACD_ASSERT (twin1->m_incidentFace != 0);
2658 #endif
2659
2660
2661 dgEdge* retEdge = edge->m_twin->m_prev->m_twin;
2662 if (retEdge == edge->m_twin->m_next) {
2663 return NULL;
2664 }
2665 if (retEdge == edge->m_twin) {
2666 return NULL;
2667 }
2668 if (retEdge == edge->m_next) {
2669 retEdge = edge->m_prev->m_twin;
2670 if (retEdge == edge->m_twin->m_next) {
2671 return NULL;
2672 }
2673 if (retEdge == edge->m_twin) {
2674 return NULL;
2675 }
2676 }
2677
2678 dgEdge* lastEdge = NULL;
2679 dgEdge* firstEdge = NULL;
2680 if ((edge->m_incidentFace >= 0) && (edge->m_twin->m_incidentFace >= 0)) {
2681 lastEdge = edge->m_prev->m_twin;
2682 firstEdge = edge->m_twin->m_next->m_twin->m_next;
2683 } else if (edge->m_twin->m_incidentFace >= 0) {
2684 firstEdge = edge->m_twin->m_next->m_twin->m_next;
2685 lastEdge = edge;
2686 } else {
2687 lastEdge = edge->m_prev->m_twin;
2688 firstEdge = edge->m_twin->m_next;
2689 }
2690
2691 for (dgEdge* ptr = firstEdge; ptr != lastEdge; ptr = ptr->m_twin->m_next) {
2692 dgEdge* badEdge = polyhedra->FindEdge (edge->m_twin->m_incidentVertex, ptr->m_twin->m_incidentVertex);
2693 if (badEdge) {
2694 return NULL;
2695 }
2696 }
2697
2698 dgEdge* const twin = edge->m_twin;
2699 if (twin->m_next == twin->m_prev->m_prev) {
2700 twin->m_prev->m_twin->m_twin = twin->m_next->m_twin;
2701 twin->m_next->m_twin->m_twin = twin->m_prev->m_twin;
2702
2703 RemoveHalfEdge (polyhedra, twin->m_prev);
2704 RemoveHalfEdge (polyhedra, twin->m_next);
2705 } else {
2706 twin->m_next->m_prev = twin->m_prev;
2707 twin->m_prev->m_next = twin->m_next;
2708 }
2709
2710 if (edge->m_next == edge->m_prev->m_prev) {
2711 edge->m_next->m_twin->m_twin = edge->m_prev->m_twin;
2712 edge->m_prev->m_twin->m_twin = edge->m_next->m_twin;
2713 RemoveHalfEdge (polyhedra, edge->m_next);
2714 RemoveHalfEdge (polyhedra, edge->m_prev);
2715 } else {
2716 edge->m_next->m_prev = edge->m_prev;
2717 edge->m_prev->m_next = edge->m_next;
2718 }
2719
2720 HACD_ASSERT (twin->m_twin->m_incidentVertex == v0);
2721 HACD_ASSERT (edge->m_twin->m_incidentVertex == v1);
2722 RemoveHalfEdge (polyhedra, twin);
2723 RemoveHalfEdge (polyhedra, edge);
2724
2725 dgEdge* ptr = retEdge;
2726 do {
2727 dgPolyhedra::dgPairKey pairKey (v0, ptr->m_twin->m_incidentVertex);
2728
2729 dgPolyhedra::dgTreeNode* node = polyhedra->Find (pairKey.GetVal());
2730 if (node) {
2731 if (&node->GetInfo() == ptr) {
2732 dgPolyhedra::dgPairKey key (v1, ptr->m_twin->m_incidentVertex);
2733 ptr->m_incidentVertex = v1;
2734 node = polyhedra->ReplaceKey (node, key.GetVal());
2735 HACD_ASSERT (node);
2736 }
2737 }
2738
2739 dgPolyhedra::dgPairKey TwinKey (ptr->m_twin->m_incidentVertex, v0);
2740 node = polyhedra->Find (TwinKey.GetVal());
2741 if (node) {
2742 if (&node->GetInfo() == ptr->m_twin) {
2743 dgPolyhedra::dgPairKey key (ptr->m_twin->m_incidentVertex, v1);
2744 node = polyhedra->ReplaceKey (node, key.GetVal());
2745 HACD_ASSERT (node);
2746 }
2747 }
2748
2749 ptr = ptr->m_twin->m_next;
2750 } while (ptr != retEdge);
2751
2752 return retEdge;
2753 }
2754
2755
2756
Optimize(const hacd::HaF64 * const array,hacd::HaI32 strideInBytes,hacd::HaF64 tol)2757 void dgPolyhedra::Optimize (const hacd::HaF64* const array, hacd::HaI32 strideInBytes, hacd::HaF64 tol)
2758 {
2759 dgList <dgEdgeCollapseEdgeHandle>::dgListNode *handleNodePtr;
2760
2761 hacd::HaI32 stride = hacd::HaI32 (strideInBytes / sizeof (hacd::HaF64));
2762
2763 #ifdef __ENABLE_SANITY_CHECK
2764 HACD_ASSERT (SanityCheck ());
2765 #endif
2766
2767 hacd::HaI32 edgeCount = GetEdgeCount() * 4 + 1024 * 16;
2768 hacd::HaI32 maxVertexIndex = GetLastVertexIndex();
2769
2770 dgStack<dgBigVector> vertexPool (maxVertexIndex);
2771 dgStack<dgVertexCollapseVertexMetric> vertexMetrics (maxVertexIndex + 512);
2772
2773 dgList <dgEdgeCollapseEdgeHandle> edgeHandleList;
2774 dgStack<char> heapPool (2 * edgeCount * hacd::HaI32 (sizeof (hacd::HaF64) + sizeof (dgEdgeCollapseEdgeHandle*) + sizeof (hacd::HaI32)));
2775 dgDownHeap<dgList <dgEdgeCollapseEdgeHandle>::dgListNode* , hacd::HaF64> bigHeapArray(&heapPool[0], heapPool.GetSizeInBytes());
2776
2777 NormalizeVertex (maxVertexIndex, &vertexPool[0], array, stride);
2778 memset (&vertexMetrics[0], 0, maxVertexIndex * sizeof (dgVertexCollapseVertexMetric));
2779 CalculateAllMetrics (this, &vertexMetrics[0], &vertexPool[0]);
2780
2781
2782 hacd::HaF64 tol2 = tol * tol;
2783 Iterator iter (*this);
2784 for (iter.Begin(); iter; iter ++) {
2785 dgEdge* const edge = &(*iter);
2786
2787 edge->m_userData = 0;
2788 hacd::HaI32 index0 = edge->m_incidentVertex;
2789 hacd::HaI32 index1 = edge->m_twin->m_incidentVertex;
2790
2791 dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
2792 dgVector p (&vertexPool[index1].m_x);
2793 hacd::HaF64 cost = metric.Evalue (p);
2794 if (cost < tol2) {
2795 cost = EdgePenalty (&vertexPool[0], edge);
2796
2797 if (cost > hacd::HaF64 (0.0f)) {
2798 dgEdgeCollapseEdgeHandle handle (edge);
2799 handleNodePtr = edgeHandleList.Addtop (handle);
2800 bigHeapArray.Push (handleNodePtr, cost);
2801 }
2802 }
2803 }
2804
2805
2806 while (bigHeapArray.GetCount()) {
2807 handleNodePtr = bigHeapArray[0];
2808
2809 dgEdge* edge = handleNodePtr->GetInfo().m_edge;
2810 bigHeapArray.Pop();
2811 edgeHandleList.Remove (handleNodePtr);
2812
2813 if (edge) {
2814 CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], edge);
2815
2816 hacd::HaI32 index0 = edge->m_incidentVertex;
2817 hacd::HaI32 index1 = edge->m_twin->m_incidentVertex;
2818 dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
2819 dgBigVector p (vertexPool[index1]);
2820
2821 if ((metric.Evalue (p) < tol2) && (EdgePenalty (&vertexPool[0], edge) > hacd::HaF64 (0.0f))) {
2822
2823 #ifdef __ENABLE_SANITY_CHECK
2824 HACD_ASSERT (SanityCheck ());
2825 #endif
2826
2827 edge = CollapseEdge(this, edge);
2828
2829 #ifdef __ENABLE_SANITY_CHECK
2830 HACD_ASSERT (SanityCheck ());
2831 #endif
2832 if (edge) {
2833 // Update vertex metrics
2834 CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], edge);
2835
2836 // Update metrics for all surrounding vertex
2837 dgEdge* ptr = edge;
2838 do {
2839 CalculateVertexMetrics (&vertexMetrics[0], &vertexPool[0], ptr->m_twin);
2840 ptr = ptr->m_twin->m_next;
2841 } while (ptr != edge);
2842
2843 // calculate edge cost of all incident edges
2844 hacd::HaI32 mark = IncLRU();
2845 ptr = edge;
2846 do {
2847 HACD_ASSERT (ptr->m_mark != mark);
2848 ptr->m_mark = mark;
2849
2850 index0 = ptr->m_incidentVertex;
2851 index1 = ptr->m_twin->m_incidentVertex;
2852
2853 dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
2854 dgBigVector p (vertexPool[index1]);
2855
2856 hacd::HaF64 cost = hacd::HaF32 (-1.0f);
2857 if (metric.Evalue (p) < tol2) {
2858 cost = EdgePenalty (&vertexPool[0], ptr);
2859 }
2860
2861 if (cost > hacd::HaF64 (0.0f)) {
2862 dgEdgeCollapseEdgeHandle handle (ptr);
2863 handleNodePtr = edgeHandleList.Addtop (handle);
2864 bigHeapArray.Push (handleNodePtr, cost);
2865 } else {
2866 dgEdgeCollapseEdgeHandle* const handle = (dgEdgeCollapseEdgeHandle*)IntToPointer (ptr->m_userData);
2867 if (handle) {
2868 handle->m_edge = NULL;
2869 }
2870 ptr->m_userData = hacd::HaU64 (NULL);
2871
2872 }
2873
2874 ptr = ptr->m_twin->m_next;
2875 } while (ptr != edge);
2876
2877
2878 // calculate edge cost of all incident edges to a surrounding vertex
2879 ptr = edge;
2880 do {
2881 dgEdge* const incidentEdge = ptr->m_twin;
2882
2883 dgEdge* ptr1 = incidentEdge;
2884 do {
2885 index0 = ptr1->m_incidentVertex;
2886 index1 = ptr1->m_twin->m_incidentVertex;
2887
2888 if (ptr1->m_mark != mark) {
2889 ptr1->m_mark = mark;
2890 dgVertexCollapseVertexMetric &metric = vertexMetrics[index0];
2891 dgBigVector p (vertexPool[index1]);
2892
2893 hacd::HaF64 cost = hacd::HaF32 (-1.0f);
2894 if (metric.Evalue (p) < tol2) {
2895 cost = EdgePenalty (&vertexPool[0], ptr1);
2896 }
2897
2898 if (cost > hacd::HaF64 (0.0f)) {
2899 HACD_ASSERT (cost > hacd::HaF64(0.0f));
2900 dgEdgeCollapseEdgeHandle handle (ptr1);
2901 handleNodePtr = edgeHandleList.Addtop (handle);
2902 bigHeapArray.Push (handleNodePtr, cost);
2903 } else {
2904 dgEdgeCollapseEdgeHandle *handle;
2905 handle = (dgEdgeCollapseEdgeHandle*)IntToPointer (ptr1->m_userData);
2906 if (handle) {
2907 handle->m_edge = NULL;
2908 }
2909 ptr1->m_userData = hacd::HaU64 (NULL);
2910
2911 }
2912 }
2913
2914 if (ptr1->m_twin->m_mark != mark) {
2915 ptr1->m_twin->m_mark = mark;
2916 dgVertexCollapseVertexMetric &metric = vertexMetrics[index1];
2917 dgBigVector p (vertexPool[index0]);
2918
2919 hacd::HaF64 cost = hacd::HaF32 (-1.0f);
2920 if (metric.Evalue (p) < tol2) {
2921 cost = EdgePenalty (&vertexPool[0], ptr1->m_twin);
2922 }
2923
2924 if (cost > hacd::HaF64 (0.0f)) {
2925 HACD_ASSERT (cost > hacd::HaF64(0.0f));
2926 dgEdgeCollapseEdgeHandle handle (ptr1->m_twin);
2927 handleNodePtr = edgeHandleList.Addtop (handle);
2928 bigHeapArray.Push (handleNodePtr, cost);
2929 } else {
2930 dgEdgeCollapseEdgeHandle *handle;
2931 handle = (dgEdgeCollapseEdgeHandle*) IntToPointer (ptr1->m_twin->m_userData);
2932 if (handle) {
2933 handle->m_edge = NULL;
2934 }
2935 ptr1->m_twin->m_userData = hacd::HaU64 (NULL);
2936
2937 }
2938 }
2939
2940 ptr1 = ptr1->m_twin->m_next;
2941 } while (ptr1 != incidentEdge);
2942
2943 ptr = ptr->m_twin->m_next;
2944 } while (ptr != edge);
2945 }
2946 }
2947 }
2948 }
2949 }
2950
2951
FindEarTip(dgEdge * const face,const hacd::HaF64 * const pool,hacd::HaI32 stride,dgDownHeap<dgEdge *,hacd::HaF64> & heap,const dgBigVector & normal) const2952 dgEdge* dgPolyhedra::FindEarTip (dgEdge* const face, const hacd::HaF64* const pool, hacd::HaI32 stride, dgDownHeap<dgEdge*, hacd::HaF64>& heap, const dgBigVector &normal) const
2953 {
2954 dgEdge* ptr = face;
2955 dgBigVector p0 (&pool[ptr->m_prev->m_incidentVertex * stride]);
2956 dgBigVector p1 (&pool[ptr->m_incidentVertex * stride]);
2957 dgBigVector d0 (p1 - p0);
2958 hacd::HaF64 f = sqrt (d0 % d0);
2959 if (f < hacd::HaF64 (1.0e-10f)) {
2960 f = hacd::HaF64 (1.0e-10f);
2961 }
2962 d0 = d0.Scale (hacd::HaF64 (1.0f) / f);
2963
2964 hacd::HaF64 minAngle = hacd::HaF32 (10.0f);
2965 do {
2966 dgBigVector p2 (&pool [ptr->m_next->m_incidentVertex * stride]);
2967 dgBigVector d1 (p2 - p1);
2968 hacd::HaF32 f = dgSqrt (d1 % d1);
2969 if (f < hacd::HaF32 (1.0e-10f)) {
2970 f = hacd::HaF32 (1.0e-10f);
2971 }
2972 d1 = d1.Scale (hacd::HaF32 (1.0f) / f);
2973 dgBigVector n (d0 * d1);
2974
2975 hacd::HaF64 angle = normal % n;
2976 if (angle >= hacd::HaF64 (0.0f)) {
2977 heap.Push (ptr, angle);
2978 }
2979
2980 if (angle < minAngle) {
2981 minAngle = angle;
2982 }
2983
2984 d0 = d1;
2985 p1 = p2;
2986 ptr = ptr->m_next;
2987 } while (ptr != face);
2988
2989 if (minAngle > hacd::HaF32 (0.1f)) {
2990 return heap[0];
2991 }
2992
2993 dgEdge* ear = NULL;
2994 while (heap.GetCount()) {
2995 ear = heap[0];
2996 heap.Pop();
2997
2998 if (FindEdge (ear->m_prev->m_incidentVertex, ear->m_next->m_incidentVertex)) {
2999 continue;
3000 }
3001
3002 dgBigVector p0 (&pool [ear->m_prev->m_incidentVertex * stride]);
3003 dgBigVector p1 (&pool [ear->m_incidentVertex * stride]);
3004 dgBigVector p2 (&pool [ear->m_next->m_incidentVertex * stride]);
3005
3006 dgBigVector p10 (p1 - p0);
3007 dgBigVector p21 (p2 - p1);
3008 dgBigVector p02 (p0 - p2);
3009
3010 for (ptr = ear->m_next->m_next; ptr != ear->m_prev; ptr = ptr->m_next) {
3011 dgBigVector p (&pool [ptr->m_incidentVertex * stride]);
3012
3013 hacd::HaF64 side = ((p - p0) * p10) % normal;
3014 if (side < hacd::HaF64 (0.05f)) {
3015 side = ((p - p1) * p21) % normal;
3016 if (side < hacd::HaF64 (0.05f)) {
3017 side = ((p - p2) * p02) % normal;
3018 if (side < hacd::HaF32 (0.05f)) {
3019 break;
3020 }
3021 }
3022 }
3023 }
3024
3025 if (ptr == ear->m_prev) {
3026 break;
3027 }
3028 }
3029
3030 return ear;
3031 }
3032
3033
3034
3035
3036
3037 //dgEdge* TriangulateFace (dgPolyhedra& polyhedra, dgEdge* face, const hacd::HaF32* const pool, hacd::HaI32 stride, dgDownHeap<dgEdge*, hacd::HaF32>& heap, dgVector* const faceNormalOut)
TriangulateFace(dgEdge * face,const hacd::HaF64 * const pool,hacd::HaI32 stride,dgDownHeap<dgEdge *,hacd::HaF64> & heap,dgBigVector * const faceNormalOut)3038 dgEdge* dgPolyhedra::TriangulateFace (dgEdge* face, const hacd::HaF64* const pool, hacd::HaI32 stride, dgDownHeap<dgEdge*, hacd::HaF64>& heap, dgBigVector* const faceNormalOut)
3039 {
3040 dgEdge* perimeter [1024 * 16];
3041 dgEdge* ptr = face;
3042 hacd::HaI32 perimeterCount = 0;
3043 do {
3044 perimeter[perimeterCount] = ptr;
3045 perimeterCount ++;
3046 HACD_ASSERT (perimeterCount < hacd::HaI32 (sizeof (perimeter) / sizeof (perimeter[0])));
3047 ptr = ptr->m_next;
3048 } while (ptr != face);
3049 perimeter[perimeterCount] = face;
3050 HACD_ASSERT ((perimeterCount + 1) < hacd::HaI32 (sizeof (perimeter) / sizeof (perimeter[0])));
3051
3052 dgBigVector normal (FaceNormal (face, pool, hacd::HaI32 (stride * sizeof (hacd::HaF64))));
3053
3054 hacd::HaF64 dot = normal % normal;
3055 if (dot < hacd::HaF64 (1.0e-12f)) {
3056 if (faceNormalOut) {
3057 *faceNormalOut = dgBigVector (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
3058 }
3059 return face;
3060 }
3061 normal = normal.Scale (hacd::HaF64 (1.0f) / sqrt (dot));
3062 if (faceNormalOut) {
3063 *faceNormalOut = normal;
3064 }
3065
3066
3067 while (face->m_next->m_next->m_next != face) {
3068 dgEdge* const ear = FindEarTip (face, pool, stride, heap, normal);
3069 if (!ear) {
3070 return face;
3071 }
3072 if ((face == ear) || (face == ear->m_prev)) {
3073 face = ear->m_prev->m_prev;
3074 }
3075 dgEdge* const edge = AddHalfEdge (ear->m_next->m_incidentVertex, ear->m_prev->m_incidentVertex);
3076 if (!edge) {
3077 return face;
3078 }
3079 dgEdge* const twin = AddHalfEdge (ear->m_prev->m_incidentVertex, ear->m_next->m_incidentVertex);
3080 if (!twin) {
3081 return face;
3082 }
3083 HACD_ASSERT (twin);
3084
3085
3086 edge->m_mark = ear->m_mark;
3087 edge->m_userData = ear->m_next->m_userData;
3088 edge->m_incidentFace = ear->m_incidentFace;
3089
3090 twin->m_mark = ear->m_mark;
3091 twin->m_userData = ear->m_prev->m_userData;
3092 twin->m_incidentFace = ear->m_incidentFace;
3093
3094 edge->m_twin = twin;
3095 twin->m_twin = edge;
3096
3097 twin->m_prev = ear->m_prev->m_prev;
3098 twin->m_next = ear->m_next;
3099 ear->m_prev->m_prev->m_next = twin;
3100 ear->m_next->m_prev = twin;
3101
3102 edge->m_next = ear->m_prev;
3103 edge->m_prev = ear;
3104 ear->m_prev->m_prev = edge;
3105 ear->m_next = edge;
3106
3107 heap.Flush ();
3108 }
3109 return NULL;
3110 }
3111
3112
MarkAdjacentCoplanarFaces(dgPolyhedra & polyhedraOut,dgEdge * const face,const hacd::HaF64 * const pool,hacd::HaI32 strideInBytes)3113 void dgPolyhedra::MarkAdjacentCoplanarFaces (dgPolyhedra& polyhedraOut, dgEdge* const face, const hacd::HaF64* const pool, hacd::HaI32 strideInBytes)
3114 {
3115 const hacd::HaF64 normalDeviation = hacd::HaF64 (0.9999f);
3116 const hacd::HaF64 distanceFromPlane = hacd::HaF64 (1.0f / 128.0f);
3117
3118 hacd::HaI32 faceIndex[1024 * 4];
3119 dgEdge* stack[1024 * 4];
3120 dgEdge* deleteEdge[1024 * 4];
3121
3122 hacd::HaI32 deleteCount = 1;
3123 deleteEdge[0] = face;
3124 hacd::HaI32 stride = hacd::HaI32 (strideInBytes / sizeof (hacd::HaF64));
3125
3126 HACD_ASSERT (face->m_incidentFace > 0);
3127
3128 dgBigVector normalAverage (FaceNormal (face, pool, strideInBytes));
3129 hacd::HaF64 dot = normalAverage % normalAverage;
3130 if (dot > hacd::HaF64 (1.0e-12f)) {
3131 hacd::HaI32 testPointsCount = 1;
3132 dot = hacd::HaF64 (1.0f) / sqrt (dot);
3133 dgBigVector normal (normalAverage.Scale (dot));
3134
3135 dgBigVector averageTestPoint (&pool[face->m_incidentVertex * stride]);
3136 dgBigPlane testPlane(normal, - (averageTestPoint % normal));
3137
3138 polyhedraOut.BeginFace();
3139
3140 IncLRU();
3141 hacd::HaI32 faceMark = IncLRU();
3142
3143 hacd::HaI32 faceIndexCount = 0;
3144 dgEdge* ptr = face;
3145 do {
3146 ptr->m_mark = faceMark;
3147 faceIndex[faceIndexCount] = ptr->m_incidentVertex;
3148 faceIndexCount ++;
3149 HACD_ASSERT (faceIndexCount < hacd::HaI32 (sizeof (faceIndex) / sizeof (faceIndex[0])));
3150 ptr = ptr ->m_next;
3151 } while (ptr != face);
3152 polyhedraOut.AddFace(faceIndexCount, faceIndex);
3153
3154 hacd::HaI32 index = 1;
3155 deleteCount = 0;
3156 stack[0] = face;
3157 while (index) {
3158 index --;
3159 dgEdge* const face = stack[index];
3160 deleteEdge[deleteCount] = face;
3161 deleteCount ++;
3162 HACD_ASSERT (deleteCount < hacd::HaI32 (sizeof (deleteEdge) / sizeof (deleteEdge[0])));
3163 HACD_ASSERT (face->m_next->m_next->m_next == face);
3164
3165 dgEdge* edge = face;
3166 do {
3167 dgEdge* const ptr = edge->m_twin;
3168 if (ptr->m_incidentFace > 0) {
3169 if (ptr->m_mark != faceMark) {
3170 dgEdge* ptr1 = ptr;
3171 faceIndexCount = 0;
3172 do {
3173 ptr1->m_mark = faceMark;
3174 faceIndex[faceIndexCount] = ptr1->m_incidentVertex;
3175 HACD_ASSERT (faceIndexCount < hacd::HaI32 (sizeof (faceIndex) / sizeof (faceIndex[0])));
3176 faceIndexCount ++;
3177 ptr1 = ptr1 ->m_next;
3178 } while (ptr1 != ptr);
3179
3180 dgBigVector normal1 (FaceNormal (ptr, pool, strideInBytes));
3181 dot = normal1 % normal1;
3182 if (dot < hacd::HaF64 (1.0e-12f)) {
3183 deleteEdge[deleteCount] = ptr;
3184 deleteCount ++;
3185 HACD_ASSERT (deleteCount < hacd::HaI32 (sizeof (deleteEdge) / sizeof (deleteEdge[0])));
3186 } else {
3187 //normal1 = normal1.Scale (hacd::HaF64 (1.0f) / sqrt (dot));
3188 dgBigVector testNormal (normal1.Scale (hacd::HaF64 (1.0f) / sqrt (dot)));
3189 dot = normal % testNormal;
3190 if (dot >= normalDeviation) {
3191 dgBigVector testPoint (&pool[ptr->m_prev->m_incidentVertex * stride]);
3192 hacd::HaF64 dist = fabs (testPlane.Evalue (testPoint));
3193 if (dist < distanceFromPlane) {
3194 testPointsCount ++;
3195
3196 averageTestPoint += testPoint;
3197 testPoint = averageTestPoint.Scale (hacd::HaF64 (1.0f) / hacd::HaF64(testPointsCount));
3198
3199 normalAverage += normal1;
3200 testNormal = normalAverage.Scale (hacd::HaF64 (1.0f) / sqrt (normalAverage % normalAverage));
3201 testPlane = dgBigPlane (testNormal, - (testPoint % testNormal));
3202
3203 polyhedraOut.AddFace(faceIndexCount, faceIndex);;
3204 stack[index] = ptr;
3205 index ++;
3206 HACD_ASSERT (index < hacd::HaI32 (sizeof (stack) / sizeof (stack[0])));
3207 }
3208 }
3209 }
3210 }
3211 }
3212
3213 edge = edge->m_next;
3214 } while (edge != face);
3215 }
3216 polyhedraOut.EndFace();
3217 }
3218
3219 for (hacd::HaI32 index = 0; index < deleteCount; index ++) {
3220 DeleteFace (deleteEdge[index]);
3221 }
3222 }
3223
3224
RefineTriangulation(const hacd::HaF64 * const vertex,hacd::HaI32 stride,dgBigVector * const normal,hacd::HaI32 perimeterCount,dgEdge ** const perimeter)3225 void dgPolyhedra::RefineTriangulation (const hacd::HaF64* const vertex, hacd::HaI32 stride, dgBigVector* const normal, hacd::HaI32 perimeterCount, dgEdge** const perimeter)
3226 {
3227 dgList<dgDiagonalEdge> dignonals;
3228
3229 for (hacd::HaI32 i = 1; i <= perimeterCount; i ++) {
3230 dgEdge* const last = perimeter[i - 1];
3231 for (dgEdge* ptr = perimeter[i]->m_prev; ptr != last; ptr = ptr->m_twin->m_prev) {
3232 dgList<dgDiagonalEdge>::dgListNode* node = dignonals.GetFirst();
3233 for (; node; node = node->GetNext()) {
3234 const dgDiagonalEdge& key = node->GetInfo();
3235 if (((key.m_i0 == ptr->m_incidentVertex) && (key.m_i1 == ptr->m_twin->m_incidentVertex)) ||
3236 ((key.m_i1 == ptr->m_incidentVertex) && (key.m_i0 == ptr->m_twin->m_incidentVertex))) {
3237 break;
3238 }
3239 }
3240 if (!node) {
3241 dgDiagonalEdge key (ptr);
3242 dignonals.Append(key);
3243 }
3244 }
3245 }
3246
3247 dgEdge* const face = perimeter[0];
3248 hacd::HaI32 i0 = face->m_incidentVertex * stride;
3249 hacd::HaI32 i1 = face->m_next->m_incidentVertex * stride;
3250 dgBigVector p0 (vertex[i0], vertex[i0 + 1], vertex[i0 + 2], hacd::HaF32 (0.0f));
3251 dgBigVector p1 (vertex[i1], vertex[i1 + 1], vertex[i1 + 2], hacd::HaF32 (0.0f));
3252
3253 dgBigVector p1p0 (p1 - p0);
3254 hacd::HaF64 mag2 = p1p0 % p1p0;
3255 for (dgEdge* ptr = face->m_next->m_next; mag2 < hacd::HaF32 (1.0e-12f); ptr = ptr->m_next) {
3256 hacd::HaI32 i1 = ptr->m_incidentVertex * stride;
3257 dgBigVector p1 (vertex[i1], vertex[i1 + 1], vertex[i1 + 2], hacd::HaF32 (0.0f));
3258 p1p0 = p1 - p0;
3259 mag2 = p1p0 % p1p0;
3260 }
3261
3262 dgMatrix matrix (dgGetIdentityMatrix());
3263 matrix.m_posit = p0;
3264 matrix.m_front = dgVector (p1p0.Scale (hacd::HaF64 (1.0f) / sqrt (mag2)));
3265 matrix.m_right = dgVector (normal->Scale (hacd::HaF64 (1.0f) / sqrt (*normal % *normal)));
3266 matrix.m_up = matrix.m_right * matrix.m_front;
3267 matrix = matrix.Inverse();
3268 matrix.m_posit.m_w = hacd::HaF32 (1.0f);
3269
3270 hacd::HaI32 maxCount = dignonals.GetCount() * dignonals.GetCount();
3271 while (dignonals.GetCount() && maxCount) {
3272 maxCount --;
3273 dgList<dgDiagonalEdge>::dgListNode* const node = dignonals.GetFirst();
3274 dgDiagonalEdge key (node->GetInfo());
3275 dignonals.Remove(node);
3276 dgEdge* const edge = FindEdge(key.m_i0, key.m_i1);
3277 if (edge) {
3278 hacd::HaI32 i0 = edge->m_incidentVertex * stride;
3279 hacd::HaI32 i1 = edge->m_next->m_incidentVertex * stride;
3280 hacd::HaI32 i2 = edge->m_next->m_next->m_incidentVertex * stride;
3281 hacd::HaI32 i3 = edge->m_twin->m_prev->m_incidentVertex * stride;
3282
3283 dgBigVector p0 (vertex[i0], vertex[i0 + 1], vertex[i0 + 2], hacd::HaF64 (0.0f));
3284 dgBigVector p1 (vertex[i1], vertex[i1 + 1], vertex[i1 + 2], hacd::HaF64 (0.0f));
3285 dgBigVector p2 (vertex[i2], vertex[i2 + 1], vertex[i2 + 2], hacd::HaF64 (0.0f));
3286 dgBigVector p3 (vertex[i3], vertex[i3 + 1], vertex[i3 + 2], hacd::HaF64 (0.0f));
3287
3288 p0 = matrix.TransformVector(p0);
3289 p1 = matrix.TransformVector(p1);
3290 p2 = matrix.TransformVector(p2);
3291 p3 = matrix.TransformVector(p3);
3292
3293 hacd::HaF64 circleTest[3][3];
3294 circleTest[0][0] = p0[0] - p3[0];
3295 circleTest[0][1] = p0[1] - p3[1];
3296 circleTest[0][2] = circleTest[0][0] * circleTest[0][0] + circleTest[0][1] * circleTest[0][1];
3297
3298 circleTest[1][0] = p1[0] - p3[0];
3299 circleTest[1][1] = p1[1] - p3[1];
3300 circleTest[1][2] = circleTest[1][0] * circleTest[1][0] + circleTest[1][1] * circleTest[1][1];
3301
3302 circleTest[2][0] = p2[0] - p3[0];
3303 circleTest[2][1] = p2[1] - p3[1];
3304 circleTest[2][2] = circleTest[2][0] * circleTest[2][0] + circleTest[2][1] * circleTest[2][1];
3305
3306 hacd::HaF64 error;
3307 hacd::HaF64 det = Determinant3x3 (circleTest, &error);
3308 if (det < hacd::HaF32 (0.0f)) {
3309 dgEdge* frontFace0 = edge->m_prev;
3310 dgEdge* backFace0 = edge->m_twin->m_prev;
3311
3312 FlipEdge(edge);
3313
3314 if (perimeterCount > 4) {
3315 dgEdge* backFace1 = backFace0->m_next;
3316 dgEdge* frontFace1 = frontFace0->m_next;
3317 for (hacd::HaI32 i = 0; i < perimeterCount; i ++) {
3318 if (frontFace0 == perimeter[i]) {
3319 frontFace0 = NULL;
3320 }
3321 if (frontFace1 == perimeter[i]) {
3322 frontFace1 = NULL;
3323 }
3324
3325 if (backFace0 == perimeter[i]) {
3326 backFace0 = NULL;
3327 }
3328 if (backFace1 == perimeter[i]) {
3329 backFace1 = NULL;
3330 }
3331 }
3332
3333 if (backFace0 && (backFace0->m_incidentFace > 0) && (backFace0->m_twin->m_incidentFace > 0)) {
3334 dgDiagonalEdge key (backFace0);
3335 dignonals.Append(key);
3336 }
3337 if (backFace1 && (backFace1->m_incidentFace > 0) && (backFace1->m_twin->m_incidentFace > 0)) {
3338 dgDiagonalEdge key (backFace1);
3339 dignonals.Append(key);
3340 }
3341
3342 if (frontFace0 && (frontFace0->m_incidentFace > 0) && (frontFace0->m_twin->m_incidentFace > 0)) {
3343 dgDiagonalEdge key (frontFace0);
3344 dignonals.Append(key);
3345 }
3346
3347 if (frontFace1 && (frontFace1->m_incidentFace > 0) && (frontFace1->m_twin->m_incidentFace > 0)) {
3348 dgDiagonalEdge key (frontFace1);
3349 dignonals.Append(key);
3350 }
3351 }
3352 }
3353 }
3354 }
3355 }
3356
3357
RefineTriangulation(const hacd::HaF64 * const vertex,hacd::HaI32 stride)3358 void dgPolyhedra::RefineTriangulation (const hacd::HaF64* const vertex, hacd::HaI32 stride)
3359 {
3360 dgEdge* edgePerimeters[1024 * 16];
3361 hacd::HaI32 perimeterCount = 0;
3362
3363 dgPolyhedra::Iterator iter (*this);
3364 for (iter.Begin(); iter; iter ++) {
3365 dgEdge* const edge = &(*iter);
3366 if (edge->m_incidentFace < 0) {
3367 dgEdge* ptr = edge;
3368 do {
3369 edgePerimeters[perimeterCount] = ptr->m_twin;
3370 perimeterCount ++;
3371 HACD_ASSERT (perimeterCount < hacd::HaI32 (sizeof (edgePerimeters) / sizeof (edgePerimeters[0])));
3372 ptr = ptr->m_prev;
3373 } while (ptr != edge);
3374 break;
3375 }
3376 }
3377 HACD_ASSERT (perimeterCount);
3378 HACD_ASSERT (perimeterCount < hacd::HaI32 (sizeof (edgePerimeters) / sizeof (edgePerimeters[0])));
3379 edgePerimeters[perimeterCount] = edgePerimeters[0];
3380
3381 dgBigVector normal (FaceNormal(edgePerimeters[0], vertex, hacd::HaI32 (stride * sizeof (hacd::HaF64))));
3382 if ((normal % normal) > hacd::HaF32 (1.0e-12f)) {
3383 RefineTriangulation (vertex, stride, &normal, perimeterCount, edgePerimeters);
3384 }
3385 }
3386
3387
OptimizeTriangulation(const hacd::HaF64 * const vertex,hacd::HaI32 strideInBytes)3388 void dgPolyhedra::OptimizeTriangulation (const hacd::HaF64* const vertex, hacd::HaI32 strideInBytes)
3389 {
3390 hacd::HaI32 polygon[1024 * 8];
3391 hacd::HaI32 stride = hacd::HaI32 (strideInBytes / sizeof (hacd::HaF64));
3392
3393 dgPolyhedra leftOver;
3394 dgPolyhedra buildConvex;
3395
3396 buildConvex.BeginFace();
3397 dgPolyhedra::Iterator iter (*this);
3398
3399 for (iter.Begin(); iter; ) {
3400 dgEdge* const edge = &(*iter);
3401 iter++;
3402
3403 if (edge->m_incidentFace > 0) {
3404 dgPolyhedra flatFace;
3405 MarkAdjacentCoplanarFaces (flatFace, edge, vertex, strideInBytes);
3406 //HACD_ASSERT (flatFace.GetCount());
3407
3408 if (flatFace.GetCount()) {
3409 //flatFace.Triangulate (vertex, strideInBytes, &leftOver);
3410 //HACD_ASSERT (!leftOver.GetCount());
3411 flatFace.RefineTriangulation (vertex, stride);
3412
3413 hacd::HaI32 mark = flatFace.IncLRU();
3414 dgPolyhedra::Iterator iter (flatFace);
3415 for (iter.Begin(); iter; iter ++) {
3416 dgEdge* const edge = &(*iter);
3417 if (edge->m_mark != mark) {
3418 if (edge->m_incidentFace > 0) {
3419 dgEdge* ptr = edge;
3420 hacd::HaI32 vertexCount = 0;
3421 do {
3422 polygon[vertexCount] = ptr->m_incidentVertex;
3423 vertexCount ++;
3424 HACD_ASSERT (vertexCount < hacd::HaI32 (sizeof (polygon) / sizeof (polygon[0])));
3425 ptr->m_mark = mark;
3426 ptr = ptr->m_next;
3427 } while (ptr != edge);
3428 if (vertexCount >= 3) {
3429 buildConvex.AddFace (vertexCount, polygon);
3430 }
3431 }
3432 }
3433 }
3434 }
3435 iter.Begin();
3436 }
3437 }
3438 buildConvex.EndFace();
3439 HACD_ASSERT (GetCount() == 0);
3440 SwapInfo(buildConvex);
3441 }
3442
3443
Triangulate(const hacd::HaF64 * const vertex,hacd::HaI32 strideInBytes,dgPolyhedra * const leftOver)3444 void dgPolyhedra::Triangulate (const hacd::HaF64* const vertex, hacd::HaI32 strideInBytes, dgPolyhedra* const leftOver)
3445 {
3446 hacd::HaI32 stride = hacd::HaI32 (strideInBytes / sizeof (hacd::HaF64));
3447
3448 hacd::HaI32 count = GetCount() / 2;
3449 dgStack<char> memPool (hacd::HaI32 ((count + 512) * (2 * sizeof (hacd::HaF64))));
3450 dgDownHeap<dgEdge*, hacd::HaF64> heap(&memPool[0], memPool.GetSizeInBytes());
3451
3452 hacd::HaI32 mark = IncLRU();
3453 Iterator iter (*this);
3454 for (iter.Begin(); iter; ) {
3455 dgEdge* const thisEdge = &(*iter);
3456 iter ++;
3457
3458 if (thisEdge->m_mark == mark) {
3459 continue;
3460 }
3461 if (thisEdge->m_incidentFace < 0) {
3462 continue;
3463 }
3464
3465 count = 0;
3466 dgEdge* ptr = thisEdge;
3467 do {
3468 count ++;
3469 ptr->m_mark = mark;
3470 ptr = ptr->m_next;
3471 } while (ptr != thisEdge);
3472
3473 if (count > 3) {
3474 dgEdge* const edge = TriangulateFace (thisEdge, vertex, stride, heap, NULL);
3475 heap.Flush ();
3476
3477 if (edge) {
3478 HACD_ASSERT (edge->m_incidentFace > 0);
3479
3480 if (leftOver) {
3481 hacd::HaI32* const index = (hacd::HaI32 *) &heap[0];
3482 hacd::HaI64* const data = (hacd::HaI64 *)&index[count];
3483 hacd::HaI32 i = 0;
3484 dgEdge* ptr = edge;
3485 do {
3486 index[i] = ptr->m_incidentVertex;
3487 data[i] = hacd::HaI64 (ptr->m_userData);
3488 i ++;
3489 ptr = ptr->m_next;
3490 } while (ptr != edge);
3491 leftOver->AddFace(i, index, data);
3492
3493 }
3494 else
3495 {
3496 // dgTrace (("Deleting face:"));
3497 // ptr = edge;
3498 // do {
3499 // dgTrace (("%d ", ptr->m_incidentVertex));
3500 // } while (ptr != edge);
3501 //// dgTrace (("\n"));
3502 }
3503
3504 DeleteFace (edge);
3505 iter.Begin();
3506 }
3507 }
3508 }
3509
3510 OptimizeTriangulation (vertex, strideInBytes);
3511
3512 mark = IncLRU();
3513 m_faceSecuence = 1;
3514 for (iter.Begin(); iter; iter ++) {
3515 dgEdge* edge = &(*iter);
3516 if (edge->m_mark == mark) {
3517 continue;
3518 }
3519 if (edge->m_incidentFace < 0) {
3520 continue;
3521 }
3522 HACD_ASSERT (edge == edge->m_next->m_next->m_next);
3523
3524 for (hacd::HaI32 i = 0; i < 3; i ++) {
3525 edge->m_incidentFace = m_faceSecuence;
3526 edge->m_mark = mark;
3527 edge = edge->m_next;
3528 }
3529 m_faceSecuence ++;
3530 }
3531 }
3532
3533
RemoveColinearVertices(dgPolyhedra & flatFace,const hacd::HaF64 * const vertex,hacd::HaI32 stride)3534 static void RemoveColinearVertices (dgPolyhedra& flatFace, const hacd::HaF64* const vertex, hacd::HaI32 stride)
3535 {
3536 dgEdge* edgePerimeters[1024];
3537
3538 hacd::HaI32 perimeterCount = 0;
3539 hacd::HaI32 mark = flatFace.IncLRU();
3540 dgPolyhedra::Iterator iter (flatFace);
3541 for (iter.Begin(); iter; iter ++) {
3542 dgEdge* const edge = &(*iter);
3543 if ((edge->m_incidentFace < 0) && (edge->m_mark != mark)) {
3544 dgEdge* ptr = edge;
3545 do {
3546 ptr->m_mark = mark;
3547 ptr = ptr->m_next;
3548 } while (ptr != edge);
3549 edgePerimeters[perimeterCount] = edge;
3550 perimeterCount ++;
3551 HACD_ASSERT (perimeterCount < hacd::HaI32 (sizeof (edgePerimeters) / sizeof (edgePerimeters[0])));
3552 }
3553 }
3554
3555 for (hacd::HaI32 i = 0; i < perimeterCount; i ++) {
3556 dgEdge* edge = edgePerimeters[i];
3557 dgEdge* ptr = edge;
3558 dgVector p0 (&vertex[ptr->m_incidentVertex * stride]);
3559 dgVector p1 (&vertex[ptr->m_next->m_incidentVertex * stride]);
3560 dgVector e0 (p1 - p0) ;
3561 e0 = e0.Scale (hacd::HaF32 (1.0f) / (dgSqrt (e0 % e0) + hacd::HaF32 (1.0e-12f)));
3562 hacd::HaI32 ignoreTest = 1;
3563 do {
3564 ignoreTest = 0;
3565 dgVector p2 (&vertex[ptr->m_next->m_next->m_incidentVertex * stride]);
3566 dgVector e1 (p2 - p1);
3567 e1 = e1.Scale (hacd::HaF32 (1.0f) / (dgSqrt (e1 % e1) + hacd::HaF32 (1.0e-12f)));
3568 hacd::HaF32 dot = e1 % e0;
3569 if (dot > hacd::HaF32 (hacd::HaF32 (0.9999f))) {
3570
3571 for (dgEdge* interiorEdge = ptr->m_next->m_twin->m_next; interiorEdge != ptr->m_twin; interiorEdge = ptr->m_next->m_twin->m_next) {
3572 flatFace.DeleteEdge (interiorEdge);
3573 }
3574
3575 if (ptr->m_twin->m_next->m_next->m_next == ptr->m_twin) {
3576 HACD_ASSERT (ptr->m_twin->m_next->m_incidentFace > 0);
3577 flatFace.DeleteEdge (ptr->m_twin->m_next);
3578 }
3579
3580 HACD_ASSERT (ptr->m_next->m_twin->m_next->m_twin == ptr);
3581 edge = ptr->m_next;
3582
3583 if (!flatFace.FindEdge (ptr->m_incidentVertex, edge->m_twin->m_incidentVertex) &&
3584 !flatFace.FindEdge (edge->m_twin->m_incidentVertex, ptr->m_incidentVertex)) {
3585 ptr->m_twin->m_prev = edge->m_twin->m_prev;
3586 edge->m_twin->m_prev->m_next = ptr->m_twin;
3587
3588 edge->m_next->m_prev = ptr;
3589 ptr->m_next = edge->m_next;
3590
3591 edge->m_next = edge->m_twin;
3592 edge->m_prev = edge->m_twin;
3593 edge->m_twin->m_next = edge;
3594 edge->m_twin->m_prev = edge;
3595 flatFace.DeleteEdge (edge);
3596 flatFace.ChangeEdgeIncidentVertex (ptr->m_twin, ptr->m_next->m_incidentVertex);
3597
3598 e1 = e0;
3599 p1 = p2;
3600 edge = ptr;
3601 ignoreTest = 1;
3602 continue;
3603 }
3604 }
3605
3606 e0 = e1;
3607 p1 = p2;
3608 ptr = ptr->m_next;
3609 } while ((ptr != edge) || ignoreTest);
3610 }
3611 }
3612
3613
GetInteriorDiagonals(dgPolyhedra & polyhedra,dgEdge ** const diagonals,hacd::HaI32 maxCount)3614 static hacd::HaI32 GetInteriorDiagonals (dgPolyhedra& polyhedra, dgEdge** const diagonals, hacd::HaI32 maxCount)
3615 {
3616 hacd::HaI32 count = 0;
3617 hacd::HaI32 mark = polyhedra.IncLRU();
3618 dgPolyhedra::Iterator iter (polyhedra);
3619 for (iter.Begin(); iter; iter++) {
3620 dgEdge* const edge = &(*iter);
3621 if (edge->m_mark != mark) {
3622 if (edge->m_incidentFace > 0) {
3623 if (edge->m_twin->m_incidentFace > 0) {
3624 edge->m_twin->m_mark = mark;
3625 if (count < maxCount){
3626 diagonals[count] = edge;
3627 count ++;
3628 }
3629 HACD_ASSERT (count <= maxCount);
3630 }
3631 }
3632 }
3633 edge->m_mark = mark;
3634 }
3635
3636 return count;
3637 }
3638
IsEssensialPointDiagonal(dgEdge * const diagonal,const dgBigVector & normal,const hacd::HaF64 * const pool,hacd::HaI32 stride)3639 static bool IsEssensialPointDiagonal (dgEdge* const diagonal, const dgBigVector& normal, const hacd::HaF64* const pool, hacd::HaI32 stride)
3640 {
3641 hacd::HaF64 dot;
3642 dgBigVector p0 (&pool[diagonal->m_incidentVertex * stride]);
3643 dgBigVector p1 (&pool[diagonal->m_twin->m_next->m_twin->m_incidentVertex * stride]);
3644 dgBigVector p2 (&pool[diagonal->m_prev->m_incidentVertex * stride]);
3645
3646 dgBigVector e1 (p1 - p0);
3647 dot = e1 % e1;
3648 if (dot < hacd::HaF64 (1.0e-12f)) {
3649 return false;
3650 }
3651 e1 = e1.Scale (hacd::HaF64 (1.0f) / sqrt(dot));
3652
3653 dgBigVector e2 (p2 - p0);
3654 dot = e2 % e2;
3655 if (dot < hacd::HaF64 (1.0e-12f)) {
3656 return false;
3657 }
3658 e2 = e2.Scale (hacd::HaF64 (1.0f) / sqrt(dot));
3659
3660 dgBigVector n1 (e1 * e2);
3661
3662 dot = normal % n1;
3663 //if (dot > hacd::HaF64 (hacd::HaF32 (0.1f)f)) {
3664 //if (dot >= hacd::HaF64 (-1.0e-6f)) {
3665 if (dot >= hacd::HaF64 (0.0f)) {
3666 return false;
3667 }
3668 return true;
3669 }
3670
3671
IsEssensialDiagonal(dgEdge * const diagonal,const dgBigVector & normal,const hacd::HaF64 * const pool,hacd::HaI32 stride)3672 static bool IsEssensialDiagonal (dgEdge* const diagonal, const dgBigVector& normal, const hacd::HaF64* const pool, hacd::HaI32 stride)
3673 {
3674 return IsEssensialPointDiagonal (diagonal, normal, pool, stride) || IsEssensialPointDiagonal (diagonal->m_twin, normal, pool, stride);
3675 }
3676
3677
ConvexPartition(const hacd::HaF64 * const vertex,hacd::HaI32 strideInBytes,dgPolyhedra * const leftOversOut)3678 void dgPolyhedra::ConvexPartition (const hacd::HaF64* const vertex, hacd::HaI32 strideInBytes, dgPolyhedra* const leftOversOut)
3679 {
3680 if (GetCount()) {
3681 Triangulate (vertex, strideInBytes, leftOversOut);
3682 DeleteDegenerateFaces (vertex, strideInBytes, hacd::HaF32 (1.0e-5f));
3683 Optimize (vertex, strideInBytes, hacd::HaF32 (1.0e-4f));
3684 DeleteDegenerateFaces (vertex, strideInBytes, hacd::HaF32 (1.0e-5f));
3685
3686 if (GetCount()) {
3687 hacd::HaI32 removeCount = 0;
3688 hacd::HaI32 stride = hacd::HaI32 (strideInBytes / sizeof (hacd::HaF64));
3689
3690 hacd::HaI32 polygon[1024 * 8];
3691 dgEdge* diagonalsPool[1024 * 8];
3692 dgPolyhedra buildConvex;
3693
3694 buildConvex.BeginFace();
3695 dgPolyhedra::Iterator iter (*this);
3696 for (iter.Begin(); iter; ) {
3697 dgEdge* edge = &(*iter);
3698 iter++;
3699 if (edge->m_incidentFace > 0) {
3700
3701 dgPolyhedra flatFace;
3702 MarkAdjacentCoplanarFaces (flatFace, edge, vertex, strideInBytes);
3703
3704 if (flatFace.GetCount()) {
3705 flatFace.RefineTriangulation (vertex, stride);
3706 RemoveColinearVertices (flatFace, vertex, stride);
3707
3708 hacd::HaI32 diagonalCount = GetInteriorDiagonals (flatFace, diagonalsPool, sizeof (diagonalsPool) / sizeof (diagonalsPool[0]));
3709 if (diagonalCount) {
3710 edge = &flatFace.GetRoot()->GetInfo();
3711 if (edge->m_incidentFace < 0) {
3712 edge = edge->m_twin;
3713 }
3714 HACD_ASSERT (edge->m_incidentFace > 0);
3715
3716 dgBigVector normal (FaceNormal (edge, vertex, strideInBytes));
3717 normal = normal.Scale (hacd::HaF64 (1.0f) / sqrt (normal % normal));
3718
3719 edge = NULL;
3720 dgPolyhedra::Iterator iter (flatFace);
3721 for (iter.Begin(); iter; iter ++) {
3722 edge = &(*iter);
3723 if (edge->m_incidentFace < 0) {
3724 break;
3725 }
3726 }
3727 HACD_ASSERT (edge);
3728
3729 hacd::HaI32 isConvex = 1;
3730 dgEdge* ptr = edge;
3731 hacd::HaI32 mark = flatFace.IncLRU();
3732
3733 dgBigVector normal2 (normal);
3734 dgBigVector p0 (&vertex[ptr->m_prev->m_incidentVertex * stride]);
3735 dgBigVector p1 (&vertex[ptr->m_incidentVertex * stride]);
3736 dgBigVector e0 (p1 - p0);
3737 e0 = e0.Scale (hacd::HaF32 (1.0f) / (dgSqrt (e0 % e0) + hacd::HaF32 (1.0e-14f)));
3738 do {
3739 dgBigVector p2 (&vertex[ptr->m_next->m_incidentVertex * stride]);
3740 dgBigVector e1 (p2 - p1);
3741 e1 = e1.Scale (hacd::HaF32 (1.0f) / (sqrt (e1 % e1) + hacd::HaF32 (1.0e-14f)));
3742 hacd::HaF64 dot = (e0 * e1) % normal2;
3743 //if (dot > hacd::HaF32 (0.0f)) {
3744 if (dot > hacd::HaF32 (5.0e-3f)) {
3745 isConvex = 0;
3746 break;
3747 }
3748 ptr->m_mark = mark;
3749 e0 = e1;
3750 p1 = p2;
3751 ptr = ptr->m_next;
3752 } while (ptr != edge);
3753
3754 if (isConvex) {
3755 dgPolyhedra::Iterator iter (flatFace);
3756 for (iter.Begin(); iter; iter ++) {
3757 ptr = &(*iter);
3758 if (ptr->m_incidentFace < 0) {
3759 if (ptr->m_mark < mark) {
3760 isConvex = 0;
3761 break;
3762 }
3763 }
3764 }
3765 }
3766
3767 if (isConvex) {
3768 if (diagonalCount > 2) {
3769 hacd::HaI32 count = 0;
3770 ptr = edge;
3771 do {
3772 polygon[count] = ptr->m_incidentVertex;
3773 count ++;
3774 HACD_ASSERT (count < hacd::HaI32 (sizeof (polygon) / sizeof (polygon[0])));
3775 ptr = ptr->m_next;
3776 } while (ptr != edge);
3777
3778 for (hacd::HaI32 i = 0; i < count - 1; i ++) {
3779 for (hacd::HaI32 j = i + 1; j < count; j ++) {
3780 if (polygon[i] == polygon[j]) {
3781 i = count;
3782 isConvex = 0;
3783 break ;
3784 }
3785 }
3786 }
3787 }
3788 }
3789
3790 if (isConvex) {
3791 for (hacd::HaI32 j = 0; j < diagonalCount; j ++) {
3792 dgEdge* const diagonal = diagonalsPool[j];
3793 removeCount ++;
3794 flatFace.DeleteEdge (diagonal);
3795 }
3796 } else {
3797 for (hacd::HaI32 j = 0; j < diagonalCount; j ++) {
3798 dgEdge* const diagonal = diagonalsPool[j];
3799 if (!IsEssensialDiagonal(diagonal, normal, vertex, stride)) {
3800 removeCount ++;
3801 flatFace.DeleteEdge (diagonal);
3802 }
3803 }
3804 }
3805 }
3806
3807 hacd::HaI32 mark = flatFace.IncLRU();
3808 dgPolyhedra::Iterator iter (flatFace);
3809 for (iter.Begin(); iter; iter ++) {
3810 dgEdge* const edge = &(*iter);
3811 if (edge->m_mark != mark) {
3812 if (edge->m_incidentFace > 0) {
3813 dgEdge* ptr = edge;
3814 hacd::HaI32 diagonalCount = 0;
3815 do {
3816 polygon[diagonalCount] = ptr->m_incidentVertex;
3817 diagonalCount ++;
3818 HACD_ASSERT (diagonalCount < hacd::HaI32 (sizeof (polygon) / sizeof (polygon[0])));
3819 ptr->m_mark = mark;
3820 ptr = ptr->m_next;
3821 } while (ptr != edge);
3822 if (diagonalCount >= 3) {
3823 buildConvex.AddFace (diagonalCount, polygon);
3824 }
3825 }
3826 }
3827 }
3828 }
3829 iter.Begin();
3830 }
3831 }
3832
3833 buildConvex.EndFace();
3834 HACD_ASSERT (GetCount() == 0);
3835 SwapInfo(buildConvex);
3836 }
3837 }
3838 }
3839
3840
CalculateSphere(const hacd::HaF64 * const vertex,hacd::HaI32 strideInBytes,const dgMatrix * const basis) const3841 dgSphere dgPolyhedra::CalculateSphere (const hacd::HaF64* const vertex, hacd::HaI32 strideInBytes, const dgMatrix* const basis) const
3842 {
3843 /*
3844 // this si a degenerate mesh of a flat plane calculate OOBB by discrete rotations
3845 dgStack<hacd::HaI32> pool (GetCount() * 3 + 6);
3846 hacd::HaI32* const indexList = &pool[0];
3847
3848 dgMatrix axis (dgGetIdentityMatrix());
3849 dgBigVector p0 (hacd::HaF32 ( 1.0e10f), hacd::HaF32 ( 1.0e10f), hacd::HaF32 ( 1.0e10f), hacd::HaF32 (0.0f));
3850 dgBigVector p1 (hacd::HaF32 (-1.0e10f), hacd::HaF32 (-1.0e10f), hacd::HaF32 (-1.0e10f), hacd::HaF32 (0.0f));
3851
3852 hacd::HaI32 stride = hacd::HaI32 (strideInBytes / sizeof (hacd::HaF64));
3853 hacd::HaI32 indexCount = 0;
3854 hacd::HaI32 mark = IncLRU();
3855 dgPolyhedra::Iterator iter(*this);
3856 for (iter.Begin(); iter; iter ++) {
3857 dgEdge* const edge = &(*iter);
3858 if (edge->m_mark != mark) {
3859 dgEdge *ptr = edge;
3860 do {
3861 ptr->m_mark = mark;
3862 ptr = ptr->m_twin->m_next;
3863 } while (ptr != edge);
3864 hacd::HaI32 index = edge->m_incidentVertex;
3865 indexList[indexCount + 6] = edge->m_incidentVertex;
3866 dgBigVector point (vertex[index * stride + 0], vertex[index * stride + 1], vertex[index * stride + 2], hacd::HaF32 (0.0f));
3867 for (hacd::HaI32 i = 0; i < 3; i ++) {
3868 if (point[i] < p0[i]) {
3869 p0[i] = point[i];
3870 indexList[i * 2 + 0] = index;
3871 }
3872 if (point[i] > p1[i]) {
3873 p1[i] = point[i];
3874 indexList[i * 2 + 1] = index;
3875 }
3876 }
3877 indexCount ++;
3878 }
3879 }
3880 indexCount += 6;
3881
3882
3883 dgBigVector size (p1 - p0);
3884 hacd::HaF64 volume = size.m_x * size.m_y * size.m_z;
3885
3886
3887 for (hacd::HaF32 pitch = hacd::HaF32 (0.0f); pitch < hacd::HaF32 (90.0f); pitch += hacd::HaF32 (10.0f)) {
3888 dgMatrix pitchMatrix (dgPitchMatrix(pitch * hacd::HaF32 (3.1416f) / hacd::HaF32 (180.0f)));
3889 for (hacd::HaF32 yaw = hacd::HaF32 (0.0f); yaw < hacd::HaF32 (90.0f); yaw += hacd::HaF32 (10.0f)) {
3890 dgMatrix yawMatrix (dgYawMatrix(yaw * hacd::HaF32 (3.1416f) / hacd::HaF32 (180.0f)));
3891 for (hacd::HaF32 roll = hacd::HaF32 (0.0f); roll < hacd::HaF32 (90.0f); roll += hacd::HaF32 (10.0f)) {
3892 hacd::HaI32 tmpIndex[6];
3893 dgMatrix rollMatrix (dgRollMatrix(roll * hacd::HaF32 (3.1416f) / hacd::HaF32 (180.0f)));
3894 dgMatrix tmp (pitchMatrix * yawMatrix * rollMatrix);
3895 dgBigVector q0 (hacd::HaF32 ( 1.0e10f), hacd::HaF32 ( 1.0e10f), hacd::HaF32 ( 1.0e10f), hacd::HaF32 (0.0f));
3896 dgBigVector q1 (hacd::HaF32 (-1.0e10f), hacd::HaF32 (-1.0e10f), hacd::HaF32 (-1.0e10f), hacd::HaF32 (0.0f));
3897
3898 hacd::HaF32 volume1 = hacd::HaF32 (1.0e10f);
3899 for (hacd::HaI32 i = 0; i < indexCount; i ++) {
3900 hacd::HaI32 index = indexList[i];
3901 dgBigVector point (vertex[index * stride + 0], vertex[index * stride + 1], vertex[index * stride + 2], hacd::HaF32 (0.0f));
3902 point = tmp.UnrotateVector(point);
3903
3904 for (hacd::HaI32 j = 0; j < 3; j ++) {
3905 if (point[j] < q0[j]) {
3906 q0[j] = point[j];
3907 tmpIndex[j * 2 + 0] = index;
3908 }
3909 if (point[j] > q1[j]) {
3910 q1[j] = point[j];
3911 tmpIndex[j * 2 + 1] = index;
3912 }
3913 }
3914
3915
3916 dgVector size1 (q1 - q0);
3917 volume1 = size1.m_x * size1.m_y * size1.m_z;
3918 if (volume1 >= volume) {
3919 break;
3920 }
3921 }
3922
3923 if (volume1 < volume) {
3924 p0 = q0;
3925 p1 = q1;
3926 axis = tmp;
3927 volume = volume1;
3928 memcpy (indexList, tmpIndex, sizeof (tmpIndex));
3929 }
3930 }
3931 }
3932 }
3933
3934 HACD_ASSERT (0);
3935 dgSphere sphere (axis);
3936 dgVector q0 (p0);
3937 dgVector q1 (p1);
3938 sphere.m_posit = axis.RotateVector((q1 + q0).Scale (hacd::HaF32 (0.5f)));
3939 sphere.m_size = (q1 - q0).Scale (hacd::HaF32 (0.5f));
3940 return sphere;
3941 */
3942
3943 hacd::HaI32 stride = hacd::HaI32 (strideInBytes / sizeof (hacd::HaF64));
3944
3945 hacd::HaI32 vertexCount = 0;
3946 hacd::HaI32 mark = IncLRU();
3947 dgPolyhedra::Iterator iter(*this);
3948 for (iter.Begin(); iter; iter ++) {
3949 dgEdge* const edge = &(*iter);
3950 if (edge->m_mark != mark) {
3951 dgEdge* ptr = edge;
3952 do {
3953 ptr->m_mark = mark;
3954 ptr = ptr->m_twin->m_next;
3955 } while (ptr != edge);
3956 vertexCount ++;
3957 }
3958 }
3959 HACD_ASSERT (vertexCount);
3960
3961 mark = IncLRU();
3962 hacd::HaI32 vertexCountIndex = 0;
3963 dgStack<dgBigVector> pool (vertexCount);
3964 for (iter.Begin(); iter; iter ++) {
3965 dgEdge* const edge = &(*iter);
3966 if (edge->m_mark != mark) {
3967 dgEdge* ptr = edge;
3968 do {
3969 ptr->m_mark = mark;
3970 ptr = ptr->m_twin->m_next;
3971 } while (ptr != edge);
3972 hacd::HaI32 incidentVertex = edge->m_incidentVertex * stride;
3973 pool[vertexCountIndex] = dgBigVector (vertex[incidentVertex + 0], vertex[incidentVertex + 1], vertex[incidentVertex + 2], hacd::HaF32 (0.0f));
3974 vertexCountIndex ++;
3975 }
3976 }
3977 HACD_ASSERT (vertexCountIndex <= vertexCount);
3978
3979 dgMatrix axis (dgGetIdentityMatrix());
3980 dgSphere sphere (axis);
3981 dgConvexHull3d convexHull (&pool[0].m_x, sizeof (dgBigVector), vertexCountIndex, 0.0f);
3982 if (convexHull.GetCount()) {
3983 dgStack<hacd::HaI32> triangleList (convexHull.GetCount() * 3);
3984 hacd::HaI32 trianglesCount = 0;
3985 for (dgConvexHull3d::dgListNode* node = convexHull.GetFirst(); node; node = node->GetNext()) {
3986 dgConvexHull3DFace* const face = &node->GetInfo();
3987 triangleList[trianglesCount * 3 + 0] = face->m_index[0];
3988 triangleList[trianglesCount * 3 + 1] = face->m_index[1];
3989 triangleList[trianglesCount * 3 + 2] = face->m_index[2];
3990 trianglesCount ++;
3991 HACD_ASSERT ((trianglesCount * 3) <= triangleList.GetElementsCount());
3992 }
3993
3994 dgVector* const dst = (dgVector*) &pool[0].m_x;
3995 for (hacd::HaI32 i = 0; i < convexHull.GetVertexCount(); i ++) {
3996 dst[i] = convexHull.GetVertex(i);
3997 }
3998 sphere.SetDimensions (&dst[0].m_x, sizeof (dgVector), &triangleList[0], trianglesCount * 3, NULL);
3999
4000 } else if (vertexCountIndex >= 3) {
4001 dgStack<hacd::HaI32> triangleList (GetCount() * 3 * 2);
4002 hacd::HaI32 mark = IncLRU();
4003 hacd::HaI32 trianglesCount = 0;
4004 for (iter.Begin(); iter; iter ++) {
4005 dgEdge* const edge = &(*iter);
4006 if (edge->m_mark != mark) {
4007 dgEdge* ptr = edge;
4008 do {
4009 ptr->m_mark = mark;
4010 ptr = ptr->m_twin->m_next;
4011 } while (ptr != edge);
4012
4013 ptr = edge->m_next->m_next;
4014 do {
4015 triangleList[trianglesCount * 3 + 0] = edge->m_incidentVertex;
4016 triangleList[trianglesCount * 3 + 1] = ptr->m_prev->m_incidentVertex;
4017 triangleList[trianglesCount * 3 + 2] = ptr->m_incidentVertex;
4018 trianglesCount ++;
4019 HACD_ASSERT ((trianglesCount * 3) <= triangleList.GetElementsCount());
4020 ptr = ptr->m_twin->m_next;
4021 } while (ptr != edge);
4022
4023 dgVector* const dst = (dgVector*) &pool[0].m_x;
4024 for (hacd::HaI32 i = 0; i < vertexCountIndex; i ++) {
4025 dst[i] = pool[i];
4026 }
4027 sphere.SetDimensions (&dst[0].m_x, sizeof (dgVector), &triangleList[0], trianglesCount * 3, NULL);
4028 }
4029 }
4030 }
4031 return sphere;
4032
4033 }
4034