1 /*************************************************************************
2 * *
3 * Tokamak Physics Engine, Copyright (C) 2002-2007 David Lam. *
4 * All rights reserved. Email: david@tokamakphysics.com *
5 * Web: www.tokamakphysics.com *
6 * *
7 * This library is distributed in the hope that it will be useful, *
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
10 * LICENSE.TXT for more details. *
11 * *
12 *************************************************************************/
13
14 #include "math/ne_type.h"
15 #include "math/ne_debug.h"
16 #include "tokamak.h"
17 #include "containers.h"
18 #include "scenery.h"
19 #include "collision.h"
20 #include "constraint.h"
21 #include "rigidbody.h"
22 #include "scenery.h"
23 /*
24 #ifdef _WIN32
25 #include <windows.h>
26 #endif
27 */
28 #include "stack.h"
29 #include "simulator.h"
30
31 #include <stdio.h>
32 #include <assert.h>
33
34 /****************************************************************************
35 *
36 * neRegion::~neRegion
37 *
38 ****************************************************************************/
39
~neRegion()40 neRegion::~neRegion()
41 {
42 }
43
44 /****************************************************************************
45 *
46 * neRegion::Initialise
47 *
48 ****************************************************************************/
49
Initialise(neFixedTimeStepSimulator * s,neByte sortD)50 void neRegion::Initialise(neFixedTimeStepSimulator * s, neByte sortD)
51 {
52 sim = s;
53
54 maxRigidBodies = s->maxRigidBodies;
55
56 maxAnimBodies = s->maxAnimBodies;
57
58 totalBodies = maxRigidBodies + maxAnimBodies;
59
60 maxParticle = s->maxParticles;
61
62 b2b.Reserve(totalBodies * (totalBodies - 1) / 2, sim->allocator);
63
64 b2p.Reserve(totalBodies * maxParticle, sim->allocator);
65
66 newBodies.Reserve(totalBodies + maxParticle, sim->allocator);
67
68 bodies.Reserve(totalBodies + maxParticle, sim->allocator);
69
70 overlappedPairs.Reserve(sim->sizeInfo.overlappedPairsCount, sim->allocator);
71
72 sortDimension = sortD;
73
74 for (s32 i = 0; i < 3; i++)
75 {
76 if (sortD & (1 << i ) )
77 {
78 bool b = coordLists[i].Reserve((totalBodies + maxParticle) * 2, sim->allocator);
79
80 coordLists[i].dim = i;
81
82 coordLists[i].dimPower2 = 1 << i;
83
84 coordLists[i].region = this;
85
86 ASSERT(b);
87 }
88 }
89
90 // needRebuild = true;
91
92 terrainTree.sim = sim;
93
94 #ifdef _DEBUG_REGION
95 debugOn = false;
96 #endif
97 }
98
99 /****************************************************************************
100 *
101 * neRegion::AddBody
102 *
103 ****************************************************************************/
104
AddBody(neRigidBodyBase * bb,neRigidBodyBase * hint)105 bool neRegion::AddBody(neRigidBodyBase * bb, neRigidBodyBase * hint)
106 {
107 if (bb->IsInRegion())
108 return true;
109
110 neAddBodyInfo * bi = newBodies.Alloc();
111
112 bi->body = bb;
113
114 bi->hint = hint;
115
116 neRigidBodyBase ** bp = bodies.Alloc();
117
118 *bp = bi->body;
119
120 bi->body->regionHandle = (neFreeListItem<neRigidBodyBase *>*)bp;
121
122 bb->pendingAddToRegion = 1;
123
124 return true;
125 }
126
127 /****************************************************************************
128 *
129 * neRegion::RemoveBody
130 *
131 ****************************************************************************/
132
RemoveBody(neRigidBodyBase * bb)133 void neRegion::RemoveBody(neRigidBodyBase * bb)
134 {
135 if (!bb->IsInRegion())
136 return;
137
138 if (bb->pendingAddToRegion == 1)
139 {
140 bb->pendingAddToRegion = 2; // to signify adding to region is aborted
141 }
142
143 for (s32 i = 0; i < 3; i++)
144 {
145 if (sortDimension & (1 << i) )
146 {
147 if (bb->maxCoord[i])
148 {
149 coordLists[i].coordList.Dealloc(bb->maxCoord[i]);
150
151 bb->maxCoord[i] = NULL;
152 }
153 if (bb->minCoord[i])
154 {
155 coordLists[i].coordList.Dealloc(bb->minCoord[i]);
156
157 bb->minCoord[i] = NULL;
158 }
159 }
160 }
161 if (bb->regionHandle)
162 bodies.Dealloc((neRigidBodyBase**)bb->regionHandle);
163
164 bb->regionHandle = NULL;
165
166 neFreeListItem<neOverlappedPair> * oitem = (neFreeListItem<neOverlappedPair> *)(*overlappedPairs.BeginUsed());
167
168 while (oitem)
169 {
170 neOverlappedPair * op = (neOverlappedPair *) oitem;
171
172 oitem = oitem->next;
173
174 if (op->bodyA == bb || op->bodyB == bb)
175 {
176 overlappedPairs.Dealloc(op);
177 }
178 }
179 }
180
181 /****************************************************************************
182 *
183 * neRegion::UpdateOverlapPairs
184 *
185 ****************************************************************************/
186
Rebuild()187 void neRegion::Rebuild()
188 {
189 // sort coordinate list
190 for (s32 i = 0; i < 3; i++)
191 {
192 if (sortDimension & (1 << i) )
193 {
194 coordLists[i].Sort(true);
195 }
196 }
197
198 overlappedPairs.Clear();
199
200 neRigidBody_ * rb = sim->activeRB.GetHead();
201
202 while (rb)
203 {
204 neRigidBody_ * rbNext = sim->activeRB.GetNext(rb);
205
206 neRigidBody_ * rbNext_ = rbNext;
207
208 while (rbNext)
209 {
210 ResetOverlapStatus(rb, rbNext);
211
212 rbNext = sim->activeRB.GetNext(rbNext);
213 }
214 rb = rbNext_;
215 }
216
217 rb = sim->activeRB.GetHead();
218
219 while (rb)
220 {
221 neCollisionBody_ * cb = sim->activeCB.GetHead();
222
223 while (cb)
224 {
225 ResetOverlapStatus(rb, cb);
226
227 cb = sim->activeCB.GetNext(cb);
228 }
229 rb = sim->activeRB.GetNext(rb);
230 }
231 }
232
233 /****************************************************************************
234 *
235 * neRegion::Update
236 *
237 ****************************************************************************/
238
Update()239 void neRegion::Update()
240 {
241 for (s32 i = 0; i < 3; i++)
242 {
243 if (sortDimension & (1 << i) )
244 {
245 coordLists[i].Sort(false);
246 }
247 }
248
249 for (s32 k = 0; k < newBodies.GetUsedCount(); k++)
250 {
251 neAddBodyInfo * bi = &newBodies[k];
252
253 if (bi->body->pendingAddToRegion == 2 || !bi->body->IsValid())
254 {
255 continue;
256 }
257
258 ASSERT(bi->body->pendingAddToRegion == 1);
259
260 bi->body->pendingAddToRegion = 0;
261
262 InsertCoordList(bi->body, bi->hint);
263
264 neDLinkList<neRigidBodyBase *>::iterator iter;
265
266 for (iter = bodies.BeginUsed(); iter.Valid(); iter++)
267 {
268 neRigidBodyBase * b = *(*iter);
269
270 if (b->minCoord[0] == NULL && b->minCoord[1] == NULL && b->minCoord[2] == NULL)
271 continue;
272
273 if (b == bi->body)
274 continue;
275
276 neRigidBody_ * b1 = bi->body->AsRigidBody();
277
278 neRigidBody_ * b2 = b->AsRigidBody();
279
280 if ((b1 && b2))
281 {
282 if (! (b1->IsParticle() && b2->IsParticle()) )
283 ResetOverlapStatus(bi->body, b);
284 }
285 else
286 {
287 ResetOverlapStatus(bi->body, b);
288 }
289 }
290 /*
291 neRigidBodyBase ** bp = bodies.Alloc();
292
293 *bp = bi->body;
294
295 bi->body->regionHandle = (neFreeListItem<neRigidBodyBase *>*)bp;
296 */ }
297
298 newBodies.Clear();
299
300 #ifdef _DEBUG_REGION
301
302 if (debugOn)
303 {
304 for (s32 j = 0; j < 3; j++)
305 {
306 if (sortDimension & 1 << j)
307 coordLists[j].OuputDebug();
308 }
309 }
310
311 #endif
312 }
313
314 /****************************************************************************
315 *
316 * neRegion::GetOverlappedPair
317 *
318 ****************************************************************************/
319
GetOverlappedStatus(neRigidBodyBase * a,neRigidBodyBase * b)320 neOverlapped * neRegion::GetOverlappedStatus(neRigidBodyBase * a, neRigidBodyBase * b)
321 {
322 s32 smallIndex, largeIndex;
323
324 if (a->id > b->id)
325 {
326 smallIndex = b->id;
327 largeIndex = a->id;
328 }
329 else
330 {
331 smallIndex = a->id;
332 largeIndex = b->id;
333 }
334
335 neOverlapped * o;
336
337 if (largeIndex >= totalBodies)
338 {
339 //b overlapping p
340 //if (smallIndex >= maxRigidBodies) // ab 2 ab, return
341 // return NULL;
342
343 ASSERT(smallIndex < totalBodies); //ab 2 ab
344
345 ASSERT(((largeIndex - totalBodies) + smallIndex * maxParticle) < b2p.GetTotalSize());
346
347 o = &b2p[(largeIndex - totalBodies) + smallIndex * maxParticle];
348 }
349 else
350 {
351 //b overlapping b
352 ASSERT(((largeIndex * (largeIndex - 1)) / 2) + smallIndex < b2b.GetTotalSize());
353
354 o = &b2b[((largeIndex * (largeIndex - 1)) / 2) + smallIndex];
355 }
356 return o;
357 }
358 /****************************************************************************
359 *
360 * neRegion::ToggleOverlapStatus
361 *
362 ****************************************************************************/
363
ToggleOverlapStatus(neRigidBodyBase * a,neRigidBodyBase * b,neByte dimp2)364 void neRegion::ToggleOverlapStatus(neRigidBodyBase * a, neRigidBodyBase * b, neByte dimp2)
365 {
366 neOverlapped * o = GetOverlappedStatus(a,b);
367
368 ASSERT(o);
369
370 if (o->status == sortDimension)
371 {
372 o->status ^= dimp2;
373
374 //if (o->status != sortDimension && (sim->colTable.Get(a->cid, b->cid) != neCollisionTable::NE_COLLISION_IGNORE))
375 if (o->status != sortDimension && o->pairItem)
376 {
377 //remove
378 overlappedPairs.Dealloc(o->pairItem);
379
380 o->pairItem = NULL;
381 }
382 }
383 else
384 {
385 o->status ^= dimp2;
386
387 if (o->status == sortDimension && (sim->colTable.Get(a->cid, b->cid) != neCollisionTable::RESPONSE_IGNORE))
388 {
389 if (overlappedPairs.usedCount >= sim->sizeInfo.overlappedPairsCount)
390 {
391 sprintf(sim->logBuffer, "Overlap Pair buffer full. Increase buffer size.\n");
392 sim->LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
393 return;
394 }
395 //insert
396 o->pairItem = overlappedPairs.Alloc();
397
398 o->pairItem->bodyA = a;
399
400 o->pairItem->bodyB = b;
401 }
402 }
403 //zDebugMessage("toggle in %d, between objects %d and %d\n", dimp2 >> 1, a->id, b->id);
404 //ASSERT(overlappedPairs.unusedCount <= 1);
405 }
406
407 /****************************************************************************
408 *
409 * neRegion::ResetOverlapStatus
410 *
411 ****************************************************************************/
412
ResetOverlapStatus(neRigidBodyBase * a,neRigidBodyBase * b)413 void neRegion::ResetOverlapStatus(neRigidBodyBase * a, neRigidBodyBase * b)
414 {
415 neOverlapped * o = GetOverlappedStatus(a,b);
416
417 o->status = a->IsAABOverlapped(b);
418
419 if (o->status == sortDimension)
420 {
421 o->pairItem = overlappedPairs.Alloc();
422
423 neRigidBody_ * ra = a->AsRigidBody();
424
425 neRigidBody_ * rb = b->AsRigidBody();
426
427 if (ra)
428 {
429 if (ra->IsParticle())
430 {
431 if (rb)
432 {
433 ASSERT(!rb->IsParticle());
434
435 o->pairItem->bodyA = b;
436
437 o->pairItem->bodyB = a;
438 }
439 else
440 {
441 o->pairItem->bodyA = a;
442
443 o->pairItem->bodyB = b;
444 }
445 }
446 else
447 {
448 o->pairItem->bodyA = a;
449
450 o->pairItem->bodyB = b;
451 }
452 }
453 else
454 {
455 o->pairItem->bodyA = b;
456
457 o->pairItem->bodyB = a;
458 }
459 }
460 else
461 {
462 o->pairItem = NULL;
463 }
464 }
465
MakeTerrain(neTriangleMesh * tris)466 void neRegion::MakeTerrain(neTriangleMesh * tris)
467 {
468 terrainTree.BuildTree(tris->vertices, tris->vertexCount, tris->triangles, tris->triangleCount, sim->allocator);
469 }
470
FreeTerrain()471 void neRegion::FreeTerrain()
472 {
473 terrainTree.FreeTree();
474 }
475
InsertCoordList(neRigidBodyBase * bb,neRigidBodyBase * hint)476 void neRegion::InsertCoordList(neRigidBodyBase * bb, neRigidBodyBase * hint)
477 {
478 for (s32 i = 0; i < 3; i++)
479 {
480 if (sortDimension & (1 << i) )
481 {
482 coordLists[i].Add(bb, hint, i);
483 }
484 else
485 {
486 bb->maxCoord[i] = NULL;
487 bb->minCoord[i] = NULL;
488 }
489 }
490 }
491
492 /****************************************************************************
493 *
494 * neCoordList::Add
495 *
496 ****************************************************************************/
497
Add(neRigidBodyBase * bb,neRigidBodyBase * hint,s32 hintCoord)498 void neCoordList::Add(neRigidBodyBase * bb, neRigidBodyBase * hint, s32 hintCoord)
499 {
500 CCoordListEntryItem * startSearch = coordList.usedTail;
501
502 CCoordListEntry * lentry = coordList.Alloc();
503
504 lentry->bb = bb;
505
506 lentry->flag = CCoordListEntry::LowEnd;
507
508 bb->minCoord[dim] = lentry;
509
510 CCoordListEntry * hentry = coordList.Alloc();
511
512 hentry->bb = bb;
513
514 hentry->flag = CCoordListEntry::HighEnd;
515
516 bb->maxCoord[dim] = hentry;
517
518 if (bb->AsCollisionBody())
519 {
520 bb->AsCollisionBody()->UpdateAABB();
521 }
522 else
523 {
524 bb->AsRigidBody()->UpdateAABB();
525 }
526
527 if (!startSearch)
528 {
529 return;
530 }
531
532 CCoordListEntryItem * lentryItem = (CCoordListEntryItem*)lentry;
533
534 CCoordListEntryItem * hentryItem = (CCoordListEntryItem*)hentry;
535
536 if (lentryItem->thing.value >= lentryItem->prev->thing.value)
537 {
538 //already in place
539 return;
540 }
541
542 lentryItem->Remove();
543
544 hentryItem->Remove();
545
546 coordList.usedTail = startSearch;
547
548 if (hint)
549 {
550 if (hint->minBound[hintCoord] == 0)
551 {
552 hint = NULL;
553 }
554 else
555 {
556 ASSERT(hint->minBound[hintCoord] && hint->maxBound[hintCoord]);
557 }
558 }
559
560 if (!hint)
561 {
562 //search from the end of the list
563
564 CCoordListEntryItem* cur = startSearch;
565
566 neBool done = false;
567
568 do
569 {
570 if (lentryItem->thing.value < cur->thing.value)
571 {
572 if (cur->prev)
573 {
574 cur = cur->prev;
575 }
576 else
577 {
578 cur->Insert(lentryItem);
579
580 coordList.used = lentryItem;
581
582 done = true;
583 }
584 }
585 else
586 {
587 if (cur == coordList.usedTail)
588 coordList.usedTail = lentryItem;
589
590 cur->Append(lentryItem);
591
592 done = true;
593 }
594 } while (!done);
595
596 cur = startSearch;
597
598 done = false;
599
600 do
601 {
602 if (hentryItem->thing.value <= cur->thing.value)
603 {
604 ASSERT(hentryItem->thing.bb != cur->thing.bb); //should go pass the same body's low end
605
606 if (cur->prev)
607 {
608 cur = cur->prev;
609 }
610 else
611 {
612 cur->Insert(hentryItem);
613
614 coordList.used = hentryItem;
615
616 done = true;
617 }
618 }
619 else
620 {
621 if (cur == coordList.usedTail)
622 coordList.usedTail = hentryItem;
623
624 cur->Append(hentryItem);
625
626 done = true;
627 }
628 } while (!done);
629 }
630 else
631 {
632 //search from where the hint is
633
634 for (s32 i = 0; i < 2; i++)
635 {
636 CCoordListEntryItem * entryItem;
637
638 CCoordListEntryItem* cur;
639
640 neBool searchUp;
641
642 if (i == 0)
643 {
644 entryItem = lentryItem;
645
646 cur = (CCoordListEntryItem*)hint->minCoord[hintCoord];
647
648 if (entryItem->thing.value < cur->thing.value)
649 searchUp = true;
650 else
651 searchUp = false;
652 }
653 else
654 {
655 cur = lentryItem;
656
657 entryItem = hentryItem;
658
659 searchUp = false;
660 }
661
662 if (searchUp)
663 {
664 neBool done = false;
665
666 do
667 {
668 if (entryItem->thing.value < cur->thing.value)
669 {
670 if (cur->prev)
671 {
672 cur = cur->prev;
673 }
674 else
675 {
676 cur->Insert(entryItem);
677
678 coordList.used = entryItem;
679
680 done = true;
681 }
682 }
683 else
684 {
685 if (cur == coordList.usedTail)
686 coordList.usedTail = entryItem;
687
688 cur->Append(lentryItem);
689
690 done = true;
691 }
692 } while (!done);
693 }
694 else
695 {
696 neBool done = false;
697
698 do
699 {
700 if (entryItem->thing.value >= cur->thing.value)
701 {
702 if (cur->next)
703 {
704 cur = cur->next;
705 }
706 else
707 {
708 cur->Append(entryItem);
709
710 coordList.usedTail = entryItem;
711
712 done = true;
713 }
714 }
715 else
716 {
717 if (cur == coordList.used)
718 coordList.used = entryItem;
719
720 cur->Insert(entryItem);
721
722 done = true;
723 }
724 } while (!done);
725 }
726 }
727 }
728 }
729
730 /****************************************************************************
731 *
732 * neCoordList::Sort
733 *
734 ****************************************************************************/
735
Sort(bool sortOnly)736 void neCoordList::Sort(bool sortOnly)
737 {
738 neDLinkList<CCoordListEntry>::listItem * sortStart = coordList.used;
739
740 if (! sortStart)
741 return;
742
743 coordList.used = sortStart->next;
744
745 sortStart->Remove();
746
747 neDLinkList<CCoordListEntry>::listItem * usedStart = sortStart;
748
749 neDLinkList<CCoordListEntry>::listItem * usedTail = NULL;
750
751 while (coordList.used)
752 {
753 neDLinkList<CCoordListEntry>::listItem * nextUsed;
754
755 nextUsed = coordList.used->next;
756
757 coordList.used->Remove();
758
759 neDLinkList<CCoordListEntry>::listItem * insert = coordList.used;
760
761 // insert sort start here
762 bool done = false;
763
764 if (insert->thing.flag == CCoordListEntry::LowEnd)
765 {
766 neDLinkList<CCoordListEntry>::listItem * cur = sortStart;
767
768 while (!done && cur)
769 {
770 //compare
771 if (cur->thing.bb == insert->thing.bb)
772 {
773 if (cur->prev)
774 {
775 cur = cur->prev;
776 }
777 else
778 {
779 cur->Insert(insert);
780
781 usedStart = insert;
782
783 done = true;
784 }
785 }
786 else
787 {
788 if (insert->thing.value < cur->thing.value)
789 {
790 // update overlap status
791 if (cur->thing.flag == CCoordListEntry::HighEnd && !sortOnly)
792 {
793 neRigidBody_ * b1 = cur->thing.bb->AsRigidBody();
794
795 neRigidBody_ * b2 = insert->thing.bb->AsRigidBody();
796
797 if (b1 && b2)
798 {
799 if (! (b1->IsParticle() && b2->IsParticle()))
800 region->ToggleOverlapStatus(cur->thing.bb, insert->thing.bb, dimPower2);
801 }
802 else
803 {
804 region->ToggleOverlapStatus(cur->thing.bb, insert->thing.bb, dimPower2);
805 }
806 }
807 if (cur->prev)
808 {
809 cur = cur->prev;
810 }
811 else
812 {
813 cur->Insert(insert);
814
815 usedStart = insert;
816
817 done = true;
818 }
819 }
820 else
821 {
822 cur->Append(insert);
823
824 done = true;
825
826 if (cur == sortStart)
827 {
828 sortStart = insert;
829 }
830 }
831 }
832 }
833 }
834 else //HighEnd
835 {
836 neDLinkList<CCoordListEntry>::listItem * cur = sortStart;
837
838 while (!done && cur)
839 {
840 //compare
841 if (cur->thing.bb == insert->thing.bb)
842 {
843 cur->Append(insert);
844
845 done = true;
846
847 if (cur == sortStart)
848 sortStart = insert;
849 }
850 else
851 {
852 if (insert->thing.value < cur->thing.value)
853 {
854 // update overlap status
855 if (cur->thing.flag == CCoordListEntry::LowEnd && !sortOnly)
856 {
857 neRigidBody_ * b1 = cur->thing.bb->AsRigidBody();
858
859 neRigidBody_ * b2 = insert->thing.bb->AsRigidBody();
860
861 if (b1 && b2)
862 {
863 if (! (b1->IsParticle() && b2->IsParticle()))
864 region->ToggleOverlapStatus(cur->thing.bb, insert->thing.bb, dimPower2);
865 }
866 else
867 {
868 region->ToggleOverlapStatus(cur->thing.bb, insert->thing.bb, dimPower2);
869 }
870 }
871
872 if (cur->prev)
873 {
874 cur = cur->prev;
875 }
876 else
877 {
878 cur->Insert(insert);
879
880 usedStart = insert;
881
882 done = true;
883 }
884 }
885 else
886 {
887 cur->Append(insert);
888
889 done = true;
890
891 if (cur == sortStart)
892 sortStart = insert;
893 }
894 }
895 }
896 }
897
898 // insert sort end
899
900 coordList.used = nextUsed;
901 }
902
903 coordList.used = usedStart;
904
905 coordList.usedTail = sortStart;
906 }
907
908 #ifdef _DEBUG_REGION
OuputDebug()909 void neCoordList::OuputDebug()
910 {
911 neDLinkList<CCoordListEntry>::iterator iter;
912
913 zDebugMessage("Coord %d\n", this->dim);
914
915 for (iter = coordList.BeginUsed(); iter.Valid(); iter++)
916 {
917 CCoordListEntry * c = (*iter);
918
919 char * high = "High";
920 char * low = "Low";
921
922 char * fs;
923
924 if (c->flag == CCoordListEntry::LowEnd)
925 fs = low;
926 else
927 fs = high;
928
929 zDebugMessage("object id = %d %s %f \n", c->bb->id, fs, c->value);
930 }
931 }
932 #endif //_DEBUG
933