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 #define CONTACT_VALIDITY_NORMAL_DISTANCE (0.01f)
39 #define CONTACT_VALIDITY_TANGENT_DISTANCE_SQ (0.001f)//(0.01f * 0.01f)
40
41 /****************************************************************************
42 *
43 * neRigidBody_::CheckForIdle
44 *
45 ****************************************************************************/
TestZeroInTriangle(const neV3 & _p1,const neV3 & _p2,const neV3 & _p3)46 neBool TestZeroInTriangle(const neV3 & _p1, const neV3 & _p2, const neV3 & _p3)
47 {
48 ASSERT(_p1.IsFinite());
49 ASSERT(_p2.IsFinite());
50 ASSERT(_p3.IsFinite());
51
52 neV3 p1, p2, p3, average;
53
54 average = _p1 + _p2 + _p3;
55
56 average *= (1.0f / 3.0f);
57
58 //increase the triangle by 10%
59 p1 = average + (_p1 - average) * 1.1f;
60 p2 = average + (_p2 - average) * 1.1f;
61 p3 = average + (_p3 - average) * 1.1f;
62
63 //p1 cross p2
64 const s32 x = 0;
65 const s32 z = 2;
66
67 f32 cross12 = p1[z] * p2[x] - p1[x] * p2[z];
68 f32 cross23 = p2[z] * p3[x] - p2[x] * p3[z];
69 f32 cross31 = p3[z] * p1[x] - p3[x] * p1[z];
70
71 if (cross12 > 0.0f && cross23 > 0.0f && cross31 > 0.0f)
72 {
73 return true;
74 }
75 else if (cross12 < 0.0f && cross23 < 0.0f && cross31 < 0.0f)
76 {
77 return true;
78 }
79 return false;
80 }
81
CheckForIdle()82 void neRigidBody_::CheckForIdle()
83 {
84 CheckForIdleNonJoint();
85 /*
86 if (CheckStationary())
87 {
88 if ((stackInfo && IsRestPointStillValid()))
89 {
90 s32 hull = CheckRestHull();
91
92 if (hull >= 2)
93 {
94 //if (hull == 2 && lowEnergyCounter > 100)
95 // BecomeIdle();
96 //else if (hull > 2)
97 BecomeIdle();
98 }
99 }
100 }
101 */
102 }
103 #pragma optimize( "", off )
CheckForIdleNonJoint()104 void neRigidBody_::CheckForIdleNonJoint()
105 {
106 //return;
107
108 f32 e = Derive().linearVel.Dot(Derive().linearVel);
109
110 f32 f = (Derive().angularVel.Dot(Derive().angularVel)) * 1.0f;
111
112 if (e < sim->highEnergy && f < sim->highEnergy)
113 {
114 lowEnergyCounter++;
115 }
116 else
117 {
118 s32 i;
119
120 neV3 total; total.SetZero();
121
122 for (i = 0; i < NE_RB_MAX_PAST_RECORDS; i++)
123 {
124 total += GetVelRecord(i);
125 }
126
127 neV3 t;
128 t = total *(1.0f / NE_RB_MAX_PAST_RECORDS);
129 total = t;
130
131 ASSERT(total.IsFinite());
132
133 f32 g; g = total.Dot(sim->gravityVector);
134
135 if (g > 0.0f)
136 {
137 total.RemoveComponent(sim->gravityVector);
138 }
139
140 f32 v = total.Dot(total);
141
142 if (v > 2.0f)//5
143 {
144 lowEnergyCounter = 0;
145 return;
146 }
147 total.SetZero();
148
149 for (i = 0; i < NE_RB_MAX_PAST_RECORDS; i++)
150 {
151 total += GetAngVelRecord(i);
152 }
153 total *= (1.0f / NE_RB_MAX_PAST_RECORDS);
154
155 v = total.Dot(total);
156
157 if (v > 1.0f)//4
158 {
159 lowEnergyCounter = 0;
160 return;
161 }
162 lowEnergyCounter+=1;
163 }
164 if (lowEnergyCounter > 10)
165 {
166 if ((stackInfo && IsRestPointStillValid()))
167 {
168 s32 hull = CheckRestHull();
169
170 if (hull)
171 {
172 if (hull == 2 && lowEnergyCounter > 100)
173 BecomeIdle();
174 else if (hull > 2)
175 BecomeIdle();
176 }
177 }
178 }
179 }
180 #pragma optimize( "", on )
CheckForIdleJoint()181 void neRigidBody_::CheckForIdleJoint()
182 {
183 f32 e = Derive().linearVel.Length();
184
185 f32 f = col.boundingRadius * Derive().angularVel.Length();
186
187 e += f;
188
189 if (e < sleepingParam)
190 {
191 lowEnergyCounter++;
192 }
193 else
194 {
195 lowEnergyCounter = 0;
196 }
197 if (lowEnergyCounter > 50)
198 {
199 // calculate net force
200 //NE_RB_MAX_PAST_RECORDS
201
202 neV3 sum;
203
204 sum.SetZero();
205
206 for (s32 i = 0; i < NE_RB_MAX_PAST_RECORDS; i++)
207 {
208 sum += dvRecord[i];
209 }
210
211 ASSERT(sum.IsFinite());
212
213 neV3 average = sum;// / (f32)NE_RB_MAX_PAST_RECORDS;
214
215 f32 len1 = average.Length();
216
217 if (len1 < sleepingParam/*0.3f*/)
218 {
219 BecomeIdle();
220 }
221 }
222 }
223
CheckRestHull()224 s32 neRigidBody_::CheckRestHull()
225 {
226 if (totalForce.IsConsiderZero())
227 return 3;
228
229 neM3 forceFrame;
230
231 forceFrame[1] = totalForce;
232
233 forceFrame[1].Normalize();
234
235 void ChooseAxis(neV3 & x, neV3 & y, const neV3 & normal);
236
237 ChooseAxis(forceFrame[0], forceFrame[2], forceFrame[1]);
238
239 forceFrame.SetTranspose(forceFrame);
240
241 neV3 p[3];
242
243 if (GetRestHull().htype == neRestHull::TRIANGLE)
244 {
245 neBool allIdle = true;
246
247 for (s32 i = 0; i < 3; i++)
248 {
249 if (!GetRestRecord(i).IsValid())
250 {
251 ASSERT(0);
252 }
253 if (!GetRestRecord(i).CanConsiderOtherBodyIdle())
254 {
255 allIdle = false;
256
257 break;
258 }
259 p[i] = GetRestRecord(i).worldThisBody - GetPos();
260
261 p[i] = forceFrame.TransposeMulV3(p[i]);
262 }
263 if (!allIdle)
264 return 0;
265
266 neBool ret = TestZeroInTriangle(p[0], p[1], p[2]);
267
268 if (ret)
269 return 3;
270 }
271 if (GetRestHull().htype == neRestHull::LINE)
272 {
273 neBool allIdle = true;
274
275 for (s32 i = 0; i < 2; i++)
276 {
277 s32 j = GetRestHull().indices[i];
278
279 if (!GetRestRecord(j).IsValid())
280 {
281 allIdle = false;
282
283 break;
284 }
285 if (!GetRestRecord(j).CanConsiderOtherBodyIdle())
286 {
287 allIdle = false;
288
289 break;
290 }
291 p[i] = GetRestRecord(j).worldThisBody - GetPos();
292
293 p[i] = forceFrame.TransposeMulV3(p[i]);
294
295 p[i][1] = 0.0f;
296 }
297 if (!allIdle)
298 return 0;
299
300 p[2].SetZero();
301
302 f32 d = p[2].GetDistanceFromLine(p[0], p[1]);
303
304 if (d < 0.005f)
305 return 2;
306
307 return false;
308 }
309 else if (GetRestHull().htype == neRestHull::POINT)
310 {
311 neBool allIdle = true;
312
313 s32 i = GetRestHull().indices[0];
314
315 if (!GetRestRecord(i).IsValid())
316 {
317 return 0;
318 }
319 if (!GetRestRecord(i).CanConsiderOtherBodyIdle())
320 {
321 return 0;
322 }
323 p[0] = GetRestRecord(GetRestHull().indices[0]).worldThisBody - GetPos();
324
325 p[0] = forceFrame.TransposeMulV3(p[0]);
326
327 p[0][1] = 0.0f;
328
329 f32 d = sqrtf(p[0][0] * p[0][0] + p[0][2] * p[0][2]);
330
331 if (d < 0.005f)
332 return 1;
333
334 return false;
335 }
336 else
337 {
338 return false;
339 }
340 }
341
CheckStillIdle()342 neBool neRigidBody_::CheckStillIdle()
343 {
344 if (!CheckHighEnergy())
345 {
346 if (subType == NE_RIGID_PARTICLE || CheckRestHull())
347 {
348 ZeroMotion();
349
350 UpdateController();
351
352 sim->idleBodyCount++;
353 return true;
354 }
355 else
356 {
357 return false;
358 }
359
360 }
361 else
362 {
363 WakeUp();
364
365 return false;
366 }
367 }
368
CheckContactValidity()369 s32 neRigidBody_::CheckContactValidity()
370 {
371 if (!stackInfo)
372 return 0;
373
374 s32 validCount = 0;
375
376 s32 i;
377
378 neBool allIdle = false;
379
380 if (status == neRigidBody_::NE_RBSTATUS_IDLE && !isShifted)
381 {
382 allIdle = true;
383
384 for (i = 0; i < NE_RB_MAX_RESTON_RECORDS; i++)
385 {
386 if (!GetRestRecord(i).IsValid())
387 {
388 continue;
389 }
390
391 if (!GetRestRecord(i).CanConsiderOtherBodyIdle())
392 {
393 allIdle = false;
394
395 break;
396 }
397 validCount++;
398 }
399 }
400
401 if (allIdle)
402 {
403 return validCount;
404 }
405
406 validCount = 0;
407
408 for (i = 0; i < NE_RB_MAX_RESTON_RECORDS; i++)
409 {
410 if (!GetRestRecord(i).IsValid())
411 continue;
412
413 GetRestRecord(i).Update();
414
415 f32 d = GetRestRecord(i).normalWorld.Dot(sim->gravityVector);
416
417 if (d > 0.0f/*-TILT_TOLERANCE*/)
418 {
419 GetRestRecord(i).SetInvalid();
420 GetRestHull().htype = neRestHull::NONE;
421 continue;
422 }
423 if (GetRestRecord(i).normalDiff > CONTACT_VALIDITY_NORMAL_DISTANCE)
424 {
425 GetRestRecord(i).SetInvalid();
426 GetRestHull().htype = neRestHull::NONE;
427 continue;
428 }
429 if (GetRestRecord(i).tangentialDiffSq > CONTACT_VALIDITY_TANGENT_DISTANCE_SQ)
430 {
431 GetRestRecord(i).SetInvalid();
432 GetRestHull().htype = neRestHull::NONE;
433 continue;
434 }
435 validCount++;
436 }
437 if (validCount == 0)
438 stackInfo->isBroken = true;
439 else
440 stackInfo->isBroken = false;
441
442 return validCount;
443 }
444
AddContactImpulseRecord(neBool withConstraint)445 s32 neRigidBody_::AddContactImpulseRecord(neBool withConstraint)
446 {
447 s32 i = 0;
448 static neV3 world1[NE_RB_MAX_RESTON_RECORDS];
449 static neV3 world2[NE_RB_MAX_RESTON_RECORDS];
450 static neV3 diff[NE_RB_MAX_RESTON_RECORDS];
451 static f32 height[NE_RB_MAX_RESTON_RECORDS];
452 s32 validCount = 0;
453 static s32 validIndices[NE_RB_MAX_RESTON_RECORDS];
454 s32 deepestIndex = -1;
455 f32 deepest = -1.0e6f;
456
457 for (i = 0; i < NE_RB_MAX_RESTON_RECORDS; i++)
458 {
459 if (!GetRestRecord(i).IsValid())
460 continue;
461
462 if (!GetRestRecord(i).CheckOtherBody(sim))
463 {
464 GetRestRecord(i).SetInvalid();
465
466 continue;
467 }
468 GetRestRecord(i).Update();
469
470 world1[i] = State().b2w * GetRestRecord(i).bodyPoint;
471
472 world2[i] = GetRestRecord(i).GetOtherBodyPoint();
473
474 diff[i] = world1[i] - world2[i];
475
476 neV3 d; d = diff[i];
477
478 d.RemoveComponent(GetRestRecord(i).normalWorld);
479
480 if (d.Dot(d) > 0.025f)
481 {
482 GetRestRecord(i).SetInvalid();
483
484 GetRestHull().htype = neRestHull::NONE;
485
486 continue;
487 }
488
489 height[i] = diff[i].Dot(GetRestRecord(i).normalWorld);
490
491 validIndices[validCount] = i;
492
493 validCount++;
494
495 if (height[i] > deepest)
496 {
497 deepest = height[i];
498
499 deepestIndex = i;
500 }
501 }
502 if (validCount == 0)
503 {
504 GetRestHull().htype = neRestHull::NONE;
505 return 0;
506 }
507
508 if (0)//subType == NE_RIGID_PARTICLE)
509 {
510 ASSERT(deepestIndex != -1);
511
512 i = deepestIndex;
513
514 neCollisionResult tmpcr;
515 neCollisionResult * cresult = &tmpcr;
516
517 cresult->bodyA = (neRigidBodyBase*)this;
518 cresult->bodyB = GetRestRecord(i).GetOtherBody();
519 cresult->collisionFrame[2] = GetRestRecord(i).normalWorld;
520 cresult->contactA = world1[i] - GetPos();
521 cresult->contactB = world2[i] - GetRestRecord(i).GetOtherBody()->GetB2W().pos;
522 cresult->materialIdA = GetRestRecord(i).material;
523 cresult->materialIdB = GetRestRecord(i).otherMaterial;
524 cresult->depth = -GetRestRecord(i).normalDiff;//GetRestRecord(i).depth;
525 cresult->impulseType = IMPULSE_CONTACT;
526 cresult->impulseScale = 1.0f;
527 cresult->PrepareForSolver();
528
529 if (withConstraint || !cresult->CheckIdle())
530 {
531 sim->AddCollisionResult(tmpcr);
532 }
533 }
534
535 s32 j;
536
537 f32 heightScale = 1.0f;
538
539 if (validCount == 1 && height[validIndices[0]] < 0.0f)
540 {
541 //if (subType == NE_RIGID_NORMAL)
542 {
543 i = validIndices[0];
544
545 neCollisionResult tmpcr;
546 neCollisionResult * cresult = &tmpcr;
547
548 cresult->bodyA = (neRigidBodyBase*)this;
549 cresult->bodyB = GetRestRecord(i).GetOtherBody();
550 cresult->collisionFrame[2] = GetRestRecord(i).normalWorld;
551 ASSERT(cresult->collisionFrame[2].IsFinite());
552 cresult->contactA = world1[i] - GetPos();
553 cresult->contactB = world2[i] - GetRestRecord(i).GetOtherBody()->GetB2W().pos;
554 cresult->materialIdA = GetRestRecord(i).material;
555 cresult->materialIdB = GetRestRecord(i).otherMaterial;
556 cresult->depth = -GetRestRecord(i).normalDiff;//GetRestRecord(i).depth;
557 cresult->impulseType = IMPULSE_CONTACT;
558 //cresult->UpdateConstraintRelativeSpeed();
559 cresult->PrepareForSolver();
560 cresult->impulseScale = 1.0f;
561
562 if (withConstraint || !cresult->CheckIdle())
563 {
564 //*sim->cresultHeap.Alloc(0) = tmpcr;
565 sim->AddCollisionResult(tmpcr);
566 }
567 }
568 GetRestHull().htype = neRestHull::NONE;
569 return 1;
570 }
571 else if (validCount == 2)
572 {
573 s32 v1 = validIndices[0];
574 s32 v2 = validIndices[1];
575
576 neV3 d1 = world1[v1] - world1[v2];
577 neV3 d2 = world2[v1] - world2[v2];
578
579 f32 len1 = d1.Length();
580
581 if (neIsConsiderZero(len1))
582 {
583 heightScale = 1.0f;
584 }
585 else
586 {
587 f32 len2 = d2.Length();
588
589 if (neIsConsiderZero(len2))
590 {
591 heightScale = 1.0f;
592 }
593 else
594 {
595 d1 *= (1.0f / len1);
596
597 d2 *= (1.0f / len2);
598
599 heightScale = neAbs(d1.Dot(d2));
600 }
601 }
602
603 ASSERT(neIsFinite(heightScale));
604 //if (!neIsFinite(heightScale))
605 // heightScale = 1.0f;
606 }
607 else if (validCount == 3)
608 {
609 neV3 tri1[3];
610 neV3 tri2[3];
611
612 tri1[0] = world1[1] - world1[0];
613 tri1[1] = world1[2] - world1[1];
614 tri1[2] = world1[0] - world1[2];
615
616 tri2[0] = world2[1] - world2[0];
617 tri2[1] = world2[2] - world2[1];
618 tri2[2] = world2[0] - world2[2];
619
620 neV3 normal1 = tri1[1].Cross(tri1[0]);
621 neV3 normal2 = tri2[1].Cross(tri2[0]);
622
623 f32 len1 = normal1.Length();
624
625 if (neIsConsiderZero(len1))
626 {
627 heightScale = 1.0f;
628 }
629 else
630 {
631 f32 len2 = normal2.Length();
632
633 if (neIsConsiderZero(len2))
634 {
635 heightScale = 1.0f;
636 }
637 else
638 {
639 normal1 *= (1.0f / len1);
640
641 normal2 *= (1.0f / len2);
642
643 heightScale = neAbs(normal1.Dot(normal2));
644 }
645 }
646 ASSERT(neIsFinite(heightScale));
647 //if (!neIsFinite(heightScale))
648 // heightScale = 1.0f;
649 }
650
651 f32 e = 0.0005f;
652 f32 f = 1.0f - e;
653
654 heightScale = (heightScale - f) / e;
655
656 if (heightScale < 0.0f)
657 {
658 heightScale = e;
659 }
660 s32 actualValidCount = 0;
661 //f32 limit = 0.05f;
662 f32 limit = 0.01f;
663
664 for (i = 0; i < validCount; i++)
665 {
666 f32 scale = 1.0f;
667
668 j = validIndices[i];
669
670 f32 scaleLimit = 0.01f;//limit * heightScale;
671
672 if (height[j] > 0)
673 {
674 if (height[j] > scaleLimit)
675 {
676 //GetRestRecord(j).rtype = neRestRecord::REST_ON_NOT_VALID;
677 //GetRestHull().htype = neRestHull::NONE;
678 continue;
679 }
680 scale = (scaleLimit - height[j]) / scaleLimit;
681
682 scale = scale * scale * scale;
683 }
684 //if (subType == NE_RIGID_NORMAL)
685 {
686 neCollisionResult tmpcr;
687 neCollisionResult * cresult = &tmpcr;//sim->cresultHeap.Alloc(0);
688
689 cresult->bodyA = (neRigidBodyBase*)this;
690 cresult->bodyB = GetRestRecord(j).GetOtherBody();
691 cresult->collisionFrame[2] = GetRestRecord(j).normalWorld;
692 cresult->contactA = world1[j] - GetPos();
693 cresult->contactB = world2[j] - GetRestRecord(j).GetOtherBody()->GetB2W().pos;
694 cresult->materialIdA = GetRestRecord(j).material;
695 cresult->materialIdB = GetRestRecord(j).otherMaterial;
696 cresult->depth = -GetRestRecord(j).normalDiff;//GetRestRecord(j).depth;
697 cresult->impulseType = IMPULSE_CONTACT;
698 //cresult->UpdateConstraintRelativeSpeed();
699 cresult->PrepareForSolver();
700 cresult->impulseScale = scale;
701 if (withConstraint || !cresult->CheckIdle())
702 {
703 //*sim->cresultHeap.Alloc(0) = tmpcr;
704 sim->AddCollisionResult(tmpcr);
705 }
706 }
707 //sim->HandleCollision(this, GetRestRecord(j).otherBody, cresult, IMPULSE_NORMAL, scale);
708
709 GetRestHull().indices[actualValidCount++] = j;
710 }
711 if (actualValidCount >= 2)
712 {
713 if (actualValidCount == 2)
714 {
715 GetRestHull().htype = neRestHull::LINE;
716
717 ASSERT(GetRestRecord(GetRestHull().indices[0]).IsValid());
718 ASSERT(GetRestRecord(GetRestHull().indices[1]).IsValid());
719
720 GetRestHull().normal = GetRestRecord(GetRestHull().indices[0]).normalWorld +
721 GetRestRecord(GetRestHull().indices[1]).normalWorld;
722 /*
723 neV3 diff = world2[GetRestHull().indices[0]] - world2[GetRestHull().indices[1]];
724
725 neV3 cross = diff.Cross(sim->gravityVector);
726
727 GetRestHull().normal= cross.Cross(diff);
728 */
729 GetRestHull().normal.Normalize();
730
731 if (GetRestHull().normal.Dot(sim->gravityVector) < 0.0f)
732 GetRestHull().normal *= -1.0f;
733 }
734 else
735 {
736 GetRestHull().htype = neRestHull::TRIANGLE;
737
738 ASSERT(GetRestRecord(GetRestHull().indices[0]).IsValid());
739 ASSERT(GetRestRecord(GetRestHull().indices[1]).IsValid());
740 ASSERT(GetRestRecord(GetRestHull().indices[2]).IsValid());
741
742 GetRestHull().normal = GetRestRecord(GetRestHull().indices[0]).normalWorld +
743 GetRestRecord(GetRestHull().indices[1]).normalWorld +
744 GetRestRecord(GetRestHull().indices[2]).normalWorld;
745
746 /*
747 neV3 diff1 = world2[GetRestHull().indices[0]] - world2[GetRestHull().indices[1]];
748
749 neV3 diff2 = world2[GetRestHull().indices[2]] - world2[GetRestHull().indices[0]];
750
751 GetRestHull().normal = diff1.Cross(diff2);
752 */
753 GetRestHull().normal.Normalize();
754
755 if (GetRestHull().normal.Dot(sim->gravityVector) < 0.0f)
756 GetRestHull().normal *= -1.0f;
757 }
758 }
759 else
760 {
761 if (actualValidCount == 1)
762 {
763 GetRestHull().htype = neRestHull::POINT;
764
765 ASSERT(GetRestRecord(GetRestHull().indices[0]).IsValid());
766
767 GetRestHull().normal = GetRestRecord(GetRestHull().indices[0]).normalWorld;
768
769 if (GetRestHull().normal.Dot(sim->gravityVector) < 0.0f)
770 GetRestHull().normal *= -1.0f;
771 }
772 else
773 {
774 GetRestHull().htype = neRestHull::NONE;
775 }
776 }
777 return actualValidCount;
778 }
779
AddContactConstraint()780 void neRigidBody_::AddContactConstraint()
781 {
782 if (stackInfo->stackHeader == &sim->stackHeaderX)
783 {
784 if (needSolveContactDynamic)
785 {
786 needSolveContactDynamic = false;
787
788 AddContactImpulseRecord(true);
789 }
790 }
791 else
792 {
793 if (stackInfo->stackHeader->dynamicSolved)
794 return;
795
796 neByte ** p = sim->pointerBuffer2.Alloc();
797
798 ASSERT(p);
799
800 *p = (neByte*)(stackInfo->stackHeader);
801
802 stackInfo->stackHeader->dynamicSolved = true;
803 }
804 }
805
CheckHighEnergy()806 neBool neRigidBody_::CheckHighEnergy()
807 {
808 f32 e;
809
810 f32 m;
811
812 if (0)//_constraintHeader)
813 {
814 e = Derive().linearVel.Length();
815
816 e += col.boundingRadius * Derive().angularVel.Length();
817
818 m = 0.5f;
819 }
820 else
821 {
822 e = Derive().linearVel.Dot(Derive().linearVel);
823
824 e += Derive().angularVel.Dot(Derive().angularVel);
825
826 m = sim->highEnergy;
827 }
828
829 if (e < m)
830 return false;
831
832 return true;
833 }
834
CheckStationary()835 neBool neRigidBody_::CheckStationary()
836 {
837 // return false;
838
839 const s32 oldCounterMax = 60;
840
841 const f32 StationarySpeed = sleepingParam;//0.2f;
842 const f32 StationaryAcc = 5.0f;
843 const f32 StationaryW = 10.f;
844 const f32 StationaryAngAcc = 10.5f;
845
846 if (oldCounter < oldCounterMax)
847 return FALSE;
848
849 neV3 deltaPos = State().b2w.pos - oldPosition;
850
851 neV3 vel = deltaPos / (sim->_currentTimeStep * oldCounterMax);
852
853 f32 speed = vel.Length();
854
855 if (speed > StationarySpeed)
856 {
857 SyncOldState();
858
859 return FALSE;
860 }
861
862 neV3 deltaVel = Derive().linearVel - oldVelocity;
863
864 neV3 acc = deltaVel / (sim->_currentTimeStep * oldCounterMax);
865
866 f32 accMag = acc.Length();
867
868 if (accMag > StationaryAcc)
869 {
870 SyncOldState();
871
872 return FALSE;
873 }
874
875 neQ oldQInvert = oldRotation;
876
877 oldQInvert.Invert();
878
879 neQ deltaQ = State().q * oldQInvert;
880
881 neV3 axis; f32 angle;
882
883 deltaQ.GetAxisAngle(axis, angle);
884
885 f32 angularVel = angle / (sim->_currentTimeStep * oldCounterMax);
886
887 if (angularVel > StationaryW)
888 {
889 SyncOldState();
890
891 return FALSE;
892 }
893
894 neV3 deltaW = Derive().angularVel - oldAngularVelocity;
895
896 f32 angularAcc = deltaW.Length();
897
898 angularAcc /= (sim->_currentTimeStep * oldCounterMax);
899
900 if (angularAcc > StationaryAngAcc)
901 {
902 SyncOldState();
903
904 return FALSE;
905 }
906 /* Derive().linearVel *= 0.9f;
907
908 neV3 am = State().angularMom * 0.9f;
909
910 SetAngMom(am);
911 */
912 return true;
913 }
914
915 /****************************************************************************
916 *
917 * neRigidBody_::AddStackInfo
918 *
919 ****************************************************************************/
920
AddStackInfo(neRestRecord & rc)921 neBool neRigidBody_::AddStackInfo(neRestRecord & rc)
922 {
923 if (!stackInfo)
924 return NewStackInfo(rc);
925
926 neRigidBody_ * rb = (neRigidBody_ *) rc.GetOtherBody();
927
928 ASSERT(stackInfo->stackHeader);
929
930 if (stackInfo->isTerminator)
931 {
932 stackInfo->isTerminator = false;
933
934 //ResetRestOnRecords();
935 }
936
937 AddRestContact(rc);
938
939 if (rc.GetOtherCollisionBody())
940 {
941 return true;
942 }
943
944 if (!rb->stackInfo)
945 {
946 if (stackInfo->stackHeader->isHeaderX)
947 {
948 sim->stackHeaderX.Remove(stackInfo);
949
950 sim->NewStackHeader(stackInfo);
951
952 return rb->NewStackInfoTerminator(stackInfo->stackHeader);
953 }
954 else
955 {
956 return rb->NewStackInfoTerminator(stackInfo->stackHeader);
957 }
958 }
959
960 neStackHeader * otherStackHeader = rb->stackInfo->stackHeader;
961
962 ASSERT(otherStackHeader);
963
964 if (otherStackHeader->isHeaderX)
965 {
966 if (stackInfo->stackHeader->isHeaderX)
967 {
968 sim->stackHeaderX.Remove(stackInfo);
969
970 sim->stackHeaderX.Remove(rb->stackInfo);
971
972 sim->NewStackHeader(stackInfo);
973
974 stackInfo->stackHeader->Add(rb->stackInfo);
975
976 //stackInfo->stackHeader->CheckHeader();
977 }
978 else
979 {
980 otherStackHeader->Remove(rb->stackInfo);
981
982 stackInfo->stackHeader->Add(rb->stackInfo);
983
984 //stackInfo->stackHeader->CheckHeader();
985 }
986 }
987 else
988 {
989 if (stackInfo->stackHeader->isHeaderX)
990 {
991 stackInfo->stackHeader->Remove(stackInfo);
992
993 otherStackHeader->Add(stackInfo);
994
995 //otherStackHeader->CheckHeader();
996 }
997 else
998 {
999 if (stackInfo->stackHeader != otherStackHeader)
1000 {
1001 // merge
1002 otherStackHeader->ChangeHeader(stackInfo->stackHeader);
1003
1004 //stackInfo->stackHeader->CheckHeader();
1005
1006 sim->stackHeaderHeap.Dealloc(otherStackHeader);
1007 }
1008 }
1009 }
1010 return true;
1011 }
1012
ResetRestOnRecords()1013 void neRigidBody_::ResetRestOnRecords()
1014 {
1015 /* for (s32 i = 0; i < NE_MAX_REST_ON; i++)
1016 {
1017 GetRestRecord(i).Init();
1018 }
1019 */
1020 }
1021
FreeStackInfo()1022 void neRigidBody_::FreeStackInfo()
1023 {
1024 ASSERT(stackInfo);
1025
1026 sim->stackInfoHeap.Dealloc(stackInfo, 1);
1027
1028 stackInfo = NULL;
1029 }
1030
NewStackInfo(neRestRecord & rc)1031 neBool neRigidBody_::NewStackInfo(neRestRecord & rc)
1032 {
1033 ASSERT(stackInfo == NULL);
1034
1035 stackInfo = sim->stackInfoHeap.Alloc(1);
1036
1037 if (!stackInfo)
1038 {
1039 if (sim->logLevel >= neSimulator::LOG_OUTPUT_LEVEL_ONE)
1040 {
1041 sprintf(sim->logBuffer, MSG_STACK_BUFFER_FULL);
1042 sim->LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
1043 }
1044 return false;
1045 }
1046
1047 stackInfo->Init();
1048
1049 {
1050 ASSERT(AllRestRecordInvalid());
1051 }
1052 ResetRestOnRecords();
1053
1054 stackInfo->body = this;
1055
1056 stackInfo->isTerminator = false;
1057
1058 AddRestContact(rc);
1059
1060 if (rc.GetOtherCollisionBody())
1061 {
1062 sim->stackHeaderX.Add(stackInfo);
1063
1064 return true;
1065 }
1066
1067 neRigidBody_ * rb = (neRigidBody_ *) rc.GetOtherBody();
1068
1069 if (!rb->stackInfo)
1070 {
1071 sim->NewStackHeader(stackInfo);
1072
1073 ASSERT(stackInfo->stackHeader);
1074
1075 return rb->NewStackInfoTerminator(stackInfo->stackHeader);
1076 }
1077
1078 neStackHeader * otherStackHeader = rb->stackInfo->stackHeader;
1079
1080 ASSERT(otherStackHeader);
1081
1082 if (otherStackHeader->isHeaderX)
1083 {
1084 sim->stackHeaderX.Remove(rb->stackInfo);
1085
1086 sim->NewStackHeader(stackInfo);
1087
1088 stackInfo->stackHeader->Add(rb->stackInfo);
1089
1090 //stackInfo->stackHeader->CheckHeader();
1091 }
1092 else
1093 {
1094 otherStackHeader->Add(stackInfo);
1095
1096 //otherStackHeader->CheckHeader();
1097 }
1098 if (!rb->_constraintHeader)
1099 rb->WakeUp();
1100
1101 rb = rc.GetOtherRigidBody();
1102
1103 if (rb && !rb->_constraintHeader)
1104 {
1105 rb->WakeUp();
1106 }
1107 return true;
1108 }
1109
NewStackInfoTerminator(neStackHeader * header)1110 neBool neRigidBody_::NewStackInfoTerminator(neStackHeader * header)
1111 {
1112 ASSERT(stackInfo == NULL);
1113
1114 stackInfo = sim->stackInfoHeap.Alloc(1);
1115
1116 ASSERT(stackInfo);
1117
1118 if (!stackInfo)
1119 {
1120 if (sim->logLevel >= neSimulator::LOG_OUTPUT_LEVEL_ONE)
1121 {
1122 sprintf(sim->logBuffer, MSG_STACK_BUFFER_FULL);
1123 sim->LogOutput(neSimulator::LOG_OUTPUT_LEVEL_ONE);
1124 }
1125 return false;
1126 }
1127 stackInfo->Init();
1128
1129 {
1130 ASSERT(AllRestRecordInvalid());
1131 }
1132
1133 ResetRestOnRecords();
1134
1135 stackInfo->body = this;
1136
1137 stackInfo->isTerminator = true;
1138
1139 header->Add(stackInfo);
1140
1141 //header->CheckHeader();
1142
1143 return true;
1144 }
1145
MigrateNewHeader(neStackHeader * newHeader,neStackHeader * curHeader)1146 void neRigidBody_::MigrateNewHeader(neStackHeader * newHeader, neStackHeader * curHeader)
1147 {
1148 ASSERT(stackInfo);
1149 ASSERT(stackInfo->stackHeader != newHeader);
1150
1151 ASSERT(curHeader == stackInfo->stackHeader);
1152
1153 neStackHeader * oldHeader = stackInfo->stackHeader;
1154
1155 oldHeader->Remove(stackInfo);
1156
1157 newHeader->Add(stackInfo);
1158
1159 // oldHeader->CheckHeader();
1160
1161 for (s32 i = 0; i < NE_RB_MAX_RESTON_RECORDS; i++)
1162 {
1163
1164
1165 if (GetRestRecord(i).GetOtherBody())
1166 {
1167 neRigidBody_* otherBody = GetRestRecord(i).GetOtherRigidBody();
1168
1169 if (otherBody)
1170 {
1171 if (!otherBody->stackInfo)
1172 continue;
1173
1174 if (otherBody->stackInfo->stackHeader == newHeader)
1175 continue;
1176
1177 if (!otherBody->stackInfo->isTerminator)
1178 otherBody->MigrateNewHeader(newHeader, curHeader);
1179 }
1180 }
1181 }
1182 }
1183
IsRestPointStillValid()1184 neBool neRigidBody_::IsRestPointStillValid()
1185 {
1186 if (!stackInfo || stackInfo->isTerminator)
1187 return false;
1188
1189 s32 count = 0;
1190
1191 switch (GetRestHull().htype)
1192 {
1193 case neRestHull::LINE:
1194 count = 2;
1195 //return false;
1196 break;
1197
1198 case neRestHull::TRIANGLE:
1199 count = 3;
1200 break;
1201 default:
1202 return false;
1203 }
1204
1205 for (s32 i = 0; i < count; i++)
1206 {
1207 neV3 world1, world2;
1208
1209 s32 j = GetRestHull().indices[i];
1210
1211 ASSERT(j < 3);
1212 ASSERT(j >= 0);
1213
1214 ASSERT(GetRestRecord(j).IsValid());
1215 //if (GetRestRecord(j).rtype != neRestRecord::REST_ON_NOT_VALID)
1216 // continue;
1217
1218 world1 = State().b2w * GetRestRecord(j).bodyPoint;
1219
1220 world2 = GetRestRecord(j).GetOtherBodyPoint();
1221
1222 neV3 diff; diff = world1 - world2;
1223
1224 //diff.RemoveComponent(sim->gravityVector); //remove the vertical component
1225
1226 f32 d = diff.Dot(diff);
1227
1228 //if (d > 0.02f) // 0.05 M or 5 cm
1229 if (d > 0.002f) // 0.05 M or 5 cm
1230 {
1231 GetRestRecord(j).SetInvalid();
1232
1233 GetRestHull().htype = neRestHull::NONE;
1234
1235 return false;
1236 }
1237 }
1238 return true;
1239 }
1240 /*
1241 void neRigidBody_::ResolveRestingPenetration()
1242 {
1243 s32 i;
1244
1245 neBool s = false;
1246
1247 for (i = 0; i < NE_RB_MAX_RESTON_RECORDS; i++)
1248 {
1249 if (!GetRestRecord(i).IsValid())
1250 continue;
1251
1252 if ((GetRestRecord(i).otherBody != sim->GetTerrainBody()) &&
1253 (!GetRestRecord(i).otherBody->IsValid() || !GetRestRecord(i).otherBody->isActive))
1254 {
1255 GetRestRecord(i).rtype = neRestRecord::REST_ON_NOT_VALID;
1256
1257 GetRestRecord(i).otherBody = NULL;
1258
1259 continue;
1260 }
1261 GetRestRecord(i).Update();
1262
1263 if (GetRestRecord(i).normalDiff >= 0.0f)
1264 continue;
1265
1266 if (neAbs(GetRestRecord(i).normalDiff) < -0.005f)
1267 continue;
1268
1269 // if (neAbs(GetRestRecord(i).normalDiff) > 0.9f)
1270 // continue;
1271
1272 s = true;
1273
1274 CorrectPenetrationDrift2(i, true, 1);
1275 }
1276 }
1277 */
1278 #if 1
1279
CorrectRotation(f32 massOther,neV3 & pointThis,neV3 & pointDest,neV3 & pointDest2,s32 flag,s32 changeLast)1280 void neRigidBody_::CorrectRotation(f32 massOther, neV3 & pointThis, neV3 & pointDest, neV3 & pointDest2, s32 flag, s32 changeLast)
1281 {
1282 neV3 dir1 = pointThis - GetPos();
1283
1284 neV3 dir2 = pointDest - GetPos();
1285
1286 f32 len1 = dir1.Length();
1287
1288 if (neIsConsiderZero(len1) || !neIsFinite(len1))
1289 return;
1290
1291 f32 len2 = dir2.Length();
1292
1293 if (neIsConsiderZero(len2) || !neIsFinite(len2))
1294 return;
1295
1296 dir1 *= (1.0f / len1);
1297
1298 dir2 *= (1.0f / len2);
1299
1300 f32 dot = dir1.Dot(dir2);
1301
1302 if (neIsConsiderZero(neAbs(dot) - 1.0f))
1303 return;
1304
1305 neV3 axis = dir1.Cross(dir2);
1306
1307 axis.Normalize();
1308
1309 f32 angle = acosf(dot);
1310
1311 neQ quat; quat.Set(angle, axis);
1312
1313 quat.Normalize();
1314
1315 if (flag == 1)
1316 //if (1)
1317 {
1318 State().q = quat * State().q;
1319
1320 UpdateDerive();
1321 }
1322 else
1323 {
1324 totalRot = quat * totalRot;
1325
1326 rotCount++;
1327
1328 if (changeLast)
1329 {
1330 totalLastRot = quat * totalLastRot;
1331
1332 lastRotCount++;
1333 }
1334 }
1335 }
1336
1337 #else
1338
CorrectRotation(f32 massOther,neV3 & pointThis,neV3 & pointDest,neV3 & pointDest2,s32 flag,s32 changeLast)1339 void neRigidBody_::CorrectRotation(f32 massOther, neV3 & pointThis, neV3 & pointDest, neV3 & pointDest2, s32 flag, s32 changeLast)
1340 {
1341 neV3 p1 = pointThis - GetPos();
1342
1343 neV3 p2 = pointDest2 - pointThis;
1344
1345 f32 dot = p1.Dot(p2);
1346
1347 neV3 cross = p1.Cross(p2);
1348
1349 f32 len = cross.Length();
1350
1351 if (!neIsFinite(len) || neIsConsiderZero(len))
1352 {
1353 return;
1354 }
1355
1356 neV3 magic; magic = Derive().Iinv * cross;
1357
1358 neV3 deltaR = (mass * massOther) / (mass + massOther) * magic;
1359
1360 f32 angle = deltaR.Length();
1361
1362 deltaR *= (1.0f / angle);
1363
1364 //if (angle > 1.0f)
1365 // angle = 1.0f;
1366
1367 neQ quat; quat.Set(angle, deltaR);
1368
1369 if (flag == 1)
1370 {
1371 State().q = quat * State().q;
1372
1373 UpdateDerive();
1374 }
1375 else
1376 {
1377 totalRot = quat * totalRot;
1378
1379 rotCount++;
1380
1381 if (changeLast)
1382 {
1383 totalLastRot = quat * totalLastRot;
1384
1385 lastRotCount++;
1386 }
1387 }
1388 }
1389
1390 #endif
1391
1392
CorrectPosition(neV3 & pointThis,neV3 & pointDest,s32 flag,s32 changeLast)1393 void neRigidBody_::CorrectPosition(neV3 & pointThis, neV3 & pointDest, s32 flag, s32 changeLast)
1394 {
1395 neV3 shift = pointDest - pointThis;
1396
1397 if (flag == 1)
1398 //if (1)
1399 {
1400 State().b2w.pos = GetPos() + shift;
1401 //SetPos(GetPos() + shift);
1402 }
1403 else
1404 {
1405 totalTrans += shift;
1406
1407 transCount++;
1408
1409 if (changeLast)
1410 {
1411 totalLastTrans += shift;
1412
1413 lastTransCount++;
1414 }
1415 }
1416 }
1417 /*
1418 void neRigidBody_::CorrectPenetrationRotation()
1419 {
1420 if (!stackInfo)
1421 return;
1422
1423 s32 i;
1424
1425 s32 deepestIndex = -1;
1426
1427 f32 deepest = 0.0f;
1428
1429 for (i = 0; i < NE_RB_MAX_RESTON_RECORDS; i++)
1430 {
1431 if (GetRestRecord(i).rtype == neRestRecord::REST_ON_NOT_VALID)
1432 continue;
1433
1434 if ((GetRestRecord(i).otherBody != sim->GetTerrainBody()) &&
1435 (!GetRestRecord(i).otherBody->IsValid() || !GetRestRecord(i).otherBody->isActive))
1436 {
1437 GetRestRecord(i).rtype = neRestRecord::REST_ON_NOT_VALID;
1438
1439 GetRestRecord(i).otherBody = NULL;
1440
1441 continue;
1442 }
1443 GetRestRecord(i).Update();
1444
1445 if (GetRestRecord(i).normalDiff >= 0.0f)
1446 continue;
1447
1448 if (GetRestRecord(i).normalDiff >= -0.005f) // never move things out completely
1449 continue;
1450
1451 if (neAbs(GetRestRecord(i).normalDiff) > 1.0f)
1452 continue;
1453
1454 CorrectPenetrationRotation2(i, false);
1455 }
1456 }
1457
1458 void neRigidBody_::CorrectPenetrationTranslation()
1459 {
1460 if (!stackInfo)
1461 return;
1462
1463 s32 i;
1464
1465 s32 deepestIndex = -1;
1466
1467 f32 deepest = 0.0f;
1468
1469 for (i = 0; i < NE_RB_MAX_RESTON_RECORDS; i++)
1470 {
1471 if (GetRestRecord(i).rtype == neRestRecord::REST_ON_NOT_VALID)
1472 continue;
1473
1474 if ((GetRestRecord(i).otherBody != sim->GetTerrainBody()) &&
1475 (!GetRestRecord(i).otherBody->IsValid() || !GetRestRecord(i).otherBody->isActive))
1476 {
1477 GetRestRecord(i).rtype = neRestRecord::REST_ON_NOT_VALID;
1478
1479 GetRestRecord(i).otherBody = NULL;
1480
1481 continue;
1482 }
1483 GetRestRecord(i).Update();
1484
1485 if (GetRestRecord(i).normalDiff >= 0.0f)
1486 continue;
1487
1488 if (GetRestRecord(i).normalDiff >= -0.005f) // never move things out completely
1489 continue;
1490
1491 if (neAbs(GetRestRecord(i).normalDiff) > 1.0f)
1492 continue;
1493
1494 CorrectPenetrationTranslation2(i, false);
1495 }
1496 }
1497 */
CorrectPenetrationRotation2(s32 index,neBool slide)1498 void neRigidBody_::CorrectPenetrationRotation2(s32 index, neBool slide)
1499 {
1500 /* neRigidBodyBase * rb = GetRestRecord(index).otherBody;
1501
1502 f32 effectiveMassA, effectiveMassB, mass2;
1503
1504 neV3 dir = GetRestRecord(index).normalWorld;
1505
1506 neV3 pointA = GetRestRecord(index).worldThisBody;
1507
1508 neV3 pointB = GetRestRecord(index).worldOtherBody;
1509
1510 f32 alinear, arotate;
1511
1512 f32 blinear, brotate;
1513
1514 effectiveMassA = TestImpulse(dir, pointA, alinear, arotate);
1515
1516 if (rb->AsRigidBody())
1517 {
1518 effectiveMassB = rb->AsRigidBody()->TestImpulse(dir * -1.0f, pointB, blinear, brotate);
1519
1520 mass2 = rb->AsRigidBody()->mass;
1521 }
1522 else
1523 {
1524 effectiveMassB = 0.0f;
1525
1526 mass2 = 1.0e6f;
1527 }
1528
1529 neV3 diff = pointA - pointB;
1530
1531 f32 dot;
1532
1533 slide = 0;
1534
1535 if (slide)
1536 {
1537 dot = diff.Dot(GetRestRecord(index).normalWorld);
1538
1539 diff = dot * GetRestRecord(index).normalWorld;
1540 }
1541
1542 f32 scale = 0.5f;
1543
1544 neV3 midA = pointA - (effectiveMassA) / (effectiveMassA + effectiveMassB) * diff * scale * arotate;
1545
1546 neV3 midB = pointB + (effectiveMassB) / (effectiveMassA + effectiveMassB) * diff * scale * brotate;
1547
1548 CorrectRotation(mass2, pointA, midA, pointB, 0, true);
1549
1550 if (rb->AsRigidBody())
1551 {
1552 rb->AsRigidBody()->CorrectRotation(mass, pointB, midB, pointA, 0, true);
1553 }
1554 */
1555 }
1556
CorrectPenetrationTranslation2(s32 index,neBool slide)1557 void neRigidBody_::CorrectPenetrationTranslation2(s32 index, neBool slide)
1558 {
1559 /*
1560 neRigidBodyBase * rb = GetRestRecord(index).otherBody;
1561
1562 f32 effectiveMassA, effectiveMassB, mass2;
1563
1564 neV3 pointA = State().b2w * GetRestRecord(index).bodyPoint;
1565
1566 neV3 pointB = rb->GetB2W() * GetRestRecord(index).otherBodyPoint;
1567
1568 neV3 dir = GetRestRecord(index).normalWorld;
1569
1570 f32 alinear, arotate;
1571
1572 f32 blinear, brotate;
1573
1574 effectiveMassA = TestImpulse(dir, pointA, alinear, arotate);
1575
1576 if (rb->AsRigidBody())
1577 {
1578 effectiveMassB = rb->AsRigidBody()->TestImpulse(dir * -1.0f, pointB, blinear, brotate);
1579
1580 mass2 = rb->AsRigidBody()->mass;
1581 }
1582 else
1583 {
1584 effectiveMassB = 0.0f;
1585
1586 mass2 = 1.0e6f;
1587 }
1588
1589 neV3 diff = pointA - pointB;
1590
1591 slide = 0;
1592
1593 if (slide)
1594 {
1595 f32 dot = diff.Dot(GetRestRecord(index).normalWorld);
1596
1597 diff = dot * GetRestRecord(index).normalWorld;
1598 }
1599
1600 neV3 midA, midB;
1601
1602 f32 scale = 0.5f;
1603
1604 if (!slide)
1605 midA = pointA - (effectiveMassA) / (effectiveMassA + effectiveMassB) * diff * scale * alinear;
1606 else
1607 midA = pointA - (effectiveMassA) / (effectiveMassA + effectiveMassB) * diff * scale * alinear;
1608
1609 midB = pointB + (effectiveMassB) / (effectiveMassA + effectiveMassB) * diff * scale * blinear;
1610
1611 CorrectPosition(pointA, midA, 0, true);
1612
1613 if (rb->AsRigidBody())
1614 rb->AsRigidBody()->CorrectPosition(pointB, midB, 0, true);
1615 */
1616 }
1617 #if 0
1618 void neRigidBody_::CorrectPenetrationDrift()
1619 {
1620 if (!stackInfo)
1621 return;
1622
1623 s32 i;
1624
1625 s32 deepestIndex = -1;
1626
1627 f32 deepest = 0.0f;
1628
1629 for (i = 0; i < NE_RB_MAX_RESTON_RECORDS; i++)
1630 {
1631 if (GetRestRecord(i).rtype == neRestRecord::REST_ON_NOT_VALID)
1632 continue;
1633
1634 if ((GetRestRecord(i).otherBody != sim->GetTerrainBody()) &&
1635 (!GetRestRecord(i).otherBody->IsValid() || !GetRestRecord(i).otherBody->isActive))
1636 {
1637 GetRestRecord(i).rtype = neRestRecord::REST_ON_NOT_VALID;
1638
1639 GetRestRecord(i).otherBody = NULL;
1640
1641 continue;
1642 }
1643 GetRestRecord(i).Update();
1644
1645 if (GetRestRecord(i).normalDiff >= 0.0f)
1646 continue;
1647
1648 if (GetRestRecord(i).normalDiff >= -0.005f) // never move things out completely
1649 continue;
1650
1651 if (neAbs(GetRestRecord(i).normalDiff) > 2.0f)
1652 continue;
1653
1654 CorrectPenetrationDrift2(i, false, 1);
1655 /*
1656 if (GetRestRecord(i).normalDiff < deepest)
1657 {
1658 deepestIndex = i;
1659
1660 deepest = GetRestRecord(i).normalDiff;
1661 }
1662 */ }
1663 // if (deepestIndex != -1)
1664 // CorrectPenetrationDrift2(deepestIndex, false);
1665 }
1666
1667 void neRigidBody_::CorrectPenetrationDrift2(s32 index, neBool slide, s32 flag)
1668 {
1669 // remember current position
1670
1671 // neV3 posA = GetPos();
1672
1673 neQ currentQ = State().q;
1674
1675 currentQ.Invert();
1676
1677 //neQ deltaQA = correctionInfo.lastQuat * currentQ;
1678
1679 // neV3 posB;
1680
1681 //neQ deltaQB;
1682
1683 /* if (GetRestRecord(index).otherBody->AsRigidBody())
1684 {
1685 posB = GetRestRecord(index).otherBody->AsRigidBody()->GetPos();
1686
1687 currentQ = GetRestRecord(index).otherBody->AsRigidBody()->State().q;
1688
1689 currentQ.Invert();
1690
1691 deltaQB = GetRestRecord(index).otherBody->AsRigidBody()->correctionInfo.lastQuat *
1692 currentQ;
1693 }
1694 */
1695 f32 err = 0.0f;
1696
1697 neRigidBodyBase * rb = GetRestRecord(index).otherBody;
1698
1699 f32 effectiveMassA, effectiveMassB, mass2;
1700
1701 neV3 dir = GetRestRecord(index).normalWorld;
1702
1703 neV3 pointA = GetRestRecord(index).worldThisBody;
1704
1705 neV3 pointB = GetRestRecord(index).worldOtherBody;
1706
1707 f32 alinear, arotate;
1708
1709 f32 blinear, brotate;
1710
1711 effectiveMassA = TestImpulse(dir, pointA, alinear, arotate);
1712
1713 if (rb->AsRigidBody())
1714 {
1715 neV3 tmp = dir * -1.0f;
1716
1717 effectiveMassB = rb->AsRigidBody()->TestImpulse(tmp, pointB, blinear, brotate);
1718
1719 mass2 = rb->AsRigidBody()->mass;
1720 }
1721 else
1722 {
1723 effectiveMassB = 0.0f;
1724
1725 mass2 = 1.0e6f;
1726
1727 brotate = 0.0f;
1728
1729 blinear = 0.0f;
1730 }
1731 //effectiveMassB = 0.0f;
1732
1733 neV3 diff = pointA - pointB;
1734
1735 f32 dot;
1736
1737 if (slide)
1738 {
1739 dot = diff.Dot(GetRestRecord(index).normalWorld);
1740
1741 diff = dot * GetRestRecord(index).normalWorld;
1742 }
1743
1744 f32 scaleA = 0.8f;
1745
1746 f32 scaleB = 0.1f;
1747
1748 neV3 midA = pointA - (effectiveMassA) / (effectiveMassA + effectiveMassB) * diff * arotate * scaleA;
1749
1750 neV3 midB = pointB + (effectiveMassB) / (effectiveMassA + effectiveMassB) * diff * brotate * scaleB;
1751
1752 CorrectRotation(mass2, pointA, midA, pointB, flag, false);
1753
1754 if (rb->AsRigidBody())
1755 {
1756 rb->AsRigidBody()->CorrectRotation(mass, pointB, midB, pointA, flag, false);
1757 }
1758 pointA = State().b2w * GetRestRecord(index).bodyPoint;
1759
1760 pointB = rb->GetB2W() * GetRestRecord(index).otherBodyPoint;
1761
1762 diff = pointA - pointB;
1763
1764 if (slide)
1765 {
1766 dot = diff.Dot(GetRestRecord(index).normalWorld);
1767
1768 diff = dot * GetRestRecord(index).normalWorld;
1769 }
1770
1771 if (!slide)
1772 midA = pointA - (effectiveMassA) / (effectiveMassA + effectiveMassB) * diff * alinear * scaleA;
1773 else
1774 midA = pointA - (effectiveMassA) / (effectiveMassA + effectiveMassB) * diff * alinear * scaleA;
1775
1776 midB = pointB + (effectiveMassB) / (effectiveMassA + effectiveMassB) * diff * blinear * scaleB;
1777
1778 CorrectPosition(pointA, midA, flag, false);
1779
1780 if (rb->AsRigidBody())
1781 rb->AsRigidBody()->CorrectPosition(pointB, midB, flag, false);
1782 /*
1783 neV3 shifted = GetPos() - posA;
1784
1785 correctionInfo.lastPos += shifted;
1786
1787 correctionInfo.lastQuat = deltaQA * State().q;
1788
1789 if (GetRestRecord(index).otherBody->AsRigidBody())
1790 {
1791 shifted = GetRestRecord(index).otherBody->AsRigidBody()->GetPos() - posB;
1792
1793 GetRestRecord(index).otherBody->AsRigidBody()->correctionInfo.lastPos += shifted;
1794
1795 GetRestRecord(index).otherBody->AsRigidBody()->correctionInfo.lastQuat = deltaQB *
1796 GetRestRecord(index).otherBody->AsRigidBody()->State().q;
1797 }
1798 */
1799 }
1800 #endif
1801
TestImpulse(neV3 & dir,neV3 & pt,f32 & linear,f32 & angular)1802 f32 neRigidBody_::TestImpulse(neV3 & dir, neV3 & pt, f32 & linear, f32 & angular)
1803 {
1804 neV3 point = pt - GetPos();
1805
1806 neV3 dv = dir * oneOnMass;
1807
1808 neV3 da = point.Cross(dir);
1809
1810 neV3 angVel = Derive().Iinv * da;
1811
1812 neV3 vel;
1813
1814 neV3 dav = angVel.Cross(point);
1815
1816 vel = dv + dav;
1817
1818 //neV3 dist = vel;// / sim->currentTimeStep;
1819
1820 f32 linearSpeed = dv.Length();
1821
1822 f32 angularSpeed = dav.Length();
1823
1824 f32 totalSpeed = linearSpeed + angularSpeed;
1825
1826 linear = linearSpeed / totalSpeed;
1827
1828 angular = angularSpeed / totalSpeed;
1829
1830 f32 ret = linearSpeed + angularSpeed;//dist.Length();
1831
1832 if (neIsFinite(ret))
1833 return ret;
1834
1835 return 0.0f;
1836 }
1837
ShiftPosition(const neV3 & delta)1838 void neRigidBody_::ShiftPosition(const neV3 & delta)
1839 {
1840 neConstraintHeader * header = GetConstraintHeader();
1841
1842 SetPos(GetPos() + delta);
1843 }
1844
AllRestRecordInvalid()1845 neBool neRigidBody_::AllRestRecordInvalid()
1846 {
1847 for (s32 i = 0; i < 3; i++)
1848 {
1849 if (GetRestRecord(i).IsValid())
1850 return false;
1851 }
1852 return true;
1853 }
1854