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 "stack.h"
23 #include "simulator.h"
24 #include "scenery.h"
25 #include "message.h"
26
27 //#include <assert.h>
28 #include <stdio.h>
29 #ifdef _WIN32
30 #include <windows.h>
31 #endif
32
33 char neFixedTimeStepSimulator::logBuffer[256];
34
35 //extern void DrawLine(const neV3 & colour, neV3 * startpoint, s32 count);
36
37 extern s32 currentMicroStep;
38
39 #define NE_HIGH_ENERGY 1.0f
40
41 void ChooseAxis(neV3 & x, neV3 & y, const neV3 & normal);
42
43 /****************************************************************************
44 *
45 * neFixedTimeStepSimulator::neFixedTimeStepSimulator(
46 *
47 ****************************************************************************/
48
neFixedTimeStepSimulator(const neSimulatorSizeInfo & _sizeInfo,neAllocatorAbstract * alloc,const neV3 * grav)49 neFixedTimeStepSimulator::neFixedTimeStepSimulator(const neSimulatorSizeInfo & _sizeInfo, neAllocatorAbstract * alloc, const neV3 * grav)
50 {
51 sizeInfo = _sizeInfo;
52
53 if (alloc)
54 {
55 allocator = alloc;
56 }
57 else
58 {
59 allocator = &allocDef;
60 }
61 neV3 g;
62
63 if (grav)
64 g = *grav;
65 else
66 g.SetZero();
67
68 neFixedTimeStepSimulator::Initialise(g);
69
70 for (int i=0; i < MAX_MATERIAL; i++)
71 {
72 materials[i].density = 1.0f;
73 materials[i].friction = .5f;
74 materials[i].resititution = 0.4f;
75 }
76 perfReport = NULL;
77
78 treeNodes.Reserve(100, allocator, 100);
79
80 triangleIndex.Reserve(200, allocator, 200);
81
82 constraintHeap.Reserve(sizeInfo.constraintsCount , allocator);
83
84 constraintHeaders.Reserve(sizeInfo.constraintSetsCount , allocator);
85
86 //miniConstraintHeap.Reserve(sizeInfo.constraintBufferSize, allocator);
87
88 stackInfoHeap.Reserve(sizeInfo.rigidBodiesCount + sizeInfo.rigidParticleCount , allocator);
89
90 stackHeaderHeap.Reserve(sizeInfo.rigidBodiesCount + sizeInfo.rigidParticleCount + 100, allocator);
91
92 controllerHeap.Reserve(sizeInfo.controllersCount, allocator);
93
94 sensorHeap.Reserve(sizeInfo.sensorsCount, allocator);
95
96 geometryHeap.Reserve(sizeInfo.geometriesCount, allocator);
97
98 pointerBuffer1.Reserve(1000, allocator, 100);
99
100 pointerBuffer2.Reserve(1000, allocator, 100);
101
102 cresultHeap.Reserve(100, allocator, 100);
103
104 cresultHeap2.Reserve(100, allocator, 100);
105
106 //fastImpulseHeap.Reserve(500, allocator);
107
108 logCallback = NULL;
109
110 collisionCallback = NULL;
111
112 breakageCallback = NULL;
113
114 terrainQueryCallback = NULL;
115
116 customCDRB2RBCallback = NULL;
117
118 customCDRB2ABCallback = NULL;
119
120 logLevel = neSimulator::LOG_OUTPUT_LEVEL_NONE;
121
122 // solver.sim = this;
123
124 fakeCollisionBody.moved = false;
125
126 fakeCollisionBody.sim = this;
127
128 fakeCollisionBody.id = -1;
129
130 fakeCollisionBody.cookies = 0;
131
132 fakeCollisionBody.isActive = true;
133
134 highEnergy = NE_HIGH_ENERGY;
135
136 #ifdef _WIN32
137 perf = nePerformanceData::Create();
138
139 perf->Init();
140 #else
141 perf = NULL;
142 #endif
143
144 timeFromLastFrame = 0.0f;
145
146 lastTimeStep = 0;
147 }
148
149 /****************************************************************************
150 *
151 * neFixedTimeStepSimulator::Initialise
152 *
153 ****************************************************************************/
SetGravity(const neV3 & g)154 void neFixedTimeStepSimulator::SetGravity(const neV3 & g)
155 {
156 gravity = g;
157
158 gravityVector = gravity;
159
160 gravityVector.Normalize();
161
162 restingSpeed = sqrtf(gravity.Dot(gravity));
163
164 //restingSpeed = sqrtf(restingSpeed * 2.0e-2f) * 4.0f;
165
166 restingSpeed = restingSpeed * 0.3f;
167
168 gravityMag = g.Length();
169
170 if (!neIsFinite(gravityMag))
171 {
172 gravityMag = 0.0f;
173 }
174 }
175
Initialise(const neV3 & _gravity)176 void neFixedTimeStepSimulator::Initialise(const neV3& _gravity)
177 {
178 buildCoordList = true;
179
180 SetGravity(_gravity);
181
182 maxRigidBodies = sizeInfo.rigidBodiesCount;
183
184 maxAnimBodies = sizeInfo.animatedBodiesCount;
185
186 maxParticles = sizeInfo.rigidParticleCount;
187
188 if (!rigidBodyHeap.Reserve(maxRigidBodies, allocator))
189 {
190 sprintf(logBuffer, MSG_MEMORY_ALLOC_FAILED);
191
192 LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
193
194 return;
195 }
196
197 if (!collisionBodyHeap.Reserve(maxAnimBodies, allocator))
198 {
199 sprintf(logBuffer, MSG_MEMORY_ALLOC_FAILED);
200
201 LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
202
203 return;
204 }
205
206 if (!rigidParticleHeap.Reserve(maxParticles, allocator))
207 {
208 sprintf(logBuffer, MSG_MEMORY_ALLOC_FAILED);
209
210 LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
211
212 return;
213 }
214
215 region.Initialise(this, neRegion::SORT_DIMENSION_X | neRegion::SORT_DIMENSION_Y | neRegion::SORT_DIMENSION_Z);
216
217 stepSoFar = 0;
218
219 stackHeaderX.Null();
220
221 stackHeaderX.isHeaderX = true;
222
223 stackHeaderX.sim = this;
224
225 fakeCollisionBody.b2w.SetIdentity();
226 }
227
228 /****************************************************************************
229 *
230 * neFixedTimeStepSimulator::SetCollisionCallback
231 *
232 ****************************************************************************/
233
SetCollisionCallback(neCollisionCallback * fn)234 neCollisionCallback * neFixedTimeStepSimulator::SetCollisionCallback(neCollisionCallback * fn)
235 {
236 neCollisionCallback * ret = collisionCallback;
237
238 collisionCallback = fn;
239
240 return ret;
241 }
242
243 /****************************************************************************
244 *
245 * neFixedTimeStepSimulator::SetLogOutputCallback
246 *
247 ****************************************************************************/
248
SetLogOutputCallback(neLogOutputCallback * fn)249 neLogOutputCallback * neFixedTimeStepSimulator::SetLogOutputCallback(neLogOutputCallback * fn)
250 {
251 neLogOutputCallback * ret = logCallback;
252
253 logCallback = fn;
254
255 return ret;
256 }
257
258 /****************************************************************************
259 *
260 * neFixedTimeStepSimulator::SetLogOutputLevel
261 *
262 ****************************************************************************/
263
SetLogOutputLevel(neSimulator::LOG_OUTPUT_LEVEL lvl)264 void neFixedTimeStepSimulator::SetLogOutputLevel(neSimulator::LOG_OUTPUT_LEVEL lvl)
265 {
266 logLevel = lvl;
267 }
268
269 /****************************************************************************
270 *
271 * neFixedTimeStepSimulator::LogOutput
272 *
273 ****************************************************************************/
274
LogOutput(neSimulator::LOG_OUTPUT_LEVEL lvl)275 void neFixedTimeStepSimulator::LogOutput(neSimulator::LOG_OUTPUT_LEVEL lvl)
276 {
277 if (!logCallback)
278 return;
279
280 if (lvl <= logLevel)
281 logCallback(logBuffer);
282 }
283
284 /****************************************************************************
285 *
286 * neFixedTimeStepSimulator::SetMaterial
287 *
288 ****************************************************************************/
289
SetMaterial(s32 index,f32 friction,f32 restitution,f32 density)290 bool neFixedTimeStepSimulator::SetMaterial(s32 index, f32 friction, f32 restitution, f32 density)
291 {
292 if (index < 0)
293 return false;
294
295 if (index >= MAX_MATERIAL)
296 return false;
297
298 materials[index].density = density;
299 materials[index].friction = friction;
300 materials[index].resititution = restitution;
301
302 return true;
303 }
304
305 /****************************************************************************
306 *
307 * neFixedTimeStepSimulator::GetMaterial
308 *
309 ****************************************************************************/
310
GetMaterial(s32 index,f32 & friction,f32 & restitution,f32 & density)311 bool neFixedTimeStepSimulator::GetMaterial(s32 index, f32& friction, f32& restitution, f32& density)
312 {
313 if (index < 0)
314 return false;
315
316 if (index >= MAX_MATERIAL)
317 return false;
318
319 density = materials[index].density;
320 friction = materials[index].friction;
321 restitution = materials[index].resititution;
322
323 return true;
324 }
325
326 /****************************************************************************
327 *
328 * neFixedTimeStepSimulator::CreateRigidBody
329 *
330 ****************************************************************************/
331
CreateRigidBody(neBool isParticle)332 neRigidBody_* neFixedTimeStepSimulator::CreateRigidBody(neBool isParticle)
333 {
334 neRigidBody_ * ret;
335
336 if (!isParticle)
337 {
338 ret = rigidBodyHeap.Alloc(1);
339
340 if (!ret)
341 {
342 sprintf(logBuffer, MSG_RUN_OUT_RIDIGBODY);
343
344 LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
345
346 return NULL;
347 }
348
349 //ASSERT(ret);
350
351 new (ret) neRigidBody_;
352
353 activeRB.Add(ret);
354
355 ret->id = rigidBodyHeap.GetID(ret);
356
357 ret->subType = NE_RIGID_NORMAL;
358 }
359 else
360 {
361 ret = rigidParticleHeap.Alloc(1);
362
363 if (!ret)
364 {
365 sprintf(logBuffer, MSG_RUN_OUT_RIDIGPARTICLE);
366
367 LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
368
369 return NULL;
370 }
371 //ASSERT(ret);
372
373 new (ret) neRigidBody_;
374
375 activeRP.Add(ret);
376
377 ret->id = rigidParticleHeap.GetID(ret) + rigidBodyHeap.Size() + collisionBodyHeap.Size();
378
379 ret->subType = NE_RIGID_PARTICLE;
380 }
381
382 ret->col.convexCount = 0;
383
384 ret->col.obb.Initialise();
385
386 ret->sim = this;
387
388 return ret;
389 }
390
CreateRigidBodyFromConvex(TConvex * convex,neRigidBodyBase * originalBody)391 neRigidBody_ * neFixedTimeStepSimulator::CreateRigidBodyFromConvex(TConvex * convex, neRigidBodyBase * originalBody)
392 {
393 //make sure convex belong to this body and
394 //this convex is not the only convex on this body
395
396 originalBody->BeginIterateGeometry();
397
398 TConvex * con;
399
400 s32 ccount = 0;
401
402 neBool found = false;
403
404 while (con = originalBody->GetNextGeometry())
405 {
406 if (con == convex)
407 {
408 found = true;
409 }
410 ccount++;
411 }
412
413 if (ccount == 1 || !found)
414 {
415 return NULL;
416 }
417 neBool isParticle = false;
418
419 if (convex->breakInfo.flag == neGeometry::NE_BREAK_ALL_PARTICLE ||
420 convex->breakInfo.flag == neGeometry::NE_BREAK_NORMAL_PARTICLE ||
421 convex->breakInfo.flag == neGeometry::NE_BREAK_NEIGHBOUR_PARTICLE)
422 {
423 isParticle = true;
424 }
425
426 neRigidBody_ * newBody = CreateRigidBody(false);
427
428 if (!newBody)
429 {
430 return NULL;
431 }
432
433 newBody->mass = convex->breakInfo.mass;
434
435 newBody->oneOnMass = 1.0f / convex->breakInfo.mass;
436
437 newBody->Ibody.SetIdentity();
438
439 for (s32 i = 0; i < 3; i++)
440 newBody->Ibody[i][i] = convex->breakInfo.inertiaTensor[i];
441
442 newBody->IbodyInv.SetInvert(newBody->Ibody);
443
444 convex->breakInfo.flag = neGeometry::NE_BREAK_DISABLE;
445
446 newBody->State().b2w = originalBody->GetB2W() * convex->c2p;
447
448 newBody->State().q.SetupFromMatrix3(newBody->State().rot());
449
450 originalBody->col.convexCount--;
451
452 if (originalBody->col.convex == convex)
453 originalBody->col.convex = (TConvex*)((TConvexItem *)convex)->next;
454
455 if (originalBody->col.convexCount == 0 && originalBody->isActive)
456 {
457 region.RemoveBody(originalBody);
458 }
459 else if (originalBody->col.convexCount == 1)
460 {
461 originalBody->RecalcBB();
462 }
463
464 ((TConvexItem *)convex)->Remove();
465
466 convex->c2p.SetIdentity();
467
468 newBody->col.convex = convex;
469
470 newBody->col.convexCount++;
471
472 newBody->RecalcBB();
473
474 region.AddBody(newBody, originalBody);
475
476 return newBody;
477 }
478
479 /****************************************************************************
480 *
481 * neFixedTimeStepSimulator::CreateAnimateBody
482 *
483 ****************************************************************************/
484
CreateCollisionBody()485 neCollisionBody_* neFixedTimeStepSimulator::CreateCollisionBody()
486 {
487 neCollisionBody_ * ret = collisionBodyHeap.Alloc(1);
488
489 //ASSERT(ret);
490
491 new (ret) neCollisionBody_;
492
493 activeCB.Add(ret);
494
495 ret->id = collisionBodyHeap.GetID(ret) + rigidBodyHeap.Size();
496
497 ret->col.convexCount = 0;
498
499 ret->col.obb.Initialise();
500
501 ret->sim = this;
502
503 ret->b2w.SetIdentity();
504
505 //region.AddBody(ret);
506
507 return ret;
508 }
509
510 /****************************************************************************
511 *
512 * neFixedTimeStepSimulator::Free
513 *
514 ****************************************************************************/
515
Free(neRigidBodyBase * bb)516 void neFixedTimeStepSimulator::Free(neRigidBodyBase * bb)
517 {
518 if (bb->AsCollisionBody())
519 {
520 neCollisionBody_* cb = reinterpret_cast<neCollisionBody_*>(bb);
521
522 if (collisionBodyHeap.CheckBelongAndInUse(cb))
523 {
524 ((neCollisionBody_*)bb)->Free();
525
526 if (bb->isActive)
527 activeCB.Remove(cb);
528 else
529 inactiveCB.Remove(cb);
530
531 collisionBodyHeap.Dealloc(cb, 1);
532 }
533 else
534 {
535 sprintf(logBuffer, MSG_TRYING_TO_FREE_INVALID_CB);
536
537 LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
538 }
539 }
540 else
541 {
542 neRigidBody_* rb = reinterpret_cast<neRigidBody_*>(bb);
543
544 rb->Free();
545
546 if (rb->IsParticle())
547 {
548 if (rigidParticleHeap.CheckBelongAndInUse(rb))
549 {
550 if (rb->isActive)
551 activeRP.Remove(rb);
552 else
553 inactiveRP.Remove(rb);
554
555 rigidParticleHeap.Dealloc(rb, 1);
556 }
557 else
558 {
559 sprintf(logBuffer, MSG_TRYING_TO_FREE_INVALID_RP);
560
561 LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
562 }
563 }
564 else
565 {
566 if (rigidBodyHeap.CheckBelongAndInUse(rb))
567 {
568 if (rb->isActive)
569 activeRB.Remove(rb);
570 else
571 inactiveRB.Remove(rb);
572
573 rigidBodyHeap.Dealloc(rb, 1);
574 }
575 else
576 {
577 sprintf(logBuffer, MSG_TRYING_TO_FREE_INVALID_RB);
578
579 LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
580 }
581 }
582 }
583 }
584
585 /****************************************************************************
586 *
587 * ~neFixedTimeStepSimulator::neFixedTimeStepSimulator
588 *
589 ****************************************************************************/
590
~neFixedTimeStepSimulator()591 neFixedTimeStepSimulator::~neFixedTimeStepSimulator()
592 {
593 FreeAllBodies();
594
595 if (perf)
596 delete perf;
597 }
598
599 ///////////////////////////////////////////////////////////////////
600
601 //#define DETAIL_PERF_REPORTING
602
603 #ifdef _WIN32
604 #define UPDATE_PERF_REPORT(n) {if (perfReport) perf->n();}
605 #else
606 #define UPDATE_PERF_REPORT(n)
607 #endif
608
Advance(nePerformanceReport * _perfReport)609 void neFixedTimeStepSimulator::Advance(nePerformanceReport * _perfReport)
610 {
611 ClearCollisionBodySensors();
612
613 UpdateAABB();
614
615 region.Update();
616
617 UPDATE_PERF_REPORT(UpdateCDCulling)
618
619 CheckCollision();
620
621 UPDATE_PERF_REPORT(UpdateCD);
622
623 CheckTerrainCollision();
624
625 UPDATE_PERF_REPORT(UpdateTerrain);
626
627 ResetTotalForce();
628
629 ApplyJointDamping();
630
631 //Advance Rigid Body Dynamic
632 AdvanceDynamicRigidBodies();
633
634 //Advance Rigid Particle Dynamic
635 AdvanceDynamicParticles();
636
637 UPDATE_PERF_REPORT(UpdateDynamic);
638
639 ResetStackHeaderFlag();
640
641 SolveAllConstrain();
642
643 //UPDATE_PERF_REPORT(UpdateConstrain1);
644
645 ResolvePenetration();
646
647 SolveContactConstrain();
648
649 UPDATE_PERF_REPORT(UpdateConstrain2);
650
651 //Advance Position
652
653 AdvancePositionRigidBodies();
654
655 AdvancePositionParticles();
656
657 UPDATE_PERF_REPORT(UpdatePosition);
658
659 UpdateConstraintControllers();
660
661 UPDATE_PERF_REPORT(UpdateControllerCallback);
662 }
663
Advance(f32 time,u32 nStep,nePerformanceReport * _perfReport)664 void neFixedTimeStepSimulator::Advance(f32 time, u32 nStep, nePerformanceReport * _perfReport)
665 {
666 _currentTimeStep = time / (f32)nStep;
667
668 oneOnCurrentTimeStep = 1.0f / _currentTimeStep;
669
670 perfReport = _perfReport;
671
672 currentRecord = stepSoFar % NE_RB_MAX_PAST_RECORDS;
673
674 #ifdef _WIN32
675 if (perfReport)
676 {
677 for (s32 j = 0; j < nePerformanceReport::NE_PERF_LAST; j++)
678 {
679 perfReport->time[j] = 0.0f;
680 }
681 perf->Start();
682 }
683 #endif
684
685 int i;
686
687 for (i = 0; i < (s32)nStep; i++)
688 {
689 magicNumber = 0;
690
691 Advance(perfReport);
692 }
693
694 neCollisionBody_ * cb = activeCB.GetHead();
695
696 while (cb)
697 {
698 cb->moved = false;
699
700 cb = activeCB.GetNext(cb);
701 }
702 if (perfReport)
703 {
704 if (perfReport->reportType == nePerformanceReport::NE_PERF_SAMPLE)
705 {
706 f32 totalTime = perfReport->time[nePerformanceReport::NE_PERF_TOTAL_TIME] = perf->GetTotalTime();
707
708 #ifdef DETAIL_PERF_REPORTING
709
710 perfReport->time[nePerformanceReport::NE_PERF_DYNAMIC] = perf->dynamic / totalTime * 100.0f;
711 perfReport->time[nePerformanceReport::NE_PERF_POSITION] = perf->position / totalTime * 100.0f;
712 perfReport->time[nePerformanceReport::NE_PERF_COLLISION_DETECTION] = perf->cd / totalTime * 100.0f;
713 perfReport->time[nePerformanceReport::NE_PERF_COLLISION_CULLING] = perf->cdCulling / totalTime * 100.0f;
714 perfReport->time[nePerformanceReport::NE_PERF_TERRAIN] = perf->terrain / totalTime * 100.0f;
715 perfReport->time[nePerformanceReport::NE_PERF_TERRAIN_CULLING] = perf->terrainCulling / totalTime * 100.0f;
716 perfReport->time[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_1] = perf->constrain_1 / totalTime * 100.0f;
717 perfReport->time[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_2] = perf->constrain_2 / totalTime * 100.0f;
718 perfReport->time[nePerformanceReport::NE_PERF_CONTROLLER_CALLBACK] = perf->controllerCallback / totalTime * 100.0f;;
719 #endif
720 }
721 else
722 {
723 f32 totalTime = perf->GetTotalTime();
724
725 if (totalTime < 100.0f)
726 {
727 perfReport->numSample ++;
728
729 perfReport->accTime[nePerformanceReport::NE_PERF_TOTAL_TIME] += totalTime;
730
731 #ifdef DETAIL_PERF_REPORTING
732 perfReport->accTime[nePerformanceReport::NE_PERF_DYNAMIC] += (perf->dynamic / totalTime * 100.0f);
733 perfReport->accTime[nePerformanceReport::NE_PERF_POSITION] += (perf->position / totalTime * 100.0f);
734 perfReport->accTime[nePerformanceReport::NE_PERF_COLLISION_DETECTION] += (perf->cd / totalTime * 100.0f);
735 perfReport->accTime[nePerformanceReport::NE_PERF_COLLISION_CULLING] += (perf->cdCulling / totalTime * 100.0f);
736 perfReport->accTime[nePerformanceReport::NE_PERF_TERRAIN] += (perf->terrain / totalTime * 100.0f);
737 perfReport->accTime[nePerformanceReport::NE_PERF_TERRAIN_CULLING] += (perf->terrainCulling / totalTime * 100.0f);
738 perfReport->accTime[nePerformanceReport::NE_PERF_CONTROLLER_CALLBACK] += (perf->controllerCallback / totalTime * 100.0f);
739 perfReport->accTime[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_1] += (perf->constrain_1 / totalTime * 100.0f);
740 perfReport->accTime[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_2] += (perf->constrain_2 / totalTime * 100.0f);
741 #endif
742 perfReport->time[nePerformanceReport::NE_PERF_TOTAL_TIME] = perfReport->accTime[nePerformanceReport::NE_PERF_TOTAL_TIME] / perfReport->numSample;
743 #ifdef DETAIL_PERF_REPORTING
744 perfReport->time[nePerformanceReport::NE_PERF_DYNAMIC] = perfReport->accTime[nePerformanceReport::NE_PERF_DYNAMIC] / perfReport->numSample;
745 perfReport->time[nePerformanceReport::NE_PERF_POSITION] = perfReport->accTime[nePerformanceReport::NE_PERF_POSITION] / perfReport->numSample;
746 perfReport->time[nePerformanceReport::NE_PERF_COLLISION_DETECTION] = perfReport->accTime[nePerformanceReport::NE_PERF_COLLISION_DETECTION] / perfReport->numSample;
747 perfReport->time[nePerformanceReport::NE_PERF_COLLISION_CULLING] = perfReport->accTime[nePerformanceReport::NE_PERF_COLLISION_CULLING] / perfReport->numSample;
748 perfReport->time[nePerformanceReport::NE_PERF_TERRAIN] = perfReport->accTime[nePerformanceReport::NE_PERF_TERRAIN] / perfReport->numSample;
749 perfReport->time[nePerformanceReport::NE_PERF_TERRAIN_CULLING] = perfReport->accTime[nePerformanceReport::NE_PERF_TERRAIN_CULLING] / perfReport->numSample;
750 perfReport->time[nePerformanceReport::NE_PERF_CONTROLLER_CALLBACK] = perfReport->accTime[nePerformanceReport::NE_PERF_CONTROLLER_CALLBACK] / perfReport->numSample;
751 perfReport->time[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_1] = perfReport->accTime[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_1] / perfReport->numSample;
752 perfReport->time[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_2] = perfReport->accTime[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_2] / perfReport->numSample;
753 #endif
754 }
755 }
756 }
757
758 stepSoFar++;
759 }
760
Advance(f32 sec,f32 minTimeStep,f32 maxTimeStep,nePerformanceReport * _perfReport)761 void neFixedTimeStepSimulator::Advance(f32 sec, f32 minTimeStep, f32 maxTimeStep, nePerformanceReport * _perfReport)
762 {
763 perfReport = _perfReport;
764
765 #ifdef _WIN32
766 if (perfReport)
767 {
768 for (s32 j = 0; j < nePerformanceReport::NE_PERF_LAST; j++)
769 {
770 perfReport->time[j] = 0.0f;
771 }
772 perf->Start();
773 }
774 #endif
775
776 const f32 frameDiffTolerance = 0.2f;
777
778 f32 timeLeft = sec + timeFromLastFrame;
779
780 f32 currentTimeStep = maxTimeStep;
781
782 while (minTimeStep <= timeLeft)
783 {
784 while (currentTimeStep <= timeLeft)
785 {
786 if ((lastTimeStep > 0.0f) && neIsFinite(lastTimeStep))
787 {
788 f32 diffPercent = neAbs((currentTimeStep - lastTimeStep) / lastTimeStep);
789
790 if (diffPercent > frameDiffTolerance) // more than 20% different
791 {
792 if (currentTimeStep > lastTimeStep)
793 {
794 currentTimeStep = lastTimeStep * (1.0f + frameDiffTolerance);
795 }
796 else
797 {
798 currentTimeStep = lastTimeStep * (1.0f - frameDiffTolerance);
799 }
800 }
801 }
802
803 _currentTimeStep = currentTimeStep;
804
805 oneOnCurrentTimeStep = 1.0f / _currentTimeStep;
806
807 currentRecord = stepSoFar % NE_RB_MAX_PAST_RECORDS;
808
809 Advance(perfReport);
810
811 stepSoFar++;
812
813 timeLeft -= currentTimeStep;
814
815 lastTimeStep = currentTimeStep;
816 }
817 currentTimeStep = neMin(timeLeft, maxTimeStep);
818 }
819 timeFromLastFrame = timeLeft;
820
821 neCollisionBody_ * cb = activeCB.GetHead();
822
823 while (cb)
824 {
825 cb->moved = false;
826
827 cb = activeCB.GetNext(cb);
828 }
829 if (perfReport)
830 {
831 if (perfReport->reportType == nePerformanceReport::NE_PERF_SAMPLE)
832 {
833 f32 totalTime = perfReport->time[nePerformanceReport::NE_PERF_TOTAL_TIME] = perf->GetTotalTime();
834
835 #ifdef DETAIL_PERF_REPORTING
836
837 perfReport->time[nePerformanceReport::NE_PERF_DYNAMIC] = perf->dynamic / totalTime * 100.0f;
838 perfReport->time[nePerformanceReport::NE_PERF_POSITION] = perf->position / totalTime * 100.0f;
839 perfReport->time[nePerformanceReport::NE_PERF_COLLISION_DETECTION] = perf->cd / totalTime * 100.0f;
840 perfReport->time[nePerformanceReport::NE_PERF_COLLISION_CULLING] = perf->cdCulling / totalTime * 100.0f;
841 perfReport->time[nePerformanceReport::NE_PERF_TERRAIN] = perf->terrain / totalTime * 100.0f;
842 perfReport->time[nePerformanceReport::NE_PERF_TERRAIN_CULLING] = perf->terrainCulling / totalTime * 100.0f;
843 perfReport->time[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_1] = perf->constrain_1 / totalTime * 100.0f;
844 perfReport->time[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_2] = perf->constrain_2 / totalTime * 100.0f;
845 perfReport->time[nePerformanceReport::NE_PERF_CONTROLLER_CALLBACK] = perf->controllerCallback / totalTime * 100.0f;;
846 #endif
847 }
848 else
849 {
850 f32 totalTime = perf->GetTotalTime();
851
852 if (totalTime < 100.0f)
853 {
854 perfReport->numSample ++;
855
856 perfReport->accTime[nePerformanceReport::NE_PERF_TOTAL_TIME] += totalTime;
857
858 #ifdef DETAIL_PERF_REPORTING
859 perfReport->accTime[nePerformanceReport::NE_PERF_DYNAMIC] += (perf->dynamic / totalTime * 100.0f);
860 perfReport->accTime[nePerformanceReport::NE_PERF_POSITION] += (perf->position / totalTime * 100.0f);
861 perfReport->accTime[nePerformanceReport::NE_PERF_COLLISION_DETECTION] += (perf->cd / totalTime * 100.0f);
862 perfReport->accTime[nePerformanceReport::NE_PERF_COLLISION_CULLING] += (perf->cdCulling / totalTime * 100.0f);
863 perfReport->accTime[nePerformanceReport::NE_PERF_TERRAIN] += (perf->terrain / totalTime * 100.0f);
864 perfReport->accTime[nePerformanceReport::NE_PERF_TERRAIN_CULLING] += (perf->terrainCulling / totalTime * 100.0f);
865 perfReport->accTime[nePerformanceReport::NE_PERF_CONTROLLER_CALLBACK] += (perf->controllerCallback / totalTime * 100.0f);
866 perfReport->accTime[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_1] += (perf->constrain_1 / totalTime * 100.0f);
867 perfReport->accTime[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_2] += (perf->constrain_2 / totalTime * 100.0f);
868 #endif
869 perfReport->time[nePerformanceReport::NE_PERF_TOTAL_TIME] = perfReport->accTime[nePerformanceReport::NE_PERF_TOTAL_TIME] / perfReport->numSample;
870 #ifdef DETAIL_PERF_REPORTING
871 perfReport->time[nePerformanceReport::NE_PERF_DYNAMIC] = perfReport->accTime[nePerformanceReport::NE_PERF_DYNAMIC] / perfReport->numSample;
872 perfReport->time[nePerformanceReport::NE_PERF_POSITION] = perfReport->accTime[nePerformanceReport::NE_PERF_POSITION] / perfReport->numSample;
873 perfReport->time[nePerformanceReport::NE_PERF_COLLISION_DETECTION] = perfReport->accTime[nePerformanceReport::NE_PERF_COLLISION_DETECTION] / perfReport->numSample;
874 perfReport->time[nePerformanceReport::NE_PERF_COLLISION_CULLING] = perfReport->accTime[nePerformanceReport::NE_PERF_COLLISION_CULLING] / perfReport->numSample;
875 perfReport->time[nePerformanceReport::NE_PERF_TERRAIN] = perfReport->accTime[nePerformanceReport::NE_PERF_TERRAIN] / perfReport->numSample;
876 perfReport->time[nePerformanceReport::NE_PERF_TERRAIN_CULLING] = perfReport->accTime[nePerformanceReport::NE_PERF_TERRAIN_CULLING] / perfReport->numSample;
877 perfReport->time[nePerformanceReport::NE_PERF_CONTROLLER_CALLBACK] = perfReport->accTime[nePerformanceReport::NE_PERF_CONTROLLER_CALLBACK] / perfReport->numSample;
878 perfReport->time[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_1] = perfReport->accTime[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_1] / perfReport->numSample;
879 perfReport->time[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_2] = perfReport->accTime[nePerformanceReport::NE_PERF_CONTRAIN_SOLVING_2] / perfReport->numSample;
880 #endif
881 }
882 }
883 }
884 }
885
ResetTotalForce()886 void neFixedTimeStepSimulator::ResetTotalForce()
887 {
888 neRigidBody_ * rb = activeRB.GetHead();
889
890 while (rb)
891 {
892 rb->totalForce.SetZero();
893
894 rb->totalTorque.SetZero();
895
896 rb = activeRB.GetNext(rb);
897 }
898 rb = activeRP.GetHead();
899
900 while (rb)
901 {
902 rb->totalForce.SetZero();
903
904 rb->totalTorque.SetZero();
905
906 rb = activeRP.GetNext(rb);
907 }
908 }
909
AdvanceDynamicRigidBodies()910 void neFixedTimeStepSimulator::AdvanceDynamicRigidBodies()
911 {
912 neRigidBody_ * rb = activeRB.GetHead();
913
914 idleBodyCount = 0;
915
916 while (rb)
917 {
918 rb->AdvanceDynamic(_currentTimeStep);
919
920 rb = activeRB.GetNext(rb);
921 }
922 }
923
AdvanceDynamicParticles()924 void neFixedTimeStepSimulator::AdvanceDynamicParticles()
925 {
926 neRigidBody_ * rp = activeRP.GetHead();
927
928 while (rp)
929 {
930 rp->AdvanceDynamic(_currentTimeStep);
931
932 rp = activeRP.GetNext(rp);
933 }
934 }
935
AdvancePositionRigidBodies()936 void neFixedTimeStepSimulator::AdvancePositionRigidBodies()
937 {
938 neRigidBody_ * rb = activeRB.GetHead();
939
940 while (rb)
941 {
942 rb->needSolveContactDynamic = true;
943
944 if (rb->status != neRigidBody_::NE_RBSTATUS_IDLE)
945 {
946 if (!rb->_constraintHeader)
947 {
948 rb->CheckForIdle();
949 }
950
951 if (rb->status != neRigidBody_::NE_RBSTATUS_IDLE)
952 rb->AdvancePosition(_currentTimeStep);
953 }
954
955 rb = activeRB.GetNext(rb);
956 }
957 }
958
AdvancePositionParticles()959 void neFixedTimeStepSimulator::AdvancePositionParticles()
960 {
961 neRigidBody_ * rp = activeRP.GetHead();
962
963 while (rp)
964 {
965 rp->needSolveContactDynamic = true;
966
967 if (rp->status != neRigidBody_::NE_RBSTATUS_IDLE)
968 {
969 if (!rp->_constraintHeader)
970 {
971 rp->CheckForIdle();
972 }
973
974 if (rp->status != neRigidBody_::NE_RBSTATUS_IDLE)
975 rp->AdvancePosition(_currentTimeStep);
976 }
977
978 rp = activeRP.GetNext(rp);
979 }
980 }
981
ApplyJointDamping()982 void neFixedTimeStepSimulator::ApplyJointDamping()
983 {
984 neFreeListItem<neConstraintHeader> * hitem = (neFreeListItem<neConstraintHeader> *)(*constraintHeaders.BeginUsed());
985
986 while (hitem)
987 {
988 neConstraintHeader * h = (neConstraintHeader *)hitem;
989
990 hitem = hitem->next;
991
992 neFreeListItem<_neConstraint> * citem = (neFreeListItem<_neConstraint> *) h->head;
993
994 while (citem)
995 {
996 _neConstraint * c = (_neConstraint *)citem;
997
998 citem = citem->next;
999
1000 if (c->enable)
1001 c->ApplyDamping();
1002 }
1003 }
1004 }
1005
ClearCollisionBodySensors()1006 void neFixedTimeStepSimulator::ClearCollisionBodySensors()
1007 {
1008 neCollisionBody_ * cb = activeCB.GetHead();
1009
1010 while (cb)
1011 {
1012 if (cb->sensors)
1013 cb->ClearSensor();
1014
1015 cb = activeCB.GetNext(cb);
1016 }
1017
1018 neRigidBody_ * rp = activeRP.GetHead();
1019
1020 while (rp)
1021 {
1022 if (rp->sensors)
1023 rp->ClearSensor();
1024
1025 rp = activeRP.GetNext(rp);
1026 }
1027
1028 rp = activeRB.GetHead();
1029
1030 while (rp)
1031 {
1032 if (rp->sensors)
1033 rp->ClearSensor();
1034
1035 rp = activeRP.GetNext(rp);
1036 }
1037 }
1038
UpdateAABB()1039 void neFixedTimeStepSimulator::UpdateAABB()
1040 {
1041 neRigidBody_ * rb = activeRB.GetHead();
1042
1043 while (rb)
1044 {
1045 rb->UpdateAABB();
1046
1047 rb = activeRB.GetNext(rb);
1048 }
1049 neRigidBody_ * rp = activeRP.GetHead();
1050
1051 while (rp)
1052 {
1053 rp->UpdateAABB();
1054
1055 rp = activeRP.GetNext(rp);
1056 }
1057 neCollisionBody_ * cb = activeCB.GetHead();
1058
1059 while (cb)
1060 {
1061 if (cb->moved)
1062 cb->UpdateAABB();
1063
1064 cb = activeCB.GetNext(cb);
1065 }
1066 }
1067
1068 /****************************************************************************
1069 *
1070 * neFixedTimeStepSimulator::CheckCollision
1071 *
1072 ****************************************************************************/
1073
CheckCollision()1074 void neFixedTimeStepSimulator::CheckCollision()
1075 {
1076 //OutputDebugString("region.Update\n");/////////////////////////////////
1077
1078 neDLinkList<neOverlappedPair>::iterator oiter;
1079
1080 neCollisionResult result;
1081
1082 neV3 backupVector;
1083 //OutputDebugString("obj 2 obj test\n");/////////////////////////////////
1084
1085 for (oiter = region.overlappedPairs.BeginUsed(); oiter.Valid(); oiter++)
1086 {
1087 //ASSERT((*oiter)->bodyA);
1088 //ASSERT((*oiter)->bodyB);
1089
1090 neRigidBodyBase * bodyA;
1091 neRigidBodyBase * bodyB;
1092
1093 bodyA = (*oiter)->bodyA;
1094 bodyB = (*oiter)->bodyB;
1095
1096 neRigidBody_* ra = bodyA->AsRigidBody();
1097 neRigidBody_* rb = bodyB->AsRigidBody();
1098
1099 neCollisionBody_* ca = bodyA->AsCollisionBody();
1100 neCollisionBody_* cb = bodyB->AsCollisionBody();
1101
1102 if (ca && cb)
1103 continue;
1104
1105 neCollisionTable::neReponseBitFlag collisionflag = colTable.Get(bodyA->cid, bodyB->cid);
1106
1107 if (collisionflag == neCollisionTable::RESPONSE_IGNORE)
1108 continue;
1109
1110 neBool isCustomeCD = false;
1111
1112 result.penetrate = false;
1113
1114 if (ca)
1115 {
1116 if (rb->status != neRigidBody_::NE_RBSTATUS_IDLE ||
1117 rb->isShifted ||
1118 ca->moved)
1119 {
1120 if ((rb->isCustomCD || ca->isCustomCD))
1121 {
1122 if (customCDRB2ABCallback)
1123 {
1124 isCustomeCD = true;
1125
1126 neCustomCDInfo cdInfo;
1127
1128 memset(&cdInfo, 0, sizeof(cdInfo));
1129
1130 if (customCDRB2ABCallback((neRigidBody*)rb, (neAnimatedBody*)ca, cdInfo))
1131 {
1132 result.penetrate = true;
1133 result.bodyA = ca;
1134 result.bodyB = rb;
1135 result.collisionFrame[2] = -cdInfo.collisionNormal;
1136 result.materialIdA = cdInfo.materialIdB;
1137 result.materialIdB = cdInfo.materialIdA;
1138 result.contactA = cdInfo.worldContactPointB - ca->GetB2W().pos;
1139 result.contactB = cdInfo.worldContactPointA - rb->GetB2W().pos;
1140 result.contactAWorld = cdInfo.worldContactPointB;
1141 result.contactBWorld = cdInfo.worldContactPointA;
1142 result.contactABody = ca->GetB2W().rot.TransposeMulV3(result.contactA);
1143 result.contactBBody = rb->GetB2W().rot.TransposeMulV3(result.contactB);
1144 result.depth = cdInfo.penetrationDepth;
1145 ChooseAxis(result.collisionFrame[0], result.collisionFrame[1], result.collisionFrame[2]);
1146 }
1147 }
1148 }
1149 else
1150 {
1151 backupVector = rb->backupVector - ca->backupVector;
1152
1153 CollisionTest(result, ca->col, ca->b2w,
1154 rb->col, rb->State().b2w, backupVector);
1155
1156 if (rb->sensors)
1157 {
1158 CollisionTestSensor(&rb->col.obb,
1159 rb->sensors,
1160 rb->State().b2w,
1161 ca->col,
1162 ca->b2w,
1163 ca);
1164 }
1165 }
1166 }
1167 }
1168 else
1169 {
1170 if (cb)
1171 {
1172 if (ra->status != neRigidBody_::NE_RBSTATUS_IDLE ||
1173 ra->isShifted ||
1174 cb->moved)
1175 {
1176 if ((ra->isCustomCD || cb->isCustomCD))
1177 {
1178 if (customCDRB2ABCallback)
1179 {
1180 isCustomeCD = true;
1181
1182 neCustomCDInfo cdInfo;
1183
1184 memset(&cdInfo, 0, sizeof(cdInfo));
1185
1186 if (customCDRB2ABCallback((neRigidBody*)ra, (neAnimatedBody*)cb, cdInfo))
1187 {
1188 result.penetrate = true;
1189 result.bodyA = ra;
1190 result.bodyB = cb;
1191 result.collisionFrame[2] = cdInfo.collisionNormal;
1192 result.materialIdA = cdInfo.materialIdA;
1193 result.materialIdB = cdInfo.materialIdB;
1194 result.contactA = cdInfo.worldContactPointA - ra->GetB2W().pos;
1195 result.contactB = cdInfo.worldContactPointB - cb->GetB2W().pos;
1196 result.contactAWorld = cdInfo.worldContactPointA;
1197 result.contactBWorld = cdInfo.worldContactPointB;
1198 result.contactABody = ra->GetB2W().rot.TransposeMulV3(result.contactA);
1199 result.contactBBody = cb->GetB2W().rot.TransposeMulV3(result.contactB);
1200 result.depth = cdInfo.penetrationDepth;
1201 ChooseAxis(result.collisionFrame[0], result.collisionFrame[1], result.collisionFrame[2]);
1202 }
1203 }
1204 }
1205 else
1206 {
1207 backupVector = cb->backupVector - ra->backupVector;
1208
1209 CollisionTest(result, ra->col, ra->State().b2w,
1210 cb->col, cb->b2w, backupVector);
1211
1212 if (ra->sensors)
1213 {
1214 CollisionTestSensor(&ra->col.obb,
1215 ra->sensors,
1216 ra->State().b2w,
1217 cb->col,
1218 cb->b2w,
1219 cb);
1220 }
1221 }
1222 }
1223 }
1224 else
1225 {
1226 neBool doCollision = false;
1227
1228 if (ra->GetConstraintHeader() &&
1229 (ra->GetConstraintHeader() == rb->GetConstraintHeader()))
1230 {
1231 if (ra->isCollideConnected && rb->isCollideConnected)
1232 {
1233 if (ra->status != neRigidBody_::NE_RBSTATUS_IDLE ||
1234 rb->status != neRigidBody_::NE_RBSTATUS_IDLE)
1235
1236 doCollision = true;
1237 }
1238 }
1239 else
1240 {
1241 if (ra->status != neRigidBody_::NE_RBSTATUS_IDLE ||
1242 rb->status != neRigidBody_::NE_RBSTATUS_IDLE ||
1243 ra->isShifted ||
1244 rb->isShifted)
1245 {
1246 doCollision = true;
1247 }
1248 }
1249 if (doCollision)
1250 {
1251 if ((ra->isCustomCD || rb->isCustomCD))
1252 {
1253 if (customCDRB2RBCallback)
1254 {
1255 isCustomeCD = true;
1256
1257 neCustomCDInfo cdInfo;
1258
1259 memset(&cdInfo, 0, sizeof(cdInfo));
1260
1261 if (customCDRB2RBCallback((neRigidBody*)ra, (neRigidBody*)rb, cdInfo))
1262 {
1263 result.penetrate = true;
1264 result.bodyA = ra;
1265 result.bodyB = rb;
1266 result.collisionFrame[2] = cdInfo.collisionNormal;
1267 result.materialIdA = cdInfo.materialIdA;
1268 result.materialIdB = cdInfo.materialIdB;
1269 result.contactA = cdInfo.worldContactPointA - ra->GetB2W().pos;
1270 result.contactB = cdInfo.worldContactPointB - rb->GetB2W().pos;
1271 result.contactAWorld = cdInfo.worldContactPointA;
1272 result.contactBWorld = cdInfo.worldContactPointB;
1273 result.contactABody = ra->GetB2W().rot.TransposeMulV3(result.contactA);
1274 result.contactBBody = rb->GetB2W().rot.TransposeMulV3(result.contactB);
1275 result.depth = cdInfo.penetrationDepth;
1276 ChooseAxis(result.collisionFrame[0], result.collisionFrame[1], result.collisionFrame[2]);
1277 }
1278 }
1279 }
1280 else
1281 {
1282 backupVector = rb->backupVector - ra->backupVector;
1283
1284 CollisionTest(result, ra->col, ra->State().b2w,
1285 rb->col, rb->State().b2w, backupVector);
1286 if (ra->sensors)
1287 {
1288 CollisionTestSensor(&ra->col.obb,
1289 ra->sensors,
1290 ra->State().b2w,
1291 rb->col,
1292 rb->State().b2w,
1293 rb);
1294 }
1295 if (rb->sensors)
1296 {
1297 CollisionTestSensor(&rb->col.obb,
1298 rb->sensors,
1299 rb->State().b2w,
1300 ra->col,
1301 ra->State().b2w,
1302 ra);
1303 }
1304 }
1305 }
1306 }
1307 }
1308 // if (perfReport)
1309 // perf.UpdateCD();
1310
1311 if (result.penetrate)
1312 {
1313 neBool bothAnimated = false;
1314
1315 if (ra && ra->status == neRigidBody_::NE_RBSTATUS_ANIMATED &&
1316 rb && rb->status == neRigidBody_::NE_RBSTATUS_ANIMATED)
1317 {
1318 bothAnimated = true;
1319 }
1320 neBool response = true;
1321
1322 if (!result.collisionFrame[2].IsFinite() || result.collisionFrame[2].IsConsiderZero())
1323 {
1324 response = false;
1325 }
1326
1327 result.impulseType = IMPULSE_NORMAL;
1328
1329 if ((collisionflag & neCollisionTable::RESPONSE_IMPULSE) &&
1330 response &&
1331 (!bothAnimated))
1332 {
1333 result.bodyA = bodyA;
1334 result.bodyB = bodyB;
1335 RegisterPenetration(bodyA, bodyB, result);
1336 }
1337 if ((collisionflag & neCollisionTable::RESPONSE_CALLBACK) && collisionCallback && !isCustomeCD)
1338 {
1339 static neCollisionInfo cinfo;
1340
1341 cinfo.bodyA = (neByte *)bodyA;
1342 cinfo.bodyB = (neByte *)bodyB;
1343 cinfo.typeA = bodyA->btype == NE_OBJECT_COLISION? NE_ANIMATED_BODY : NE_RIGID_BODY;
1344 cinfo.typeB = bodyB->btype == NE_OBJECT_COLISION? NE_ANIMATED_BODY : NE_RIGID_BODY;
1345 cinfo.materialIdA = result.materialIdA;
1346 cinfo.materialIdB = result.materialIdB;
1347 cinfo.geometryA = (neGeometry*)result.convexA;
1348 cinfo.geometryB = (neGeometry*)result.convexB;
1349 cinfo.bodyContactPointA = result.contactABody;
1350 cinfo.bodyContactPointB = result.contactBBody;
1351 cinfo.worldContactPointA = result.contactAWorld;
1352 cinfo.worldContactPointB = result.contactBWorld;
1353 cinfo.relativeVelocity = result.initRelVelWorld;
1354 cinfo.collisionNormal = result.collisionFrame[2];
1355
1356 collisionCallback(cinfo);
1357 }
1358 }
1359 }
1360 }
1361 //OutputDebugString("terrain test\n");/////////////////////////////////
1362
CheckTerrainCollision()1363 void neFixedTimeStepSimulator::CheckTerrainCollision()
1364 {
1365 neCollisionResult result;
1366
1367 neTreeNode & rootNode = region.GetTriangleTree().GetRoot();
1368
1369 neT3 identity;
1370
1371 identity.SetIdentity();
1372
1373 neCollision & triCol = fakeCollisionBody.col;
1374
1375 triCol.convexCount = 1;
1376
1377 triCol.obb.SetTransform(identity);
1378
1379 neV3 backupVector;
1380
1381 for (s32 mop = 0; mop < 2; mop++)
1382 {
1383 neList<neRigidBody_> * activeList = &activeRB;
1384
1385 if (mop == 1)
1386 {
1387 activeList = &activeRP;
1388
1389 }
1390 neRigidBody_ * rb = activeList->GetHead();
1391
1392 //for (riter = rbHeap.BeginUsed(); riter.Valid(); riter++)
1393 while (rb)
1394 {
1395 neRigidBody_ * bodyA = (rb);
1396
1397 if (bodyA->status == neRigidBody_::NE_RBSTATUS_IDLE &&
1398 !bodyA->isShifted)
1399 {
1400 rb = activeList->GetNext(rb);
1401
1402 continue;
1403 }
1404 backupVector = -rb->backupVector;
1405
1406 treeNodes.Clear();
1407
1408 triangleIndex.Clear();
1409
1410 if (!terrainQueryCallback)
1411 {
1412 rootNode.GetCandidateNodes(treeNodes, bodyA->minBound, bodyA->maxBound, 0);
1413
1414 if (treeNodes.GetUsedCount() == 0)
1415 {
1416 rb = activeList->GetNext(rb);
1417
1418 continue;
1419 }
1420
1421 //printf("node count %d\n", treeNodes.GetUsedCount());
1422
1423 for (s32 i = 0; i < treeNodes.GetUsedCount(); i++)
1424 {
1425 neTreeNode * t = treeNodes[i];
1426
1427 for (s32 j = 0; j < t->triangleIndices.GetUsedCount(); j++)
1428 {
1429 s32 k;
1430
1431 for (k = 0; k < triangleIndex.GetUsedCount(); k++)
1432 {
1433 if (t->triangleIndices[j] == triangleIndex[k])
1434 break;
1435 }
1436 if (k == triangleIndex.GetUsedCount())
1437 {
1438 s32 * triIndex = triangleIndex.Alloc();
1439
1440 //ASSERT(triIndex);
1441
1442 *triIndex = t->triangleIndices[j];
1443 }
1444 }
1445 }
1446
1447 #ifdef _WIN32
1448 if (perfReport)
1449 perf->UpdateTerrainCulling();
1450 #endif
1451 triCol.obb.SetTerrain(triangleIndex, region.terrainTree.triangles, region.terrainTree.vertices);
1452
1453 triCol.convex = &triCol.obb;
1454
1455 CollisionTest(result, bodyA->col, ((neRigidBody_*)bodyA)->State().b2w,
1456 triCol, identity, backupVector);
1457
1458 if (bodyA->sensors)
1459 {
1460 CollisionTestSensor(&bodyA->col.obb,
1461 bodyA->sensors,
1462 bodyA->State().b2w,
1463 triCol,
1464 identity,
1465 NULL);
1466 }
1467
1468 }
1469 else
1470 {
1471 neTriangle * tris;
1472 s32 triCount;
1473 s32 * candidates;
1474 s32 candidateCount;
1475 neV3 *verts;
1476
1477 static neSimpleArray<s32> _candArray;
1478 static neArray<neTriangle_> _triArray;
1479
1480 terrainQueryCallback(bodyA->minBound, bodyA->maxBound, &candidates, &tris, &verts, &candidateCount, &triCount, (neRigidBody*)bodyA);
1481
1482 #ifdef _WIN32
1483 if (perfReport)
1484 perf->UpdateTerrainCulling();
1485 #endif
1486
1487 _candArray.MakeFromPointer(candidates, candidateCount);
1488
1489 _triArray.MakeFromPointer((neTriangle_*)tris, triCount);
1490
1491 triCol.obb.SetTerrain(_candArray, _triArray, verts);
1492
1493 triCol.convex = &triCol.obb;
1494
1495 CollisionTest(result, bodyA->col, ((neRigidBody_*)bodyA)->State().b2w,
1496 triCol, identity, backupVector);
1497
1498 if (bodyA->sensors)
1499 {
1500 CollisionTestSensor(&bodyA->col.obb,
1501 bodyA->sensors,
1502 bodyA->State().b2w,
1503 triCol,
1504 identity,
1505 NULL);
1506 }
1507 _candArray.MakeFromPointer(NULL, 0);
1508 _triArray.MakeFromPointer(NULL, 0);
1509 }
1510 if (result.penetrate)
1511 {
1512 result.impulseType = IMPULSE_NORMAL;
1513
1514 neCollisionTable::neReponseBitFlag collisionflag = colTable.Get(bodyA->cid, -1); //-1 is terrain
1515
1516 if ((collisionflag & neCollisionTable::RESPONSE_IMPULSE) &&
1517 bodyA->status != neRigidBody_::NE_RBSTATUS_ANIMATED)
1518 {
1519 result.bodyA = bodyA;
1520 result.bodyB = &fakeCollisionBody;
1521
1522 RegisterPenetration(bodyA, &fakeCollisionBody, result);
1523 }
1524 if ((collisionflag & neCollisionTable::RESPONSE_CALLBACK) && collisionCallback)
1525 {
1526 static neCollisionInfo cinfo;
1527
1528 cinfo.bodyA = (neByte *)bodyA;
1529 cinfo.bodyB = (neByte *)result.convexB;
1530 cinfo.typeA = NE_RIGID_BODY;
1531 cinfo.typeB = NE_TERRAIN;
1532 cinfo.materialIdA = result.materialIdA;
1533 cinfo.materialIdB = result.materialIdB;
1534 cinfo.geometryA = (neGeometry*)result.convexA;
1535 cinfo.geometryB = NULL;
1536 cinfo.bodyContactPointA = result.contactABody;
1537 cinfo.bodyContactPointB = result.contactBBody;
1538 cinfo.worldContactPointA = result.contactAWorld;
1539 cinfo.worldContactPointB = result.contactBWorld;
1540 cinfo.relativeVelocity = result.initRelVelWorld;
1541 cinfo.collisionNormal = result.collisionFrame[2];
1542
1543 collisionCallback(cinfo);
1544 }
1545 }
1546
1547 rb = activeList->GetNext(rb);
1548 }
1549 }//mop
1550
1551 for (s32 mop2 = 0; mop2 < 2; mop2++)
1552 {
1553 neList<neRigidBody_> * activeList = &activeRB;
1554
1555 if (mop2 == 1)
1556 {
1557 activeList = &activeRP;
1558
1559 }
1560 neRigidBody_ * rb = activeList->GetHead();
1561
1562 while (rb)
1563 {
1564 if (rb->isShifted2)
1565 {
1566 rb->isShifted = true;
1567 rb->isShifted2 = false;
1568 }
1569 else
1570 {
1571 rb->isShifted = false;
1572 rb->isShifted2 = false;
1573 }
1574 rb = activeList->GetNext(rb);
1575 }
1576 }
1577 }
1578 /****************************************************************************
1579 *
1580 * neFixedTimeStepSimulator::SolveConstrain
1581 *
1582 ****************************************************************************/
1583 /*
1584 void RecalcRelative(neCollisionResult * cresult)
1585 {
1586 cresult->initRelVelWorld = cresult->bodyA->VelocityAtPoint(cresult->contactA) * -1.0f;
1587
1588 if (cresult->bodyB)
1589 cresult->initRelVelWorld += cresult->bodyB->VelocityAtPoint(cresult->contactB);
1590
1591 cresult->relativeSpeed = cresult->initRelVelWorld.Length();
1592
1593 if (!cresult->penetrate)
1594 {
1595 cresult->collisionFrame[2] = cresult->initRelVelWorld * (1.0f / cresult->relativeSpeed);
1596 }
1597 else
1598 {
1599 cresult->relativeSpeed = cresult->initRelVelWorld.Dot(cresult->collisionFrame[2]);
1600
1601 if (cresult->relativeSpeed < 0.0f)
1602 cresult->relativeSpeed = 0.0f;
1603 }
1604 }
1605 */
1606 /*
1607 void neFixedTimeStepSimulator::SolveConstrain()
1608 {
1609 neDLinkList<neConstraintHeader>::iterator chiter;
1610
1611 for (chiter = constraintHeaders.BeginUsed(); chiter.Valid(); chiter++)
1612 {
1613 //check if the whole chain is idle
1614 neConstraintHeader * header = *chiter;
1615
1616 neBodyHandle * bodyHandle = header->bodies.GetHead();
1617
1618 neBool allIdle = true;
1619
1620 while (bodyHandle)
1621 {
1622 neRigidBody_ * rb = bodyHandle->thing->AsRigidBody();
1623
1624 if (rb && rb->status != neRigidBody_::NE_RBSTATUS_IDLE)
1625 {
1626 allIdle = false;
1627
1628 break;
1629 }
1630 bodyHandle = bodyHandle->next;
1631 }
1632
1633 if (allIdle)
1634 continue;
1635
1636 _neConstraint * constraint = header->head;
1637
1638 constraint = (*chiter)->head;
1639
1640 while (constraint)
1641 {
1642 if (constraint->limitStates[0].enableLimit ||
1643 constraint->limitStates[1].enableLimit)
1644 {
1645 constraint->CheckLimit();
1646 }
1647 neFreeListItem<_neConstraint> * item = (neFreeListItem<_neConstraint> *)constraint;
1648
1649 constraint = (_neConstraint*)(item->next);
1650 }
1651 }
1652 }
1653 */
1654
1655 /****************************************************************************
1656 *
1657 * neFixedTimeStepSimulator::RegisterPenetration
1658 *
1659 ****************************************************************************/
1660
RegisterPenetration(neRigidBodyBase * bodyA,neRigidBodyBase * bodyB,neCollisionResult & cresult)1661 void neFixedTimeStepSimulator::RegisterPenetration(neRigidBodyBase * bodyA, neRigidBodyBase * bodyB, neCollisionResult & cresult)
1662 {
1663 neRigidBody_ * ba = bodyA->AsRigidBody();
1664
1665 neRigidBody_ * bb = bodyB->AsRigidBody();
1666
1667 neRestRecord rc;
1668
1669 neBool isConnected = false;
1670
1671 if (ba)
1672 {
1673 isConnected = ba->IsConstraintNeighbour(bodyB);
1674 }
1675 else
1676 {
1677 if (bb)
1678 {
1679 isConnected = bb->IsConstraintNeighbour(bodyA);
1680 }
1681 }
1682 if (isConnected && !(bodyA->isCollideDirectlyConnected && bodyB->isCollideDirectlyConnected))
1683 {
1684 //HandleCollision(bodyA, bodyB, cresult, IMPULSE_NORMAL, 1.0f);
1685
1686 return;
1687 }
1688 if (ba && bb)
1689 {
1690 if (bb->IsParticle() )
1691 {
1692 //ASSERT(!ba->IsParticle());
1693
1694 CollisionRigidParticle(ba, bb, cresult);
1695
1696 return;
1697 }
1698 }
1699
1700 rc.depth = cresult.depth;
1701
1702 f32 alignWithGravity = cresult.collisionFrame[2].Dot(gravityVector);
1703
1704 //f32 angle = 0.3f;
1705 f32 angle = 0.3f;
1706
1707 if (1)//neAbs(alignWithGravity) > angle)
1708 {
1709 neV3 velA = bodyA->VelocityAtPoint(cresult.contactA);
1710
1711 neV3 velB = bodyB->VelocityAtPoint(cresult.contactB);
1712
1713 cresult.initRelVelWorld = velA - velB;
1714
1715 if (alignWithGravity < 0.0f) //ba on top
1716 {
1717 if (ba)
1718 {
1719 cresult.PrepareForSolver();
1720
1721 HandleCollision(bodyA, bodyB, cresult, IMPULSE_NORMAL, 1.0f);
1722
1723 neV3 normalBody;
1724
1725 if (bodyB)
1726 normalBody = bodyB->GetB2W().rot.TransposeMulV3(cresult.collisionFrame[2]);
1727 else
1728 normalBody = cresult.collisionFrame[2];
1729
1730 rc.SetTmp(bodyB, cresult.contactABody, cresult.contactBBody, normalBody, cresult.materialIdA, cresult.materialIdB);
1731
1732 ba->AddStackInfo(rc);
1733 }
1734 else if (bb)
1735 {
1736 SimpleShift(cresult);
1737
1738 cresult.PrepareForSolver();
1739
1740 HandleCollision(bodyA, bodyB, cresult, IMPULSE_NORMAL, 1.0f);
1741 }
1742 }
1743 else
1744 {
1745 if (bb)
1746 {
1747 cresult.PrepareForSolver();
1748
1749 HandleCollision(bodyA, bodyB, cresult, IMPULSE_NORMAL, 1.0f);
1750
1751 //if (cresult.relativeSpeed < restingSpeed)
1752 {
1753 neV3 normalBody;
1754
1755 if (bodyA)
1756 normalBody = bodyA->GetB2W().rot.TransposeMulV3(cresult.collisionFrame[2] * -1.0f);
1757 else
1758 normalBody = cresult.collisionFrame[2] * -1.0f;
1759
1760 rc.SetTmp(bodyA, cresult.contactBBody, cresult.contactABody, normalBody, cresult.materialIdB, cresult.materialIdA);
1761
1762 bb->AddStackInfo(rc);
1763 }
1764 }
1765 else if (ba)
1766 {
1767 SimpleShift(cresult);
1768
1769 cresult.PrepareForSolver();
1770
1771 HandleCollision(bodyA, bodyB, cresult, IMPULSE_NORMAL, 1.0f);
1772 }
1773 }
1774 }
1775 else // not resting collision, resolve now
1776 {
1777 if (ba && bb)
1778 {
1779 if (!ba->isShifted && ba->status == neRigidBody_::NE_RBSTATUS_IDLE)
1780 {
1781 f32 e = ba->Derive().linearVel.Dot(ba->Derive().linearVel);
1782
1783 e += ba->Derive().angularVel.Dot(ba->Derive().angularVel);
1784
1785 if (e > highEnergy)
1786 {
1787 SimpleShift(cresult);
1788 }
1789 else
1790 {
1791 bb->SetPos( bb->GetPos() - (cresult.collisionFrame[2] * cresult.depth));
1792
1793 bb->isShifted2 = true;
1794 }
1795 }
1796 else if (!bb->isShifted && bb->status == neRigidBody_::NE_RBSTATUS_IDLE)
1797 {
1798 f32 e = bb->Derive().linearVel.Dot(bb->Derive().linearVel);
1799
1800 e += bb->Derive().angularVel.Dot(bb->Derive().angularVel);
1801
1802 if (e > highEnergy)
1803 {
1804 SimpleShift(cresult);
1805 }
1806 else
1807 {
1808 ba->SetPos( ba->GetPos() + (cresult.collisionFrame[2] * cresult.depth));
1809
1810 ba->isShifted2 = true;
1811 }
1812 }
1813 else
1814 {
1815 SimpleShift(cresult);
1816 }
1817 }
1818 else
1819 {
1820 SimpleShift(cresult);
1821 }
1822 cresult.PrepareForSolver();
1823
1824 HandleCollision(bodyA, bodyB, cresult, IMPULSE_NORMAL, 1.0f);
1825 }
1826 }
1827
CollisionRigidParticle(neRigidBody_ * ba,neRigidBody_ * bb,neCollisionResult & cresult)1828 void neFixedTimeStepSimulator::CollisionRigidParticle(neRigidBody_ * ba, neRigidBody_ * bb, neCollisionResult & cresult)
1829 {
1830 cresult.PrepareForSolver();
1831
1832 HandleCollision(ba, bb, cresult, IMPULSE_NORMAL, 1.0f);
1833
1834 neV3 shift;
1835
1836 shift = cresult.collisionFrame[2] * cresult.depth;
1837
1838 bb->SetPos( bb->GetPos() - shift);
1839 }
1840
SimpleShift(const neCollisionResult & cresult)1841 void neFixedTimeStepSimulator::SimpleShift(const neCollisionResult & cresult)
1842 {
1843 neV3 shift; shift = cresult.collisionFrame[2] * cresult.depth;
1844
1845 f32 aratio, bratio;
1846
1847 neRigidBodyBase * bodyA = cresult.bodyA;
1848
1849 neRigidBodyBase * bodyB = cresult.bodyB;
1850
1851 neRigidBody_ * ba = bodyA->AsRigidBody();
1852
1853 neRigidBody_ * bb = bodyB->AsRigidBody();
1854
1855 if (!ba)
1856 {
1857 aratio = 0.0f;
1858 bratio = 1.0f;
1859
1860 if (bb)
1861 bb->isShifted2 = true;
1862 }
1863 else if (!bb)
1864 {
1865 aratio = 1.0f;
1866 bratio = 0.0f;
1867
1868 if (ba)
1869 ba->isShifted2 = true;
1870 }
1871 else
1872 {
1873 ba->isShifted2 = true;
1874 bb->isShifted2 = true;
1875
1876 f32 totalMass = ba->mass + bb->mass;
1877
1878 aratio = bb->mass / totalMass;
1879
1880 bratio = ba->mass / totalMass;
1881 }
1882
1883 if (ba)
1884 {
1885 ba->SetPos( ba->GetPos() + (shift * aratio) * 1.0f);
1886 }
1887 if (bb)
1888 {
1889 bb->SetPos( bb->GetPos() - (shift * bratio) * 1.0f);
1890 }
1891 }
1892
1893
CheckBreakage(neRigidBodyBase * originalBody,TConvex * convex,const neV3 & contactPoint,neV3 & impulse)1894 neBool neFixedTimeStepSimulator::CheckBreakage(neRigidBodyBase * originalBody, TConvex * convex, const neV3 & contactPoint, neV3 & impulse)
1895 {
1896 f32 impulseMag;
1897
1898 neV3 breakPlane;
1899
1900 neM3 rot = originalBody->GetB2W().rot * convex->c2p.rot;
1901
1902 breakPlane = rot * convex->breakInfo.breakPlane;
1903
1904 neV3 breakImpulse = impulse;
1905
1906 breakImpulse.RemoveComponent(breakPlane);
1907
1908 impulseMag = breakImpulse.Length();
1909
1910 if (impulseMag < convex->breakInfo.breakMagnitude)
1911 {
1912 return false;
1913 }
1914
1915 f32 dot = impulse.Dot(breakPlane);
1916
1917 impulse = breakPlane * dot;
1918
1919 breakImpulse *= convex->breakInfo.breakMagnitude / impulseMag;
1920
1921 neRigidBody_* newBody = NULL;
1922
1923 neV3 newImpulse, newContactPoint;
1924
1925 newImpulse = breakImpulse * convex->breakInfo.breakAbsorb;
1926
1927 breakImpulse *= (1.0f - convex->breakInfo.breakAbsorb);
1928
1929 impulse += breakImpulse;
1930
1931 neBodyType originalBodyType;
1932
1933 if (originalBody->AsRigidBody())
1934 {
1935 originalBodyType = NE_RIGID_BODY;
1936 }
1937 else
1938 {
1939 originalBodyType = NE_ANIMATED_BODY;
1940 }
1941
1942 switch (convex->breakInfo.flag)
1943 {
1944 case neGeometry::NE_BREAK_NORMAL:
1945 case neGeometry::NE_BREAK_NORMAL_PARTICLE:
1946
1947 newBody = CreateRigidBodyFromConvex(convex, originalBody);
1948
1949 newContactPoint = contactPoint - newBody->GetPos();
1950
1951 newBody->ApplyCollisionImpulse(newImpulse, newContactPoint, IMPULSE_NORMAL);
1952
1953 break;
1954 case neGeometry::NE_BREAK_ALL:
1955 case neGeometry::NE_BREAK_ALL_PARTICLE:
1956
1957 break;
1958 case neGeometry::NE_BREAK_NEIGHBOUR:
1959 case neGeometry::NE_BREAK_NEIGHBOUR_PARTICLE:
1960
1961 break;
1962 }
1963 if (originalBodyType == NE_ANIMATED_BODY)
1964 {
1965 impulse = newImpulse;
1966 }
1967 if (newBody)
1968 {
1969 breakageCallback((neByte *)originalBody, originalBodyType, (neGeometry *)convex, (neRigidBody*)newBody);
1970
1971 return true;
1972 }
1973 else
1974 {
1975 return false;
1976 }
1977 }
1978
1979
ResetStackHeaderFlag()1980 void neFixedTimeStepSimulator::ResetStackHeaderFlag()
1981 {
1982 neStackHeaderItem * hitem = (neStackHeaderItem *)(*stackHeaderHeap.BeginUsed());
1983
1984 while (hitem)
1985 {
1986 neStackHeader * sheader = (neStackHeader *) hitem;
1987
1988 sheader->dynamicSolved = false;
1989
1990 hitem = hitem->next;
1991 }
1992 }
1993
1994 /****************************************************************************
1995 *
1996 * neFixedTimeStepSimulator::SetTerrainMesh
1997 *
1998 ****************************************************************************/
1999
SetTerrainMesh(neTriangleMesh * tris)2000 void neFixedTimeStepSimulator::SetTerrainMesh(neTriangleMesh * tris)
2001 {
2002 region.MakeTerrain(tris);
2003 }
2004
FreeTerrainMesh()2005 void neFixedTimeStepSimulator::FreeTerrainMesh()
2006 {
2007 region.FreeTerrain();
2008 }
2009
NewStackHeader(neStackInfo * sinfo)2010 neStackHeader * neFixedTimeStepSimulator::NewStackHeader(neStackInfo * sinfo)
2011 {
2012 neStackHeader * n = stackHeaderHeap.Alloc();
2013
2014 //ASSERT(n);
2015
2016 n->Null();
2017
2018 n->sim = this;
2019
2020 if (sinfo)
2021 n->Add(sinfo);
2022
2023 return n;
2024 }
2025
NewConstraintHeader()2026 neConstraintHeader * neFixedTimeStepSimulator::NewConstraintHeader()
2027 {
2028 neConstraintHeader * ret = constraintHeaders.Alloc();
2029
2030 ret->Reset();
2031
2032 return ret;
2033 }
2034
CheckStackHeader()2035 void neFixedTimeStepSimulator::CheckStackHeader()
2036 {
2037 neStackHeaderItem * item = (neStackHeaderItem *)(*stackHeaderHeap.BeginUsed());
2038
2039 while (item)
2040 {
2041 neStackHeader * h = (neStackHeader *)item;
2042
2043 //ASSERT(h->infoCount > 0);
2044 //assert(h->infoCount > 0);
2045 item = item->next;
2046 }
2047 }
2048
UpdateConstraintControllers()2049 void neFixedTimeStepSimulator::UpdateConstraintControllers()
2050 {
2051 neFreeListItem<neConstraintHeader> * hitem = (neFreeListItem<neConstraintHeader> *)(*constraintHeaders.BeginUsed());
2052
2053 while (hitem)
2054 {
2055 neConstraintHeader * h = (neConstraintHeader *)hitem;
2056
2057 hitem = hitem->next;
2058
2059 neFreeListItem<_neConstraint> * citem = (neFreeListItem<_neConstraint> *) h->head;
2060
2061 while (citem)
2062 {
2063 _neConstraint * c = (_neConstraint *)citem;
2064
2065 citem = citem->next;
2066
2067 if (c->enable)
2068 c->UpdateController();
2069 }
2070 }
2071 }
2072
FreeAllBodies()2073 void neFixedTimeStepSimulator::FreeAllBodies()
2074 {
2075 neRigidBody_ * rb = activeRB.GetHead();
2076
2077 while (rb)
2078 {
2079 rb->Free();
2080
2081 neRigidBody_ * rbNext = activeRB.GetNext(rb);
2082
2083 activeRB.Remove(rb);
2084
2085 rigidBodyHeap.Dealloc(rb, 1);
2086
2087 rb = rbNext;
2088 }
2089 rb = inactiveRB.GetHead();
2090
2091 while (rb)
2092 {
2093 rb->Free();
2094
2095 neRigidBody_ * rbNext = inactiveRB.GetNext(rb);
2096
2097 inactiveRB.Remove(rb);
2098
2099 rigidBodyHeap.Dealloc(rb, 1);
2100
2101 rb = rbNext;
2102 }
2103
2104 //ASSERT(activeRB.count == 0);
2105
2106 //ASSERT(inactiveRB.count == 0);
2107
2108 activeRB.Reset();
2109
2110 inactiveRB.Reset();
2111
2112 ///////////////////////////////////////////////////////////
2113
2114 neRigidBody_ * rp = activeRP.GetHead();
2115
2116 while (rp)
2117 {
2118 rp->Free();
2119
2120 neRigidBody_ * rpNext = activeRP.GetNext(rp);
2121
2122 activeRP.Remove(rp);
2123
2124 rigidParticleHeap.Dealloc(rp, 1);
2125
2126 rp = rpNext;
2127 }
2128 rp = inactiveRP.GetHead();
2129
2130 while (rp)
2131 {
2132 rp->Free();
2133
2134 neRigidBody_ * rpNext = inactiveRP.GetNext(rp);
2135
2136 inactiveRP.Remove(rp);
2137
2138 rigidParticleHeap.Dealloc(rp, 1);
2139
2140 rp = rpNext;
2141 }
2142
2143 //ASSERT(activeRP.count == 0);
2144
2145 //ASSERT(inactiveRP.count == 0);
2146
2147 activeRP.Reset();
2148
2149 inactiveRP.Reset();
2150
2151 ///////////////////////////////////////////////////////////
2152
2153 neCollisionBody_ * cb = activeCB.GetHead();
2154
2155 while (cb)
2156 {
2157 cb->Free();
2158
2159 neCollisionBody_ * cbNext = activeCB.GetNext(cb);
2160
2161 activeCB.Remove(cb);
2162
2163 collisionBodyHeap.Dealloc(cb, 1);
2164
2165 cb = cbNext;
2166 }
2167
2168 cb = inactiveCB.GetHead();
2169
2170 while (cb)
2171 {
2172 cb->Free();
2173
2174 neCollisionBody_ * cbNext = inactiveCB.GetNext(cb);
2175
2176 inactiveCB.Remove(cb);
2177
2178 collisionBodyHeap.Dealloc(cb, 1);
2179
2180 cb = cbNext;
2181 }
2182
2183 //ASSERT(activeCB.count == 0);
2184
2185 //ASSERT(inactiveCB.count == 0);
2186
2187 activeCB.Reset();
2188
2189 inactiveCB.Reset();
2190
2191 //ASSERT(rigidBodyHeap.GetUsedCount() == 0);
2192
2193 //ASSERT(collisionBodyHeap.GetUsedCount() == 0);
2194
2195 //ASSERT(constraintHeap.GetUsedCount() == 0);
2196
2197 //ASSERT(geometryHeap.GetUsedCount() == 0);
2198
2199 //ASSERT(controllerHeap.GetUsedCount() == 0);
2200
2201 // ASSERT(miniConstraintHeap.GetUsedCount() == 0);
2202
2203 //ASSERT(stackInfoHeap.GetUsedCount() == 0);
2204 }
2205
2206 /****************************************************************************
2207 *
2208 * neFixedTimeStepSimulator::GetMemoryUsage
2209 *
2210 ****************************************************************************/
2211
GetMemoryAllocated(s32 & memoryAllocated)2212 void neFixedTimeStepSimulator::GetMemoryAllocated(s32 & memoryAllocated)
2213 {
2214 memoryAllocated = 0;
2215
2216 memoryAllocated += rigidBodyHeap.Size() * sizeof(neFreeListItem<neRigidBody_>);
2217
2218 memoryAllocated += rigidParticleHeap.Size() * sizeof(neFreeListItem<neRigidBody_>);
2219
2220 memoryAllocated += collisionBodyHeap.Size() * sizeof(neFreeListItem<neCollisionBody_>);
2221
2222 memoryAllocated += treeNodes.GetTotalSize() * sizeof(neTreeNode *);
2223
2224 memoryAllocated += triangleIndex.GetTotalSize() * sizeof(s32);
2225
2226 memoryAllocated += constraintHeaders.Size() * sizeof(neFreeListItem<neConstraintHeader>);
2227
2228 memoryAllocated += constraintHeap.Size() * sizeof(neFreeListItem<_neConstraint>);
2229
2230 // memoryAllocated += miniConstraintHeap.Size() * sizeof(neFreeListItem<neMiniConstraint>);
2231
2232 memoryAllocated += controllerHeap.Size() * sizeof(neFreeListItem<neController>);
2233
2234 memoryAllocated += stackInfoHeap.Size() * sizeof(neFreeListItem<neStackInfo>);
2235
2236 memoryAllocated += stackHeaderHeap.Size() * sizeof(neFreeListItem<neStackHeader>);
2237
2238 memoryAllocated += sensorHeap.Size() * sizeof(neFreeListItem<neSensor_>);
2239
2240 memoryAllocated += geometryHeap.Size() * sizeof(neFreeListItem<TConvex>);
2241
2242 //memoryAllocated += cresultHeap.Size() * sizeof(neFreeListItem<neCollisionResult>);
2243 memoryAllocated += cresultHeap.GetTotalSize() * sizeof(neFreeListItem<neCollisionResult>);
2244
2245 memoryAllocated += pointerBuffer1.GetTotalSize() * sizeof(neByte *);
2246
2247 memoryAllocated += pointerBuffer2.GetTotalSize() * sizeof(neByte *);
2248
2249 //region stuff
2250 memoryAllocated += region.b2b.GetTotalSize() * sizeof(neOverlapped);
2251
2252 memoryAllocated += region.b2p.GetTotalSize() * sizeof(neOverlapped);
2253
2254 memoryAllocated += region.newBodies.GetTotalSize() * sizeof(neAddBodyInfo);
2255
2256 memoryAllocated += region.bodies.Size() * sizeof(neFreeListItem<neRigidBodyBase *>);
2257
2258 memoryAllocated += region.overlappedPairs.Size() * sizeof(neFreeListItem<neOverlappedPair>);
2259
2260 memoryAllocated += region.coordLists[0].coordList.Size() * sizeof(neFreeListItem<CCoordListEntry>);
2261
2262 memoryAllocated += region.coordLists[1].coordList.Size() * sizeof(neFreeListItem<CCoordListEntry>);
2263
2264 memoryAllocated += region.coordLists[2].coordList.Size() * sizeof(neFreeListItem<CCoordListEntry>);
2265
2266 memoryAllocated += region.terrainTree.nodes.GetTotalSize() * sizeof(neTreeNode);
2267
2268 memoryAllocated += region.terrainTree.triangles.GetTotalSize() * sizeof(neTriangle_);
2269 }
2270
2271 /****************************************************************************
2272 *
2273 * neCollisionTable_::neCollisionTable_
2274 *
2275 ****************************************************************************/
2276
neCollisionTable_()2277 neCollisionTable_::neCollisionTable_()
2278 {
2279 for (s32 i = 0 ; i < NE_COLLISION_TABLE_MAX; i++)
2280 {
2281 for (s32 j = 0 ; j < NE_COLLISION_TABLE_MAX; j++)
2282 table[i][j] = table[j][i] = neCollisionTable::RESPONSE_IMPULSE;
2283
2284 terrainTable[i] = neCollisionTable::RESPONSE_IMPULSE;
2285 }
2286 }
2287
2288 /****************************************************************************
2289 *
2290 * neCollisionTable_::~neCollisionTable_
2291 *
2292 ****************************************************************************/
2293
~neCollisionTable_()2294 neCollisionTable_::~neCollisionTable_()
2295 {
2296 }
2297
2298 /****************************************************************************
2299 *
2300 * neCollisionTable_::Set
2301 *
2302 ****************************************************************************/
2303
Set(s32 collisionID1,s32 collisionID2,neCollisionTable::neReponseBitFlag value)2304 void neCollisionTable_::Set(s32 collisionID1, s32 collisionID2, neCollisionTable::neReponseBitFlag value)
2305 {
2306 //ASSERT(collisionID1 >= -1 && collisionID1 < neCollisionTable::NE_COLLISION_TABLE_MAX);
2307 //ASSERT(collisionID2 >= -1 && collisionID2 < neCollisionTable::NE_COLLISION_TABLE_MAX);
2308
2309 if (collisionID1 == -1 && collisionID2 == -1)
2310 {
2311 return;
2312 }
2313
2314 if (collisionID1 == -1)
2315 {
2316 terrainTable[collisionID2] = value;
2317 }
2318 else if (collisionID2 == -1)
2319 {
2320 terrainTable[collisionID1] = value;
2321 }
2322 else
2323 {
2324 table[collisionID1][collisionID2] = value;
2325 table[collisionID2][collisionID1] = value;
2326 }
2327 }
2328
2329 /****************************************************************************
2330 *
2331 * neCollisionTable_::Get
2332 *
2333 ****************************************************************************/
2334
Get(s32 collisionID1,s32 collisionID2)2335 neCollisionTable::neReponseBitFlag neCollisionTable_::Get(s32 collisionID1, s32 collisionID2)
2336 {
2337 //ASSERT(collisionID1 >= -1 && collisionID1 < neCollisionTable::NE_COLLISION_TABLE_MAX);
2338 //ASSERT(collisionID2 >= -1 && collisionID2 < neCollisionTable::NE_COLLISION_TABLE_MAX);
2339
2340 if (collisionID1 == -1 && collisionID2 == -1)
2341 {
2342 return neCollisionTable::RESPONSE_IGNORE;
2343 }
2344
2345 if (collisionID1 == -1)
2346 {
2347 return terrainTable[collisionID2];
2348 }
2349 else if (collisionID2 == -1)
2350 {
2351 return terrainTable[collisionID1];
2352 }
2353
2354 return table[collisionID1][collisionID2];
2355 }
2356