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 #pragma inline_recursion( on )
15 #pragma inline_depth( 250 )
16
17 #include "stdio.h"
18
19 /*
20 #ifdef _WIN32
21 #include <windows.h>
22 #endif
23 */
24
25 #include "tokamak.h"
26 #include "containers.h"
27 #include "scenery.h"
28 #include "collision.h"
29 #include "constraint.h"
30 #include "rigidbody.h"
31 #include "scenery.h"
32 #include "stack.h"
33 #include "simulator.h"
34 #include "message.h"
35
36 #define ne_Default_Mass 1.0f
37
38 //extern void DrawLine(const neV3 & colour, neV3 * startpoint, s32 count);
39
40
41 /****************************************************************************
42 *
43 * neRigidBodyState::neRigidBodyState
44 *
45 ****************************************************************************/
46
neRigidBodyState()47 neRigidBodyState::neRigidBodyState()
48 {
49 q.Identity();
50
51 b2w.SetIdentity();
52
53 // b2w.pos.SetZero(); // x
54
55 angularMom.SetZero(); // L
56
57 // rot().SetIdentity();
58
59 linearMom.SetZero();
60 }
61
62 /****************************************************************************
63 *
64 * neRigidBody_::~neRigidBody_
65 *
66 ****************************************************************************/
67
~neRigidBody_()68 neRigidBody_::~neRigidBody_()
69 {
70
71 }
72
73 /****************************************************************************
74 *
75 * neRigidBody_::neRigidBody_
76 *
77 ****************************************************************************/
78
neRigidBody_()79 neRigidBody_::neRigidBody_()
80 {
81 btype = NE_OBJECT_RIGID;
82
83 rbExtra = &eggs;
84
85 curState = 0;
86
87 mass = ne_Default_Mass;
88
89 oneOnMass = 1.0f / ne_Default_Mass;
90
91 IbodyInv.SetZero();
92
93 IbodyInv.M[0][0] = oneOnMass;
94 IbodyInv.M[1][1] = oneOnMass;
95 IbodyInv.M[2][2] = oneOnMass;
96
97 Ibody.SetZero();
98
99 IbodyInv.M[0][0] = ne_Default_Mass;
100 IbodyInv.M[1][1] = ne_Default_Mass;
101 IbodyInv.M[2][2] = ne_Default_Mass;
102
103 cookies = 0;
104
105 force.SetZero();
106 torque.SetZero();
107
108 gravityOn = true;
109
110 status = NE_RBSTATUS_NORMAL;
111
112 SetConstraintHeader(NULL);
113
114 s32 i;
115
116 for (i = 0; i < NE_RB_MAX_RESTON_RECORDS; i++)
117 {
118 GetRestRecord(i).Init();
119 }
120 for (i = 0; i < NE_RB_MAX_PAST_RECORDS; i++)
121 {
122 GetVelRecord(i).SetZero();
123 GetAngVelRecord(i).SetZero();
124 dvRecord[i].SetZero();
125 davRecord[i].SetZero();
126 }
127
128 stackInfo = NULL;
129
130 lowEnergyCounter = 0;
131
132 GetRestHull().htype = neRestHull::NONE;
133
134 isShifted = isShifted2 = false;
135
136 controllers = NULL;
137
138 controllerCursor = NULL;
139
140 gforce.SetZero();
141
142 cforce.SetZero();
143
144 ctorque.SetZero();
145
146 subType = NE_RIGID_NORMAL;
147
148 acc.SetZero();
149
150 angularDamp = 0.0f;
151
152 linearDamp = 0.0f;
153
154 sleepingParam = 0.2f;
155
156 SyncOldState();
157 }
158
159 /****************************************************************************
160 *
161 * neRigidBody_::Free
162 *
163 ****************************************************************************/
164
Free()165 void neRigidBody_::Free()
166 {
167 neRigidBodyBase::Free();
168
169 //free controller
170
171 neFreeListItem<neController> * ci = (neFreeListItem<neController> *) controllers;
172
173 while (ci)
174 {
175 neFreeListItem<neController> * next = ci->next;
176
177 ci->Remove();
178
179 sim->controllerHeap.Dealloc((neController *)ci, 1);
180
181 ci = next;
182 }
183 controllers = NULL;
184
185 //free constraint header
186
187 RemoveConstraintHeader();
188
189 //free stack
190
191 if (stackInfo)
192 {
193 ASSERT(stackInfo->stackHeader);
194
195 stackInfo->stackHeader->Remove(stackInfo);
196
197 //sim->stackInfoHeap.Dealloc(stackInfo, 1);
198 FreeStackInfo();
199 }
200 for (s32 i = 0; i < NE_MAX_REST_ON; i++)
201 {
202 GetRestRecord(i).SetInvalid();
203 }
204 }
205
206 /****************************************************************************
207 *
208 * neRigidBody_::RecalcInertiaTensor
209 *
210 ****************************************************************************/
211
RecalcInertiaTensor()212 void neRigidBody_::RecalcInertiaTensor()
213 {
214 if (col.convexCount == 0)
215 return;
216
217 neM3 tensor;
218 neV3 cogShift;
219 f32 _mass = 0.0f;
220
221 f32 density = 0.0f;
222 f32 friction = 0.0f;
223 f32 restitution = 0.0f;
224
225 cogShift.SetZero();
226
227 tensor.SetZero();
228
229 if (col.convexCount == 1)
230 {
231 sim->GetMaterial(col.obb.GetMaterialId(), friction, restitution, density);
232
233 tensor = col.obb.CalcInertiaTensor(density, _mass);
234
235 IbodyInv.SetInvert(tensor);
236
237 mass = _mass;
238
239 cogShift = col.obb.c2p.pos * _mass;
240 }
241 else
242 {
243 s32 i;
244 f32 m;
245 neM3 _tensor;
246
247 for (i = 0; i < col.convexCount; i++)
248 {
249 sim->GetMaterial(col.convex[i].GetMaterialId() , friction, restitution, density);
250
251 _tensor = col.convex[i].CalcInertiaTensor(density, m);
252
253 tensor += _tensor;
254
255 _mass += m;
256
257 cogShift += col.convex[i].c2p.pos * _mass;
258 }
259
260 mass = _mass;
261 }
262 oneOnMass = 1.0f/mass;
263
264 cogShift = cogShift * (1.0f / mass);
265
266 TranslateCOM(tensor, cogShift, mass, -1.0f);
267
268 Ibody = tensor;
269
270 IbodyInv.SetInvert(tensor);
271 }
272
273 /****************************************************************************
274 *
275 * neRigidBody_::AddController
276 *
277 ****************************************************************************/
278
AddController(neRigidBodyControllerCallback * rbc,s32 period)279 neController * neRigidBody_::AddController(neRigidBodyControllerCallback * rbc, s32 period)
280 {
281 neController * c = sim->controllerHeap.Alloc(1);
282
283 if (!c)
284 {
285 sprintf(sim->logBuffer, MSG_CONTROLLER_FULL);
286
287 sim->LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
288
289 return NULL;
290 }
291 if (!controllers)
292 {
293 controllers = c;
294 }
295 else
296 {
297 //((neControllerItem *)controllers)->Append((neControllerItem *)c);
298
299 neControllerItem * citem = (neControllerItem *)controllers;
300
301 while (citem->next)
302 {
303 citem = citem->next;
304 }
305 citem->Append((neControllerItem *)c);
306 }
307 c->rb = this;
308
309 c->constraint = NULL;
310
311 c->rbc = rbc;
312
313 c->jc = NULL;
314
315 c->period = period;
316
317 c->count = period;
318
319 c->forceA.SetZero();
320
321 c->torqueA.SetZero();
322
323 c->forceB.SetZero();
324
325 c->torqueB.SetZero();
326
327 return c;
328 }
329
330 /****************************************************************************
331 *
332 * neRigidBody_::BeginIterateController
333 *
334 ****************************************************************************/
335
BeginIterateController()336 void neRigidBody_::BeginIterateController()
337 {
338 controllerCursor = (neControllerItem *)controllers;
339 }
340
341 /****************************************************************************
342 *
343 * neRigidBody_::GetNextController
344 *
345 ****************************************************************************/
346
GetNextController()347 neController * neRigidBody_::GetNextController()
348 {
349 if (!controllerCursor)
350 return NULL;
351
352 neController * ret = (neController *)controllerCursor;
353
354 controllerCursor = controllerCursor->next;
355
356 return ret;
357 }
358
359 /****************************************************************************
360 *
361 * neRigidBody_::GravityEnable
362 *
363 ****************************************************************************/
364
GravityEnable(neBool yes)365 void neRigidBody_::GravityEnable(neBool yes)
366 {
367 gravityOn = yes;
368 }
369
370 /****************************************************************************
371 *
372 * neRigidBody_::UpdateDerive
373 *
374 ****************************************************************************/
SetAngMom(const neV3 & am)375 void neRigidBody_::SetAngMom(const neV3 & am)
376 {
377 neQ w;
378
379 State().angularMom = am;
380
381 Derive().angularVel = Derive().Iinv * State().angularMom;
382
383 w.Set(Derive().angularVel, 0.0f);
384
385 Derive().qDot = w * State().q;
386
387 Derive().qDot *= 0.5f;
388 }
389
UpdateDerive()390 void neRigidBody_::UpdateDerive()
391 {
392 neRigidBodyState & state = State();
393
394 state.q = state.q.Normalize();
395
396 state.rot() = state.q.BuildMatrix3();
397
398 // Iinv = R * IbodyInv Rtrans
399 //derive.Iinv = state.rot() * IbodyInv * neM3_BuildTranspose(state.rot());
400 derive.Iinv = state.rot() * IbodyInv * neM3().SetTranspose(state.rot());
401
402 // w = Iinv * L
403 derive.angularVel = derive.Iinv * state.angularMom;
404
405 neQ w;
406 w.W = 0.0f;
407 w.X = derive.angularVel[0];
408 w.Y = derive.angularVel[1];
409 w.Z = derive.angularVel[2];
410
411 derive.qDot = w * state.q;
412
413 derive.qDot = derive.qDot * 0.5f;
414
415 derive.speed = derive.linearVel.Length();
416 }
417
418
419 /****************************************************************************
420 *
421 * neRigidBody_::Advance
422 *
423 ****************************************************************************/
424
AdvanceDynamic(f32 tStep)425 void neRigidBody_::AdvanceDynamic(f32 tStep)
426 {
427 oldCounter++;
428
429 totalDV.SetZero();
430
431 totalDA.SetZero();
432
433 impulseCount = 0;
434
435 twistCount = 0;
436
437 isAddedToSolver = false;
438
439 if (status == neRigidBody_::NE_RBSTATUS_IDLE)
440 {
441 if (!cforce.IsConsiderZero())
442 WakeUp();
443
444 if (!ctorque.IsConsiderZero())
445 WakeUp();
446 }
447
448 if (status == neRigidBody_::NE_RBSTATUS_IDLE)
449 {
450 if (CheckStillIdle())
451 {
452 return;
453 }
454 }
455
456 if (status == neRigidBody_::NE_RBSTATUS_ANIMATED)
457 {
458 return;
459 }
460
461 dvRecord[sim->stepSoFar % NE_RB_MAX_PAST_RECORDS].SetZero();
462 davRecord[sim->stepSoFar % NE_RB_MAX_PAST_RECORDS].SetZero();
463
464 neRigidBodyState & state = State();
465
466 int newStateId = (curState + 1) % MAX_RB_STATES;
467
468 neRigidBodyState & newState = stateBuffer[newStateId];
469
470 totalForce += (force + gforce + cforce);
471
472 dvRecord[sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] = totalForce * oneOnMass * tStep;
473
474 acc = totalForce * oneOnMass;
475
476 derive.linearVel += acc * tStep;
477
478 derive.linearVel *= (1.0f - linearDamp);
479
480 totalTorque += (torque + ctorque);
481
482 //L = L0 + torque * t
483
484 MidPointIntegration(totalTorque, tStep);
485 }
486
MidPointIntegration(const neV3 & totalTorque,f32 tStep)487 void neRigidBody_::MidPointIntegration(const neV3 & totalTorque, f32 tStep)
488 {
489 State().angularMom *= (1.0f - angularDamp);
490
491 neV3 newAngularMom = State().angularMom + totalTorque * tStep;
492
493 neQ tmpQ;
494
495 tmpQ = State().q;
496
497 f32 midStep = tStep * 0.5f;
498
499 tmpQ = State().q + Derive().qDot * midStep;
500
501 neV3 tmpL = State().angularMom + totalTorque * midStep;
502
503 tmpQ = tmpQ.Normalize();
504
505 neM3 rot = tmpQ.BuildMatrix3();
506
507 neM3 tmpIinv = rot * IbodyInv * neM3().SetTranspose(rot);
508
509 neV3 tmpAngVel = tmpIinv * tmpL;
510
511 neQ tmpW;
512
513 tmpW.Set(tmpAngVel, 0.0f);
514
515 Derive().qDot = tmpW * tmpQ * 0.5f;
516
517 curState = (curState + 1) % MAX_RB_STATES;
518
519 State().angularMom = newAngularMom;
520
521 Derive().angularVel = tmpAngVel;
522
523 //SetAngMom(am);
524 }
525
ImprovedEulerIntegration(const neV3 & totalTorque,f32 tStep)526 void neRigidBody_::ImprovedEulerIntegration(const neV3 & totalTorque, f32 tStep)
527 {
528 /* neV3 newAngularMom = State().angularMom + totalTorque * tStep;
529
530 neQ tmpQ = State().q + Derive().qDot * tStep;
531
532 tmpQ.Normalize();
533
534 neM3 rot = tmpQ.BuildMatrix3();
535
536 neV3 Iinv = rot * IbodyInv * neM3().SetTranspose(rot);
537
538 neV3 angVel = Iinv * newAngularMom;
539
540
541
542 curState = (curState + 1) % MAX_RB_STATES;
543
544 State().angularMom = newAngularMom;
545 */
546 }
547
RungeKutta4Integration(const neV3 & totalTorque,f32 tStep)548 void neRigidBody_::RungeKutta4Integration(const neV3 & totalTorque, f32 tStep)
549 {
550 /* neV3 newAngularMom = State().angularMom + totalTorque * tStep;
551
552 neQ q1, q2, q3, q4;
553
554 neV3 L1, L2, L3, L4;
555
556 f32 midStep = tStep * 0.5f;
557
558 q1 = State().q + Derive().qDot * tStep;
559
560 L1 =
561
562 curState = (curState + 1) % MAX_RB_STATES;
563
564 State().angularMom = newAngularMom;
565 */
566 }
567
AdvancePosition(f32 tStep)568 void neRigidBody_::AdvancePosition(f32 tStep)
569 {
570 // needSolveContactDynamic = true;
571
572 // totalForce.SetZero();
573
574 // totalTorque.SetZero();
575
576 // if (status == neRigidBody_::NE_RBSTATUS_IDLE)
577 // return;
578
579 //derive.linearVel *= 0.99;
580
581 neV3 newPos;
582
583 newPos = GetPos() + derive.linearVel * tStep + 0.5f * acc * tStep * tStep;
584
585 neV3 bv = newPos - GetPos();
586
587 backupVector = 0.7f * backupVector + 0.3f * bv;
588
589 SetPos(newPos);
590
591 // derive.linearVel += acc * tStep;
592 /*
593 neQ tmpQ;
594
595 tmpQ = State().q;
596
597 f32 midStep = tStep * 0.5f;
598
599 tmpQ = State().q + derive.qDot * midStep;
600
601 neV3 tmpL = State().angularMom + totalTorque * midStep;
602
603 tmpQ = tmpQ.Normalize();
604
605 neM3 rot = tmpQ.BuildMatrix3();
606
607 neM3 tmpIinv = rot * IbodyInv * neM3().SetTranspose(rot);
608
609 neV3 tmpAngVel = tmpIinv * tmpL;
610
611 neQ tmpW;
612
613 tmpW.Set(tmpAngVel, 0.0f);
614
615 neQ tmpQDot = tmpW * tmpQ * 0.5f;
616 */
617 //q = q0 + qdot * t
618
619 neV3 am = State().angularMom;// * 0.95f;
620
621 SetAngMom(am);
622
623 State().q = State().q + Derive().qDot * tStep;
624
625 UpdateDerive();
626
627 UpdateController();
628
629 if (sensors)
630 ClearSensor();
631
632 GetVelRecord(sim->stepSoFar % NE_RB_MAX_PAST_RECORDS) = Derive().linearVel;
633
634 GetAngVelRecord(sim->stepSoFar % NE_RB_MAX_PAST_RECORDS) = Derive().angularVel;
635
636 //CheckForIdle();
637 }
638
639 /****************************************************************************
640 *
641 * neRigidBody_::UpdateController
642 *
643 ****************************************************************************/
644
UpdateController()645 void neRigidBody_::UpdateController()
646 {
647 if (gravityOn && status != neRigidBody_::NE_RBSTATUS_IDLE)
648 {
649 gforce = sim->gravity * mass;
650 }
651 else
652 {
653 gforce.SetZero();
654 }
655
656 cforce.SetZero();
657
658 ctorque.SetZero();
659
660 neControllerItem * ci = (neControllerItem *)controllers;
661
662 while (ci)
663 {
664 neController * con = (neController *) ci;
665
666 ci = ci->next;
667
668 if (con->count == 0)
669 {
670 ASSERT(con->rbc);
671
672 con->rbc->RigidBodyControllerCallback((neRigidBodyController*)con, sim->_currentTimeStep);
673
674 con->count = con->period;
675 }
676 else
677 {
678 con->count--;
679 }
680 con->rb->cforce += con->forceA;
681
682 con->rb->ctorque += con->torqueA;
683 }
684 }
685
686 /****************************************************************************
687 *
688 * neRigidBody_::UpdateAABB
689 *
690 ****************************************************************************/
691
UpdateAABB()692 void neRigidBody_::UpdateAABB()
693 {
694 if (col.convexCount == 0 && !isCustomCD)
695 return;
696
697 // for (s32 i = 0; i < 3; i++)
698 {
699 #if 0
700 if (minCoord[0])
701 minCoord[0]->value = p[0] /*- neAbs(c2w[0][0]) - neAbs(c2w[0][1]) - neAbs(c2w[0][2])*/ - col.boundingRadius;
702 if (maxCoord[0])
703 maxCoord[0]->value = p[0] /*+ neAbs(c2w[0][0]) + neAbs(c2w[0][1]) + neAbs(c2w[0][2]);//*/ + col.boundingRadius;
704
705 if (minCoord[1])
706 minCoord[1]->value = p[1] /*- neAbs(c2w[1][0]) - neAbs(c2w[1][1]) - neAbs(c2w[1][2])*/ - col.boundingRadius;
707 if (maxCoord[1])
708 maxCoord[1]->value = p[1] /*+ neAbs(c2w[1][0]) + neAbs(c2w[1][1]) + neAbs(c2w[1][2]);*/ + col.boundingRadius;
709
710 if (minCoord[2])
711 minCoord[2]->value = p[2] /*- neAbs(c2w[2][0]) - neAbs(c2w[2][1]) - neAbs(c2w[2][2])*/ - col.boundingRadius;
712 if (maxCoord[2])
713 maxCoord[2]->value = p[2] /*+ neAbs(c2w[2][0]) + neAbs(c2w[2][1]) + neAbs(c2w[2][2]);*/ + col.boundingRadius;
714 #else
715 //neM3 & rot = State().rot();
716
717 neT3 c2w = State().b2w * obb;
718 /*
719 neM3 c2w2 = rot * dobb;
720
721 for (s32 i = 0; i < 3; i++)
722 {
723 f32 a = neAbs(c2w2[0][i]) + neAbs(c2w2[1][i]) + neAbs(c2w2[2][i]);
724
725 debugMinBound[i] = p[i] - a;
726 }
727 */
728 /*
729 c2w.M[0][0] = obb.M[0][0] * rot.M[0][0] + obb.M[0][1] * rot.M[1][0] + obb.M[0][2] * rot.M[2][0];
730 c2w.M[0][1] = obb.M[0][0] * rot.M[0][1] + obb.M[0][1] * rot.M[1][1] + obb.M[0][2] * rot.M[2][1];
731 c2w.M[0][2] = obb.M[0][0] * rot.M[0][2] + obb.M[0][1] * rot.M[1][2] + obb.M[0][2] * rot.M[2][2];
732 c2w.M[1][0] = obb.M[1][0] * rot.M[0][0] + obb.M[1][1] * rot.M[1][0] + obb.M[1][2] * rot.M[2][0];
733 c2w.M[1][1] = obb.M[1][0] * rot.M[0][1] + obb.M[1][1] * rot.M[1][1] + obb.M[1][2] * rot.M[2][1];
734 c2w.M[1][2] = obb.M[1][0] * rot.M[0][2] + obb.M[1][1] * rot.M[1][2] + obb.M[1][2] * rot.M[2][2];
735 c2w.M[2][0] = obb.M[2][0] * rot.M[0][0] + obb.M[2][1] * rot.M[1][0] + obb.M[2][2] * rot.M[2][0];
736 c2w.M[2][1] = obb.M[2][0] * rot.M[0][1] + obb.M[2][1] * rot.M[1][1] + obb.M[2][2] * rot.M[2][1];
737 c2w.M[2][2] = obb.M[2][0] * rot.M[0][2] + obb.M[2][1] * rot.M[1][2] + obb.M[2][2] * rot.M[2][2];
738 */
739
740 neV3 &p = c2w.pos;
741
742 f32 a = neAbs(c2w.rot[0][0]) + neAbs(c2w.rot[1][0]) + neAbs(c2w.rot[2][0]);
743
744 minBound[0] = p[0] - a;
745 maxBound[0] = p[0] + a;
746
747 if (minCoord[0])
748 minCoord[0]->value = p[0] - a ;
749 if (maxCoord[0])
750 maxCoord[0]->value = p[0] + a;
751
752 a = neAbs(c2w.rot[0][1]) + neAbs(c2w.rot[1][1]) + neAbs(c2w.rot[2][1]);
753
754 minBound[1] = p[1] - a;
755 maxBound[1] = p[1] + a;
756
757 if (minCoord[1])
758 minCoord[1]->value = p[1] - a;
759 if (maxCoord[1])
760 maxCoord[1]->value = p[1] + a;
761
762 a = neAbs(c2w.rot[0][2]) + neAbs(c2w.rot[1][2]) + neAbs(c2w.rot[2][2]);
763
764 minBound[2] = p[2] - a;
765 maxBound[2] = p[2] + a;
766
767 if (minCoord[2])
768 minCoord[2]->value = p[2] - a;
769 if (maxCoord[2])
770 maxCoord[2]->value = p[2] + a;
771 #endif
772 }
773 }
774
WakeUp()775 void neRigidBody_::WakeUp()
776 {
777 status = neRigidBody_::NE_RBSTATUS_NORMAL;
778
779 lowEnergyCounter = 0;
780
781 SyncOldState();
782 }
783
SyncOldState()784 void neRigidBody_::SyncOldState()
785 {
786 oldPosition = State().b2w.pos;
787
788 oldRotation = State().q;
789
790 oldVelocity = Derive().linearVel;
791
792 oldAngularVelocity = Derive().angularVel;
793
794 oldCounter = 0;
795 }
796
BecomeIdle()797 void neRigidBody_::BecomeIdle()
798 {
799 //return;
800
801 status = neRigidBody_::NE_RBSTATUS_IDLE;
802
803 ZeroMotion();
804 }
805
ZeroMotion()806 void neRigidBody_::ZeroMotion()
807 {
808 Derive().angularVel.SetZero();
809 Derive().linearVel.SetZero();
810 Derive().qDot.Zero();
811 Derive().speed = 0.0f;
812 State().angularMom.SetZero();
813
814 for (s32 i = 0; i < NE_RB_MAX_PAST_RECORDS; i++)
815 {
816 GetVelRecord(i).SetZero();
817 GetAngVelRecord(i).SetZero();
818 dvRecord[i].SetZero();
819 }
820 }
821
822 /****************************************************************************
823 *
824 * neRigidBody_::ApplyCollisionImpulse
825 *
826 ****************************************************************************/
827
ApplyCollisionImpulse(const neV3 & impulse,const neV3 & contactPoint,neImpulseType itype)828 neBool neRigidBody_::ApplyCollisionImpulse(const neV3 & impulse, const neV3 & contactPoint, neImpulseType itype)
829 {
830 neV3 dv, da, newAM;
831
832 dv = impulse * oneOnMass;
833
834 da = contactPoint.Cross(impulse);
835
836 neBool immediate = true;
837
838 if (itype == IMPULSE_CONTACT)// && sim->solverStage == 1) // only if contact and resolve penetration stage
839 {
840 immediate = false;
841 }
842
843 /* if (itype != IMPULSE_NORMAL && sim->solverStage != 2)
844 {
845 immediate = false;
846 }
847 */
848
849 immediate = true;
850
851 if (immediate)
852 {
853 Derive().linearVel += dv;
854
855 dvRecord[sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] += dv;
856
857 neV3 dav = Derive().Iinv * da;
858
859 davRecord[sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] += dav;
860
861 newAM = State().angularMom + da;
862
863 SetAngMom(newAM);
864
865 f32 l = dv.Length();
866 /*
867 if (id == 1 && l > sim->magicNumber)
868 {
869 sim->magicNumber = l;
870 }
871 */
872 ASSERT(newAM.IsFinite());
873 }
874 else
875 {
876 totalDV += dv;
877 totalDA += da;
878 impulseCount++;
879 twistCount++;
880 }
881 return true;
882 }
883
AddRestContact(neRestRecord & rc)884 void neRigidBody_::AddRestContact(neRestRecord & rc)
885 {
886 //search existing matching records
887
888 s32 oldest = -1;
889
890 s32 freeOne = -1;
891
892 f32 shallowest = 0.0f;
893
894 s32 i;
895
896 for (i = 0; i < NE_RB_MAX_RESTON_RECORDS; i++)
897 {
898 if (!GetRestRecord(i).IsValid())
899 {
900 freeOne = i;
901
902 continue;
903 }
904 if (shallowest == 0.0f)
905 {
906 shallowest = GetRestRecord(i).depth;
907 oldest = i;
908 }
909 else if (GetRestRecord(i).depth < shallowest)
910 {
911 shallowest = GetRestRecord(i).depth;
912 oldest = i;
913 }
914 neV3 diff = rc.bodyPoint - GetRestRecord(i).bodyPoint;
915
916 f32 dot = diff.Dot(diff);
917
918 if (dot < 0.0025f) //difference of 0.05 meters, or 5 cm
919 {
920 //found
921 break;
922 }
923 }
924 /* if (i < NE_RB_MAX_RESTON_RECORDS)
925 {
926 GetRestRecord(i).SetInvalid();
927 }
928 else
929 {
930 if (freeOne != -1)
931 {
932 i = freeOne;
933 }
934 else
935 {
936 //use the olderest one
937 ASSERT(oldest != -1);
938
939 i = oldest;
940 }
941 }
942 */
943 if (i == NE_RB_MAX_RESTON_RECORDS)
944 { //not found
945
946 //find a free one
947 if (freeOne != -1)
948 {
949 i = freeOne;
950 }
951 else
952 {
953 //use the olderest one
954 ASSERT(oldest != -1);
955
956 i = oldest;
957
958 GetRestRecord(i).SetInvalid();
959 }
960 }
961 else
962 {
963 GetRestRecord(i).SetInvalid();
964 }
965 if (i >= NE_RB_MAX_RESTON_RECORDS || i < 0)
966 return;
967
968 GetRestRecord(i).Set(this, rc);
969 }
970
IsConstraintNeighbour(neRigidBodyBase * otherBody)971 neBool neRigidBody_::IsConstraintNeighbour(neRigidBodyBase * otherBody)
972 {
973 neConstraintHandle * chandle = constraintCollection.GetHead();
974
975 while (chandle)
976 {
977 _neConstraint * c = chandle->thing;
978
979 chandle = constraintCollection.GetNext(chandle);
980
981 neConstraintHandle * chandleB = otherBody->constraintCollection.GetHead();
982
983 while (chandleB)
984 {
985 _neConstraint * cB = chandleB->thing;
986
987 if (c == cB)
988 return true;
989
990 chandleB = otherBody->constraintCollection.GetNext(chandleB);
991 }
992 };
993 return false;
994 }
995
SetAngMomComponent(const neV3 & angMom,const neV3 & dir)996 void neRigidBody_::SetAngMomComponent(const neV3 & angMom, const neV3 & dir)
997 {
998 neV3 newMom = State().angularMom;
999
1000 newMom.RemoveComponent(dir);
1001
1002 newMom += angMom;
1003
1004 SetAngMom(newMom);
1005
1006 needRecalc = true;
1007 }
1008
WakeUpAllJoint()1009 void neRigidBody_::WakeUpAllJoint()
1010 {
1011 if (!GetConstraintHeader())
1012 {
1013 WakeUp();
1014 return;
1015 }
1016 neBodyHandle * bodyHandle = GetConstraintHeader()->bodies.GetHead();
1017
1018 while (bodyHandle)
1019 {
1020 if (!bodyHandle->thing->AsRigidBody())
1021 {
1022 bodyHandle = bodyHandle->next;
1023
1024 continue;
1025 }
1026
1027 bodyHandle->thing->AsRigidBody()->WakeUp();
1028
1029 bodyHandle = bodyHandle->next;
1030 }
1031 }
1032
ApplyLinearConstraint()1033 void neRigidBody_::ApplyLinearConstraint()
1034 {
1035 neV3 dv = totalDV / (f32)impulseCount;
1036
1037 // cacheImpulse = totalDV;
1038
1039 Derive().linearVel += dv;
1040
1041 totalDV.SetZero();
1042
1043 //dvRecord[sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] += dv;
1044
1045 impulseCount = 0;
1046 }
1047
ApplyAngularConstraint()1048 void neRigidBody_::ApplyAngularConstraint()
1049 {
1050 neV3 da = totalDA / (f32)twistCount;
1051
1052 // cacheTwist += da;
1053
1054 neV3 newAM = State().angularMom + da;
1055
1056 SetAngMom(newAM);
1057
1058 totalDA.SetZero();
1059
1060 twistCount = 0;
1061 }
1062 /*
1063 void neRigidBody_::ConstraintDoSleepCheck()
1064 {
1065 f32 len = dvRecord[sim->stepSoFar % NE_RB_MAX_PAST_RECORDS].Length();
1066
1067
1068 if (len > 0.3f)
1069 {
1070 //WakeUpAllJoint();
1071 //WakeUp();
1072 }
1073 else
1074 {
1075 len = davRecord[sim->stepSoFar % NE_RB_MAX_PAST_RECORDS].Length();
1076
1077 if (len > 0.5f)
1078 {
1079 //WakeUp();//AllJoint();
1080 }
1081 else
1082 {
1083 BecomeIdle();
1084 }
1085 }
1086 }
1087 */
1088
CanConsiderOtherBodyIdle()1089 neBool neRestRecord::CanConsiderOtherBodyIdle()
1090 {
1091 if (rtype == REST_ON_WORLD)
1092 return true;
1093
1094 ASSERT(otherBody != NULL);
1095
1096 neRigidBody_ * rb = otherBody->AsRigidBody();
1097
1098 if (rb != NULL)
1099 {
1100 return (rb->status == neRigidBody_::NE_RBSTATUS_IDLE);
1101 }
1102 neCollisionBody_ * cb = otherBody->AsCollisionBody();
1103
1104 ASSERT(cb);
1105
1106 return (!cb->moved);
1107 }
1108
CheckOtherBody(neFixedTimeStepSimulator * sim)1109 neBool neRestRecord::CheckOtherBody(neFixedTimeStepSimulator * sim)
1110 {
1111 ASSERT(otherBody);
1112
1113 if (otherBody != sim->GetTerrainBody() &&
1114 (!otherBody->IsValid() || !otherBody->isActive))
1115 {
1116 return false;
1117 }
1118 return true;
1119 }
1120
GetOtherBodyPoint()1121 neV3 neRestRecord::GetOtherBodyPoint()
1122 {
1123 ASSERT(otherBody);
1124
1125 neCollisionBody_ * cb = otherBody->AsCollisionBody();
1126
1127 neV3 ret;
1128
1129 if (cb)
1130 {
1131 normalWorld = cb->b2w.rot * normalBody;
1132
1133 ret = cb->b2w * otherBodyPoint;
1134 }
1135 else
1136 {
1137 normalWorld = ((neRigidBody_*)otherBody)->State().b2w.rot * normalBody;
1138
1139 ret = ((neRigidBody_*)otherBody)->State().b2w * otherBodyPoint;
1140 }
1141 return ret;
1142 }
1143
Set(neRigidBody_ * thisBody,const neRestRecord & rc)1144 void neRestRecord::Set(neRigidBody_* thisBody, const neRestRecord & rc)
1145 {
1146 bodyPoint = rc.bodyPoint;
1147 otherBodyPoint = rc.otherBodyPoint;
1148 otherBody = rc.otherBody;
1149 counter = thisBody->sim->stepSoFar;
1150 normalBody = rc.normalBody;
1151 depth = rc.depth;
1152 body = thisBody;
1153 material = rc.material;
1154 otherMaterial = rc.otherMaterial;
1155
1156 if (rc.otherBody->AsCollisionBody())
1157 rtype = neRestRecord::REST_ON_COLLISION_BODY;
1158 else
1159 rtype = neRestRecord::REST_ON_RIGID_BODY;
1160
1161 otherBody->rbRestingOnMe.Add(&restOnHandle);
1162 }
1163
SetTmp(neRigidBodyBase * _otherBody,const neV3 & contactA,const neV3 & contactB,const neV3 & _normalBody,s32 matA,s32 matB)1164 void neRestRecord::SetTmp(neRigidBodyBase * _otherBody, const neV3 & contactA, const neV3 & contactB, const neV3 & _normalBody, s32 matA, s32 matB)
1165 {
1166 otherBody = _otherBody;
1167 bodyPoint = contactA;
1168 otherBodyPoint = contactB;
1169 material = matA;
1170 otherMaterial = matB;
1171 normalBody = _normalBody;
1172 }
1173
SetInvalid()1174 void neRestRecord::SetInvalid()
1175 {
1176 //ASSERT(rtype != REST_ON_NOT_VALID);
1177
1178 rtype = REST_ON_NOT_VALID;
1179
1180 counter = 0;
1181
1182 // body = NULL;
1183
1184 if (otherBody)
1185 {
1186 otherBody->rbRestingOnMe.Remove(&restOnHandle);
1187 }
1188 otherBody = NULL;
1189 }
1190