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 f32 CONSTRAINT_THESHOLD_JOINT = 0.000f;
28
29 f32 CONSTRAINT_THESHOLD_CONTACT = 0.001f;
30
31 f32 CONSTRAINT_THESHOLD_LIMIT = 0.001f;
32
33 f32 CONSTRAINT_CONVERGE_FACTOR_JOINT = .5f;
34
35 f32 CONSTRAINT_CONVERGE_FACTOR_CONTACT = .5f;
36
37 f32 CONSTRAINT_CONVERGE_FACTOR_LIMIT = 0.5f;
38
ApplyCollisionImpulseFast(neRigidBody_ * rb,const neV3 & impulse,const neV3 & contactPoint,s32 currentRecord,neBool immediate=true)39 NEINLINE void ApplyCollisionImpulseFast(neRigidBody_ * rb, const neV3 & impulse, const neV3 & contactPoint, s32 currentRecord, neBool immediate = true)
40 {
41 neV3 dv, da;
42
43 dv = impulse * rb->oneOnMass;
44
45 da = contactPoint.Cross(impulse);
46
47 // if (immediate)
48 rb->Derive().linearVel += dv;
49 // else
50 // {
51 // rb->totalDV += dv;
52 // rb->impulseCount++;
53 // }
54
55 // rb->dvRecord[currentRecord] += dv;
56
57 // neV3 dav = rb->Derive().Iinv * da;
58
59 // rb->davRecord[currentRecord] += dav;
60
61 // if (immediate)
62 {
63 rb->State().angularMom += da;
64
65 rb->Derive().angularVel = rb->Derive().Iinv * rb->State().angularMom;
66 }
67 // else
68 // {
69 // rb->totalDA += da;
70 //
71 // rb->twistCount++;
72 // }
73 }
74
75
AddCollisionResult(neCollisionResult & cresult)76 void neFixedTimeStepSimulator::AddCollisionResult(neCollisionResult & cresult)
77 {
78 neCollisionResult * newcr = cresultHeap2.Alloc(0);
79
80 *newcr = cresult;
81 }
82
SolveConstraint(neFixedTimeStepSimulator * sim)83 f32 neCollisionResult::SolveConstraint(neFixedTimeStepSimulator * sim)
84 {
85 neV3 impulse;
86
87 f32 ret = 0.0f;
88
89 neV3 pII;
90
91 if (neIsConsiderZero(depth))
92 {
93 pII = kInv * initRelVel;
94
95 impulse = -pII;
96 }
97 else
98 {
99 neV3 & desireVel = contactAWorld;//w2c * contactAWorld;
100
101 neV3 tmp = desireVel * CONSTRAINT_CONVERGE_FACTOR_JOINT;
102
103 f32 len = depth * CONSTRAINT_CONVERGE_FACTOR_JOINT;//tmp.Length();
104
105 if (len > 0.05f)
106 {
107 tmp = desireVel * (0.05f / len);
108 }
109 tmp *= sim->oneOnCurrentTimeStep;// * CONSTRAINT_CONVERGE_FACTOR_JOINT;
110
111 neV3 deltaU = tmp + initRelVel * CONSTRAINT_CONVERGE_FACTOR_JOINT;
112
113 impulse = kInv * deltaU * -1.0f;
114 }
115 neRigidBody_ * rb;
116
117 if (bodyA && (rb = bodyA->AsRigidBody()))
118 {
119 ApplyCollisionImpulseFast(rb, impulse, contactA, sim->currentRecord);
120
121 rb->needRecalc = true;
122 }
123 if (bodyB && (rb = bodyB->AsRigidBody()))
124 {
125 neV3 bimpulse = impulse * -1.0f;
126
127 ApplyCollisionImpulseFast(rb, bimpulse, contactB, sim->currentRecord);
128
129 rb->needRecalc = true;
130 }
131 return ret;
132 }
133
SolveSlider(neFixedTimeStepSimulator * sim)134 f32 neCollisionResult::SolveSlider(neFixedTimeStepSimulator * sim)
135 {
136 neV3 impulse;
137
138 if (neIsConsiderZero(finalRelativeSpeed))
139 {
140 impulse = kInv * initRelVel * -1.0f;
141
142 impulse = impulse - impulse.Dot(contactBWorld) * contactBWorld;
143 }
144 else
145 {
146 neV3 & desireVel = contactAWorld;
147
148 neV3 tmp = desireVel * CONSTRAINT_CONVERGE_FACTOR_JOINT;
149
150 f32 len = finalRelativeSpeed * CONSTRAINT_CONVERGE_FACTOR_JOINT;//tmp.Length();
151
152 if (len > 0.05f)
153 {
154 tmp = desireVel * (0.05f / len);
155 }
156 tmp *= sim->oneOnCurrentTimeStep;// * CONSTRAINT_CONVERGE_FACTOR_JOINT;
157
158 neV3 deltaU = tmp + initRelVel * CONSTRAINT_CONVERGE_FACTOR_JOINT;
159
160 impulse = kInv * deltaU * -1.0f;
161
162 f32 dot = impulse.Dot(contactBWorld);
163
164 impulse = impulse - dot * contactBWorld;
165 }
166 neRigidBody_ * rb;
167
168 if (bodyA && (rb = bodyA->AsRigidBody()))
169 {
170 ApplyCollisionImpulseFast(rb, impulse, contactA, sim->currentRecord);
171
172 rb->needRecalc = true;
173 }
174 if (bodyB && (rb = bodyB->AsRigidBody()))
175 {
176 neV3 bimpulse = impulse * -1.0f;
177
178 ApplyCollisionImpulseFast(rb, bimpulse, contactB, sim->currentRecord);
179
180 rb->needRecalc = true;
181 }
182 return 0.0f;
183 }
184
SolveSliderLimit(neFixedTimeStepSimulator * sim)185 f32 neCollisionResult::SolveSliderLimit(neFixedTimeStepSimulator * sim)
186 {
187 ASSERT(depth >= 0.0f);
188
189 neV3 impulse;
190
191 f32 desireSpeed = depth * sim->oneOnCurrentTimeStep * CONSTRAINT_CONVERGE_FACTOR_LIMIT;
192
193 if (desireSpeed > finalRelativeSpeed)
194 {
195 f32 dU = desireSpeed - finalRelativeSpeed * CONSTRAINT_CONVERGE_FACTOR_LIMIT;
196
197 neV3 deltaU; deltaU = contactBWorld * dU;
198
199 impulse = kInv * deltaU *-1.0f;
200 }
201 else
202 {
203 return 0.0f;
204 }
205
206 neRigidBody_ * rb;
207
208 if (bodyA && (rb = bodyA->AsRigidBody()))
209 {
210 ApplyCollisionImpulseFast(rb, impulse, contactA, sim->currentRecord);
211
212 rb->needRecalc = true;
213 }
214 if (bodyB && (rb = bodyB->AsRigidBody()) && !(sim->solverLastIteration))
215 {
216 neV3 bimpulse = impulse * -1.0f;
217
218 ApplyCollisionImpulseFast(rb, bimpulse, contactB, sim->currentRecord);
219
220 rb->needRecalc = true;
221 }
222 return 0.0f;
223 }
224
SolveContact(neFixedTimeStepSimulator * sim)225 f32 neCollisionResult::SolveContact(neFixedTimeStepSimulator * sim)
226 {
227 neV3 impulse1; impulse1.SetZero();
228 neV3 impulse2; impulse2.SetZero();
229
230 f32 ret = 0.0f;
231
232 if (initRelVel[2] < 0.0f)
233 {
234 if (sim->solverStage == 0)
235 impulse1 = sim->CalcNormalImpulse(*this, FALSE);
236 else
237 impulse1 = sim->CalcNormalImpulse(*this, TRUE);
238
239 initRelVel[2] = 0.0f;
240 }
241 if (depth > 0.05f)
242 depth = 0.05f;
243
244 f32 adjustedDepth = depth - CONSTRAINT_THESHOLD_CONTACT * sim->gravityMag;
245
246 f32 desireNormalSpeed;
247
248 if (depth <= 0.0f) // -ve mean not penetrating
249 {
250
251 }
252 else if (adjustedDepth <= 0.0f)
253 {
254 }
255 else if (sim->solverStage != 0)
256 {
257 desireNormalSpeed = adjustedDepth * sim->oneOnCurrentTimeStep * CONSTRAINT_CONVERGE_FACTOR_CONTACT;
258
259 if (desireNormalSpeed > initRelVel[2])
260 {
261 neV3 deltaU;
262
263 deltaU.Set(0.0f, 0.0f, desireNormalSpeed - initRelVel[2] * CONSTRAINT_CONVERGE_FACTOR_CONTACT);
264
265 //deltaU.Set(- initRelVel[0], - initRelVel[1], desireNormalSpeed - initRelVel[2]);
266
267 ret = neAbs(deltaU[2]);
268
269 impulse2 = kInv * deltaU;
270 }
271 }
272 neV3 impulse = impulse1 + impulse2;
273
274 impulse = collisionFrame * impulse;
275
276 neRigidBody_ * rb;
277
278 if (bodyA && (rb = bodyA->AsRigidBody()))
279 {
280 ApplyCollisionImpulseFast(rb, impulse, contactA, sim->currentRecord, true);
281
282 rb->needRecalc = true;
283 }
284 if (bodyB && (rb = bodyB->AsRigidBody()) && !(sim->solverLastIteration))
285 {
286 neV3 bimpulse = impulse * -1.0f;
287
288 ApplyCollisionImpulseFast(rb, bimpulse, contactB, sim->currentRecord, true);
289
290 rb->needRecalc = true;
291 }
292 return ret;
293 }
294
SolveAngular(f32 pdepth,const neV3 & axis,f32 relAV,neFixedTimeStepSimulator * sim)295 f32 neCollisionResult::SolveAngular(f32 pdepth, const neV3 & axis, f32 relAV, neFixedTimeStepSimulator * sim)
296 {
297 neV3 deltaL;
298
299 f32 threshold = 0.00f;
300
301 f32 angularDisplacementNeeded;
302
303 if (pdepth > threshold)
304 {
305 angularDisplacementNeeded = (pdepth - threshold);
306 }
307 else if (pdepth < -threshold)
308 {
309 angularDisplacementNeeded = (pdepth + threshold);
310 }
311 else
312 {
313 ASSERT(0);
314 return 0;
315 }
316 f32 deltaAng;
317
318 f32 scaledCorrection = angularDisplacementNeeded * CONSTRAINT_CONVERGE_FACTOR_LIMIT;
319
320 neBool applyImpulse = false;
321
322 f32 angularDisplacment;
323
324 if (scaledCorrection > 0.0f)
325 {
326 if (relAV > 0.0f)
327 {
328 //spining into the limit
329
330 deltaAng = -scaledCorrection * sim->oneOnCurrentTimeStep;
331
332 deltaAng -= (relAV * CONSTRAINT_CONVERGE_FACTOR_LIMIT);
333
334 applyImpulse = true;
335 }
336 else // relAV < 0
337 {
338 //spining out of the limit, but is it fast enough?
339 return 0.0f;
340
341 angularDisplacment = relAV * sim->_currentTimeStep;
342
343 f32 d = angularDisplacementNeeded + angularDisplacment;
344
345 if (d > 0.0f)
346 {
347 deltaAng = -d * CONSTRAINT_CONVERGE_FACTOR_LIMIT * sim->oneOnCurrentTimeStep;
348
349 applyImpulse = true;
350 }
351 }
352 }
353 else
354 {
355 if (relAV < 0.0f)
356 {
357 //spining into the limit
358 deltaAng = -scaledCorrection * sim->oneOnCurrentTimeStep;
359
360 deltaAng -= (relAV * CONSTRAINT_CONVERGE_FACTOR_LIMIT);
361
362 applyImpulse = true;
363 }
364 else // relAV > 0
365 {
366 //spining out of the limit, but is it fast enough?
367 return 0.0f;
368 angularDisplacment = relAV * sim->_currentTimeStep;
369
370 f32 d = angularDisplacementNeeded + angularDisplacment;
371
372 if (d < 0.0f)
373 {
374 deltaAng = -d * CONSTRAINT_CONVERGE_FACTOR_LIMIT * sim->oneOnCurrentTimeStep;
375
376 applyImpulse = true;
377 }
378 }
379 }
380
381 if (!applyImpulse)
382 return 0;
383
384 neV3 deltaAngVel; deltaAngVel = axis * deltaAng;
385
386 deltaL = kInv * deltaAngVel;
387
388 neRigidBody_ * rb;
389
390 if (bodyA && (rb = bodyA->AsRigidBody()))
391 {
392 rb->SetAngMom(rb->State().angularMom + deltaL);
393
394 rb->davRecord[rb->sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] += rb->Derive().Iinv * deltaL;
395
396 // rb->totalDA += deltaL;
397
398 // rb->twistCount++;
399 }
400 if (bodyB && (rb = bodyB->AsRigidBody()))
401 {
402 rb->SetAngMom(rb->State().angularMom - deltaL);
403
404 rb->davRecord[rb->sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] -= rb->Derive().Iinv * deltaL;
405
406 // rb->totalDA -= deltaL;
407
408 // rb->twistCount++;
409 }
410 return 0;
411 }
412
SolveAngular2(const neV3 & axisA,const neV3 & axisB,f32 relAV,f32 desireAV,f32 maxTorque,neFixedTimeStepSimulator * sim)413 f32 neCollisionResult::SolveAngular2(const neV3 & axisA, const neV3 & axisB, f32 relAV, f32 desireAV, f32 maxTorque, neFixedTimeStepSimulator * sim)
414 {
415 f32 deltaAng = desireAV - relAV;
416
417 //deltaAng *= 0.5f;
418
419 neV3 deltaAngVel, deltaLA, deltaLB;
420
421 deltaAngVel = axisA * deltaAng;
422
423 deltaLA = kInv * deltaAngVel;
424
425 neV3 torque = deltaLA * sim->oneOnCurrentTimeStep;
426
427 f32 torqueMag = torque.Length();
428
429 if (torqueMag > maxTorque && !neIsConsiderZero(neAbs(maxTorque)))
430 {
431 deltaLA = torque * (maxTorque * sim->_currentTimeStep / torqueMag);
432 }
433
434 neRigidBody_ * rb;
435
436 if (bodyA && (rb = bodyA->AsRigidBody()))
437 {
438 rb->SetAngMom(rb->State().angularMom + deltaLA);
439
440 rb->davRecord[rb->sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] += rb->Derive().Iinv * deltaLA;
441
442 //rb->totalDA += deltaLA;
443
444 //rb->twistCount++;
445 }
446 deltaAngVel = axisB * deltaAng;
447
448 deltaLB = kInv * deltaAngVel;
449
450 torque = deltaLB * sim->oneOnCurrentTimeStep;
451
452 torqueMag = torque.Length();
453
454 if (torqueMag > maxTorque/* && !neIsConsiderZero(maxTorque)*/)
455 {
456 deltaLB = torque * (maxTorque * sim->_currentTimeStep / torqueMag);
457 }
458 if (bodyB && (rb = bodyB->AsRigidBody()))
459 {
460 rb->SetAngMom(rb->State().angularMom - deltaLB);
461
462 rb->davRecord[rb->sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] -= rb->Derive().Iinv * deltaLB;
463
464 //rb->totalDA -= deltaLB;
465
466 //rb->twistCount++;
467 }
468 return 0.0f;
469 }
470
SolveAngular3(f32 pdepth,const neV3 & axis,f32 relAV,neFixedTimeStepSimulator * sim)471 f32 neCollisionResult::SolveAngular3(f32 pdepth, const neV3 & axis, f32 relAV, neFixedTimeStepSimulator * sim)
472 {
473 neV3 deltaL;
474
475 f32 threshold = 0.00f;
476
477 //f32 angularDisplacementNeeded = -pdepth;
478 /*
479 if (pdepth > threshold)
480 {
481 angularDisplacementNeeded = (pdepth - threshold);
482 }
483 else if (pdepth < -threshold)
484 {
485 angularDisplacementNeeded = (pdepth + threshold);
486 }
487 else
488 {
489 ASSERT(0);
490 return 0;
491 }
492 */ f32 deltaAng;
493
494 f32 scaledDepth = pdepth * CONSTRAINT_CONVERGE_FACTOR_LIMIT;
495
496 if (scaledDepth > 0.1f)
497 scaledDepth = 0.1f;
498 else if (scaledDepth < -0.1f)
499 scaledDepth = -0.1f;
500
501 neBool applyImpulse = false;
502
503 // f32 angularDisplacment;
504
505 if (scaledDepth > 0.0f)
506 {
507 if (relAV > 0.0f)
508 {
509 //spining into the limit
510
511 deltaAng = -scaledDepth * sim->oneOnCurrentTimeStep;
512
513 deltaAng -= (relAV * CONSTRAINT_CONVERGE_FACTOR_LIMIT);
514
515 applyImpulse = true;
516 }
517 else //relAV < 0.0f
518 {
519 //spining out of the limit, but is it fast enough?
520
521 /* deltaAng = -scaledDepth * sim->oneOnCurrentTimeStep;
522
523 angularDisplacment = relAV * sim->_currentTimeStep;
524
525 f32 d = pdepth + angularDisplacment;
526
527 if (d > 0.0f)
528 {
529 deltaAng = -d * CONSTRAINT_CONVERGE_FACTOR_LIMIT * sim->oneOnCurrentTimeStep;
530
531 applyImpulse = true;
532 }
533 */ }
534 }
535 else
536 {
537 if (relAV < 0.0f)
538 {
539 //spining into the limit
540 deltaAng = -scaledDepth * sim->oneOnCurrentTimeStep;
541
542 deltaAng -= (relAV * CONSTRAINT_CONVERGE_FACTOR_LIMIT);
543
544 applyImpulse = true;
545 }
546 else //relAV > 0.0f
547 {
548 //spining out of the limit, but is it fast enough?
549
550 /* angularDisplacment = relAV * sim->_currentTimeStep;
551
552 f32 d = pdepth + angularDisplacment;
553
554 if (d < 0.0f)
555 {
556 deltaAng = -d * CONSTRAINT_CONVERGE_FACTOR_LIMIT * sim->oneOnCurrentTimeStep;
557
558 applyImpulse = true;
559 }
560 */ }
561 }
562
563 if (!applyImpulse)
564 return 0;
565
566 neV3 deltaAngVel; deltaAngVel = axis * deltaAng;
567
568 deltaL = kInv * deltaAngVel;
569
570 neRigidBody_ * rb;
571
572 if (bodyA && (rb = bodyA->AsRigidBody()))
573 {
574 neV3 deltaLA = k.TransposeMulV3(deltaL);
575
576 rb->SetAngMom(rb->State().angularMom + deltaLA);
577
578 rb->davRecord[rb->sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] += rb->Derive().Iinv * deltaLA;
579
580 // rb->totalDA += deltaL;
581
582 // rb->twistCount++;
583 }
584 if (bodyB && (rb = bodyB->AsRigidBody()))
585 {
586 rb->SetAngMom(rb->State().angularMom - deltaL);
587
588 rb->davRecord[rb->sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] -= rb->Derive().Iinv * deltaL;
589
590 // rb->totalDA -= deltaL;
591
592 // rb->twistCount++;
593 }
594 return 0;
595 }
596
SolveAngularPrimary(neFixedTimeStepSimulator * sim)597 f32 neCollisionResult::SolveAngularPrimary(neFixedTimeStepSimulator * sim)
598 {
599 neRigidBody_ * ba = NULL;
600
601 neRigidBody_ * bb = NULL;
602
603 f32 ava = 0.0f, avb = 0.0f;
604
605 if (bodyA)
606 {
607 ba = bodyA->AsRigidBody();
608 }
609 if (bodyB)
610 {
611 bb = bodyB->AsRigidBody();
612 }
613 if (!ba && !bb)
614 return 0.0f;
615
616 SolveAngular(depth, contactBBody/*this is the axis*/, relativeSpeed, sim);
617
618 return 0.0f;
619 }
620
SolveAngularSecondary(neFixedTimeStepSimulator * sim)621 f32 neCollisionResult::SolveAngularSecondary(neFixedTimeStepSimulator * sim)
622 {
623 neRigidBody_ * ba = NULL;
624
625 neRigidBody_ * bb = NULL;
626
627 f32 ava = 0.0f, avb = 0.0f;
628
629 if (bodyA && (ba = bodyA->AsRigidBody()))
630 {
631 ava = ba->Derive().angularVel.Dot(contactABody);
632 }
633 if (bodyB && (bb = bodyB->AsRigidBody()))
634 {
635 avb = bb->Derive().angularVel.Dot(contactBBody);
636 }
637 if (!ba && !bb)
638 return 0.0f;
639
640 f32 relAV = ava - avb;
641
642 SolveAngular3(depth, contactBBody, relAV, sim);
643 /*
644 SolveAngular(depth, contactBBody, relAV, sim);
645
646 ava = 0.0f;
647
648 avb = 0.0f;
649
650 if (bodyA && (ba = bodyA->AsRigidBody()))
651 {
652 ava = ba->Derive().angularVel.Dot(contactABody);
653 }
654 if (bodyB && (bb = bodyB->AsRigidBody()))
655 {
656 avb = bb->Derive().angularVel.Dot(contactBBody);
657 }
658 relAV = ava - avb;
659
660 SolveAngular(impulseScale, contactABody, relAV, sim);
661 */
662 return 0.0f;
663 }
664
SolveAngularMotorPrimary(neFixedTimeStepSimulator * sim)665 f32 neCollisionResult::SolveAngularMotorPrimary(neFixedTimeStepSimulator * sim)
666 {
667 neRigidBody_ * ba = NULL;
668
669 neRigidBody_ * bb = NULL;
670
671 f32 ava = 0.0f, avb = 0.0f;
672
673 if (bodyA && (ba = bodyA->AsRigidBody()))
674 {
675 ava = ba->Derive().angularVel.Dot(contactABody);
676 }
677 if (bodyB && (bb = bodyB->AsRigidBody()))
678 {
679 avb = bb->Derive().angularVel.Dot(contactBBody);
680 }
681 if (!ba && !bb)
682 return 0.0f;
683
684 f32 relAV = ava - avb;
685
686 SolveAngular2(contactABody, contactBBody, relAV, finalRelativeSpeed, depth, sim);
687
688 return 0.0f;
689 }
690
SolveRelativeLinear(neFixedTimeStepSimulator * sim)691 f32 neCollisionResult::SolveRelativeLinear(neFixedTimeStepSimulator * sim)
692 {
693 f32 velA = 0.0f, velB = 0.0f;
694
695 neRigidBody_ * ba, * bb;
696
697 if (bodyA && (ba = bodyA->AsRigidBody()))
698 {
699 velA = ba->Derive().linearVel.Dot(contactABody);
700 }
701 if (bodyB && (bb = bodyB->AsRigidBody()))
702 {
703 velB = bb->Derive().linearVel.Dot(contactABody);
704 }
705 if (!ba && !bb)
706 return 0.0f;
707
708 f32 speedRel = velA - velB;
709
710 f32 impulseMag = kInv[0][0] * (finalRelativeSpeed - speedRel);
711
712 neV3 impulse;
713
714 f32 mag = impulseMag;
715
716 if (mag > depth)
717 {
718 impulse = depth * sim->_currentTimeStep * contactABody;
719 }
720 else if (mag < -depth)
721 {
722 impulse = -depth * sim->_currentTimeStep * contactABody;
723 }
724 else
725 {
726 impulse = impulseMag * contactABody;
727 }
728
729 neV3 zeroVector; zeroVector.SetZero();
730
731 if (ba)
732 ApplyCollisionImpulseFast(ba, impulse, zeroVector, sim->currentRecord);
733
734 impulse *= -1.f;
735
736 if (bb)
737 ApplyCollisionImpulseFast(bb, impulse, zeroVector, sim->currentRecord);
738
739 return 0.0f;
740 }
741
SolveContactConstrain()742 void neFixedTimeStepSimulator::SolveContactConstrain()
743 {
744 // first solve all single object to terrain/animated body contacts
745
746 cresultHeap2.Clear();
747
748 neStackInfoItem * sitem = (neStackInfoItem * )stackHeaderX.head;
749
750 solverStage = 1;
751
752 while (sitem)
753 {
754 neStackInfo * sinfo = (neStackInfo *)sitem;
755
756 sitem = sitem->next;
757
758 if (sinfo->body->status == neRigidBody_::NE_RBSTATUS_IDLE)
759 {
760 continue;
761 }
762 if (!sinfo->body->needSolveContactDynamic)
763 {
764 continue;
765 }
766 sinfo->body->AddContactImpulseRecord(0);
767
768 if (cresultHeap2.GetUsedCount() == 0)
769 {
770 continue;
771 }
772
773 neRigidBody_* rb = NULL;
774
775
776 for (s32 tt = 0; tt < cresultHeap2.GetUsedCount(); tt++)
777 {
778 //neCollisionResult * cr = (neCollisionResult *)ci;
779 neCollisionResult * cr = &cresultHeap2[tt];//(neCollisionResult *)ci;
780
781 HandleCollision(cr->bodyA, cr->bodyB, *cr, IMPULSE_CONTACT, 1.0f/*cr->impulseScale*/);
782
783 //ci = ci->next;
784 rb = cr->bodyA->AsRigidBody();
785 }
786 cresultHeap2.Clear();
787
788 ASSERT(rb);
789
790 if (rb->CheckStationary())
791 {
792 if (rb->IsRestPointStillValid())
793 {
794 if (rb->CheckRestHull())
795 {
796 rb->BecomeIdle();
797 }
798 }
799 }
800 }
801
802 // release any empty stack header
803
804 neStackHeaderItem * hitem = (neStackHeaderItem *)(*stackHeaderHeap.BeginUsed());
805
806 while (hitem)
807 {
808 neStackHeader * sheader = (neStackHeader *) hitem;
809
810 hitem = hitem->next;
811
812 if (sheader->infoCount > 1)
813 continue;
814
815 neStackInfo * s = sheader->head;
816
817 neRigidBody_ * rb = s->body;
818
819 if (s->isTerminator)
820 {
821 ASSERT(rb->AllRestRecordInvalid());
822
823 rb->stackInfo = NULL;
824
825 sheader->Remove(s, 1);
826
827 stackInfoHeap.Dealloc(s, 1);
828
829 stackHeaderHeap.Dealloc(sheader);
830 }
831 else
832 {
833 sheader->Remove(s, 1);
834
835 stackHeaderHeap.Dealloc(sheader);
836
837 stackHeaderX.Add(s);
838 }
839 }
840 hitem = (neStackHeaderItem *)(*stackHeaderHeap.BeginUsed());
841
842 while (hitem)
843 {
844 neStackHeader * sheader = (neStackHeader *) hitem;
845
846 hitem = hitem->next;
847
848 if (sheader->isAllIdle || sheader->dynamicSolved)
849 continue;
850
851 pointerBuffer2.Clear(); // stack headers
852
853 pointerBuffer1.Clear(); // constraint headers
854
855 contactConstraintHeader.RemoveAll();
856
857 cresultHeap2.Clear();
858
859 sheader->AddToSolver(/*true*/);
860
861 #ifdef _WIN32
862 perf->UpdateConstrain1();
863 #endif
864
865 SolveOneConstrainChain(-1.0f, 2);
866
867 #ifdef _WIN32
868 perf->UpdateConstrain2();
869 #endif
870
871 if (contactConstraintHeader.StationaryCheck())
872 {
873 //all of object are stationary enough
874 contactConstraintHeader.BecomeIdle(true);
875 }
876 }
877 contactConstraintHeader.RemoveAll();
878
879 cresultHeap2.Clear();
880 }
881
SolveLocal(neCollisionResult * cr)882 f32 neFixedTimeStepSimulator::SolveLocal(neCollisionResult * cr)
883 {
884 f32 ret = 0.0f;
885
886 neV3 velA;
887
888 neV3 velB;
889
890 neRigidBody_ * ba = NULL;
891
892 neRigidBody_ * bb = NULL;
893
894 if (cr->bodyA && cr->bodyA->AsRigidBody())
895 {
896 ba = cr->bodyA->AsRigidBody();
897 }
898 if (cr->bodyB && cr->bodyB->AsRigidBody())
899 {
900 bb = cr->bodyB->AsRigidBody();
901 }
902
903 switch (cr->impulseType)
904 {
905 case IMPULSE_CONTACT:
906 case IMPULSE_CONSTRAINT:
907 case IMPULSE_SLIDER:
908 {
909 if (ba)
910 {
911 velA = ba->VelocityAtPoint(cr->contactA);
912 }
913 else
914 {
915 velA.SetZero();
916 }
917 if (bb)
918 {
919 velB = bb->VelocityAtPoint(cr->contactB);
920 }
921 else
922 {
923 velB.SetZero();
924 }
925
926 neV3 relVel = velA - velB;
927
928 cr->initRelVelWorld = relVel;
929
930 if (cr->impulseType == IMPULSE_CONTACT)
931 cr->initRelVel = cr->w2c * relVel;
932 else if (cr->impulseType == IMPULSE_SLIDER)
933 cr->initRelVel = relVel - relVel.Dot(cr->contactBWorld) * cr->contactBWorld;
934 else
935 cr->initRelVel = relVel;
936 }
937 break;
938
939 case IMPULSE_SLIDER_LIMIT_PRIMARY:
940 {
941 if (ba)
942 {
943 velA = ba->VelocityAtPoint(cr->contactA);
944 }
945 else
946 {
947 velA.SetZero();
948 }
949 if (bb)
950 {
951 velB = bb->VelocityAtPoint(cr->contactB);
952 }
953 else
954 {
955 velB.SetZero();
956 }
957
958 neV3 relVel = velA - velB;
959
960 cr->finalRelativeSpeed = relVel.Dot(cr->contactBWorld) * -1.0f;
961
962 //cr->initRelVel = cr->finalRelativeSpeed * cr->contactBWorld;
963 }
964 break;
965
966 case IMPULSE_ANGULAR_LIMIT_PRIMARY:
967 {
968 f32 wA = 0.0f, wB = 0.0f;
969
970 if (ba)
971 {
972 wA = ba->Derive().angularVel.Dot(cr->contactBBody);
973 }
974 if (bb)
975 {
976 wB = bb->Derive().angularVel.Dot(cr->contactBBody);
977 }
978 cr->relativeSpeed = wA - wB;
979 }
980 break;
981
982 case IMPULSE_ANGULAR_LIMIT_SECONDARY:
983 {
984 //do nothing
985 }
986
987 break;
988 }
989 switch (cr->impulseType)
990 {
991 case IMPULSE_CONTACT:
992
993 ret = cr->SolveContact(this);
994
995 break;
996
997 case IMPULSE_CONSTRAINT:
998 {
999 //cr->relativeSpeed = cr->initRelVel.Length();
1000
1001 //if (!neIsConsiderZero(cr->relativeSpeed))
1002 ret = cr->SolveConstraint(this);
1003 }
1004 break;
1005
1006 case IMPULSE_SLIDER:
1007 {
1008 ret = cr->SolveSlider(this);
1009 }
1010 break;
1011 case IMPULSE_SLIDER_LIMIT_PRIMARY:
1012 {
1013 ret = cr->SolveSliderLimit(this);
1014 }
1015 break;
1016 case IMPULSE_ANGULAR_LIMIT_PRIMARY:
1017
1018 //((_neConstraint*)(cr->convexA))->limitStates[0].EnforcePrimary();
1019
1020 ret = cr->SolveAngularPrimary(this);
1021
1022 break;
1023
1024 case IMPULSE_ANGULAR_LIMIT_SECONDARY:
1025
1026 //((_neConstraint*)(cr->convexA))->limitStates[1].EnforceSecondary();
1027
1028 ret = cr->SolveAngularSecondary(this);
1029
1030 break;
1031
1032 case IMPULSE_ANGULAR_MOTOR_PRIMARY:
1033
1034 ret = cr->SolveAngularMotorPrimary(this);
1035
1036 break;
1037
1038 case IMPULSE_ANGULAR_MOTOR_SECONDARY:
1039 break;
1040
1041 case IMPULSE_RELATIVE_LINEAR_VELOCITY:
1042
1043 ret = cr->SolveRelativeLinear(this);
1044
1045 break;
1046 }
1047
1048 return ret;
1049 }
1050
CheckIfStationary()1051 void neFixedTimeStepSimulator::CheckIfStationary()
1052 {
1053 neBool allStationary = true;
1054 s32 jj;
1055
1056 for (jj = 0; jj < pointerBuffer1.GetUsedCount(); jj++) // in this loop we apply the total impulse from the
1057 // solving stage to the rigid bodies
1058 {
1059 neConstraintHeader * ch = (neConstraintHeader*)pointerBuffer1[jj];
1060
1061 if (!ch->StationaryCheck())
1062 {
1063 allStationary = FALSE;
1064 }
1065 }// next jj, next constraint
1066
1067 if (!contactConstraintHeader.StationaryCheck())
1068 {
1069 allStationary = FALSE;
1070 }
1071 if (allStationary)
1072 {
1073 //make everything idle
1074 for (jj = 0; jj < pointerBuffer1.GetUsedCount(); jj++) // in this loop we apply the total impulse from the
1075 // solving stage to the rigid bodies
1076 {
1077 neConstraintHeader * ch = (neConstraintHeader*)pointerBuffer1[jj];
1078
1079 ch->BecomeIdle();
1080 }// next jj, next constraint
1081
1082 contactConstraintHeader.BecomeIdle();
1083 }
1084 else
1085 {
1086 //make everything idle
1087 for (jj = 0; jj < pointerBuffer1.GetUsedCount(); jj++) // in this loop we apply the total impulse from the
1088 // solving stage to the rigid bodies
1089 {
1090 neConstraintHeader * ch = (neConstraintHeader*)pointerBuffer1[jj];
1091
1092 ch->WakeUp();
1093 }// next jj, next constraint
1094
1095 contactConstraintHeader.WakeUp();
1096 }
1097 }
1098
1099 /****************************************************************************
1100 *
1101 * neFixedTimeStepSimulator::ResolvePenetration
1102 *
1103 ****************************************************************************/
ResolvePenetration()1104 void neFixedTimeStepSimulator::ResolvePenetration()
1105 {
1106 //CheckStackHeader();
1107
1108 neStackInfoItem * sitem = (neStackInfoItem * )stackHeaderX.head;
1109 /*
1110 while (sitem)
1111 {
1112 neStackInfo * sinfo = (neStackInfo *)sitem;
1113
1114 sitem = sitem->next;
1115
1116 if (sinfo->body->status != neRigidBody_::NE_RBSTATUS_IDLE || sinfo->body->isShifted)
1117 sinfo->Resolve();
1118 }
1119 */
1120 neSimpleArray<neStackHeader*> & activeHeaderBuffer = *((neSimpleArray<neStackHeader*>*)&pointerBuffer1);
1121
1122 activeHeaderBuffer.Clear();
1123
1124 // check if any of the stacks are all idle
1125
1126 neStackHeaderItem * hitem = (neStackHeaderItem *)(*stackHeaderHeap.BeginUsed());
1127
1128 while (hitem)
1129 {
1130 neStackHeader * sheader = (neStackHeader *) hitem;
1131
1132 sheader->isAllIdle = true;
1133
1134 neStackInfoItem * sitem = (neStackInfoItem *)sheader->head;
1135
1136 while (sitem)
1137 {
1138 neStackInfo * sinfo = (neStackInfo*) sitem;
1139
1140 sitem = sitem->next;
1141
1142 if (sinfo->body->status != neRigidBody_::NE_RBSTATUS_IDLE || sinfo->body->isShifted)
1143 {
1144 sheader->isAllIdle = false;
1145
1146 break;
1147 }
1148 }
1149
1150 hitem = hitem->next;
1151 }
1152
1153 //Resolve penetration
1154
1155 hitem = (neStackHeaderItem *)(*stackHeaderHeap.BeginUsed());
1156
1157 while (hitem)
1158 {
1159 neStackHeader * sheader = (neStackHeader *) hitem;
1160
1161 //sheader->CheckHeader();
1162
1163 if (!sheader->isAllIdle)
1164 {
1165 //if (!sheader->dynamicSolved)
1166 // sheader->Resolve();
1167
1168 *activeHeaderBuffer.Alloc() = sheader;
1169 }
1170 hitem = hitem->next;
1171 }
1172
1173 // check if any of the rest contact are still valid
1174
1175 neRigidBody_ * rb;
1176
1177 neList<neRigidBody_> * activeList;
1178
1179 for (s32 j = 0; j < 2; j++)
1180 {
1181 if (j == 0)
1182 activeList = &activeRB;
1183 else
1184 activeList = &activeRP;
1185
1186 rb = activeList->GetHead();
1187
1188 while (rb)
1189 {
1190 //if (((s32)rb->id % 3) != cc)
1191 //{
1192 // rb = activeRB.GetNext(rb);
1193 // continue;
1194 //}
1195 if (!rb->stackInfo)
1196 {
1197 rb = activeList->GetNext(rb);
1198 continue;
1199 }
1200 if (rb->stackInfo->isTerminator)
1201 {
1202 rb = activeList->GetNext(rb);
1203 continue;
1204 }
1205 s32 v;
1206
1207 v = rb->CheckContactValidity();
1208
1209 rb->isShifted = false;
1210 if (v == 0)
1211 {
1212 /* char ss[256];
1213 sprintf(ss, "%d disconnected\n", rb->id);
1214 OutputDebugString(ss);
1215 */ }
1216 if (v == 0)
1217 {
1218 if (rb->stackInfo->stackHeader->isHeaderX)
1219 {
1220 stackHeaderX.Remove(rb->stackInfo);
1221 /*
1222 char ss[256];
1223 sprintf(ss, "disconnect %d from ground\n", rb->id);
1224 OutputDebugString(ss);
1225 */
1226 stackInfoHeap.Dealloc(rb->stackInfo, 1);
1227
1228 ASSERT(rb->AllRestRecordInvalid());
1229
1230 rb->stackInfo = NULL;
1231 }
1232 }
1233
1234 if (v <= 1) //no longer resting
1235 {
1236 if (rb->status == neRigidBody_::NE_RBSTATUS_IDLE && rb->_constraintHeader == NULL)
1237 {
1238 rb->WakeUp();
1239 }
1240 }
1241 rb = activeList->GetNext(rb);
1242 }
1243 }
1244
1245 /*
1246 hitem = (neStackHeaderItem *)(*stackHeaderHeap.BeginUsed());
1247
1248 while (hitem)
1249 {
1250 neStackHeader * sheader = (neStackHeader *) hitem;
1251
1252 sheader->CheckHeader();
1253
1254 hitem = hitem->next;
1255 }
1256
1257 CheckStackHeader();
1258 */
1259
1260 // check if the stack set is disconnected
1261
1262 s32 thisFrame = stepSoFar % 5; //check stack disconnection once every 3 frames
1263
1264 for (s32 i = 0; i < activeHeaderBuffer.GetUsedCount(); i++)
1265 {
1266 s32 g = i % 5;
1267
1268 if (g == thisFrame)
1269 {
1270 if (!activeHeaderBuffer[i]->isAllIdle)
1271 {
1272 activeHeaderBuffer[i]->CheckStackDisconnected();
1273
1274 stackHeaderHeap.Dealloc(activeHeaderBuffer[i]);
1275 }
1276 }
1277 }
1278
1279 /*
1280 hitem = (neStackHeaderItem *)(*stackHeaderHeap.BeginUsed());
1281
1282 while (hitem)
1283 {
1284 neStackHeader * sheader = (neStackHeader *) hitem;
1285
1286 sheader->CheckHeader();
1287
1288 hitem = hitem->next;
1289 }
1290 */
1291 // apply collision impulse
1292
1293 // check for empty stack sets
1294 hitem = (neStackHeaderItem *)(*stackHeaderHeap.BeginUsed());
1295
1296 while (hitem)
1297 {
1298 neStackHeader * h = (neStackHeader *) hitem;
1299
1300 if (h->infoCount == 0)
1301 {
1302 hitem = hitem->next;
1303
1304 stackHeaderHeap.Dealloc(h);
1305 }
1306 else
1307 {
1308 hitem = hitem->next;
1309 }
1310 }
1311
1312 /*
1313 hitem = (neStackHeaderItem *)(*stackHeaderHeap.BeginUsed());
1314
1315 while (hitem)
1316 {
1317 neStackHeader * sheader = (neStackHeader *) hitem;
1318
1319 sheader->CheckHeader();
1320
1321 hitem = hitem->next;
1322 }
1323 */
1324 }
1325
1326 /****************************************************************************
1327 *
1328 * neFixedTimeStepSimulator::HandleCollision
1329 *
1330 ****************************************************************************/
1331
CalcNormalImpulse(neCollisionResult & cresult,neBool isContact)1332 neV3 neFixedTimeStepSimulator::CalcNormalImpulse(neCollisionResult & cresult, neBool isContact)
1333 {
1334 neV3 pI, pII, impulse;
1335
1336 pI.Set(0.0f, 0.0f, -cresult.initRelVel[2] / cresult.k[2][2]);
1337
1338 pII = cresult.kInv * cresult.initRelVel;
1339
1340 pII *= -1.0f;
1341
1342 neV3 pDiff = pII - pI;
1343
1344 f32 e = 0.0f;
1345 f32 et = 1.0f;
1346 f32 u = 0.0f;
1347
1348 f32 eA, uA, eB, uB, den;
1349
1350 GetMaterial(cresult.materialIdA, uA, eA, den);
1351
1352 GetMaterial(cresult.materialIdB, uB, eB, den);
1353
1354 if (uA < uB)
1355 u = uA;
1356 else
1357 u = uB;
1358
1359 if (isContact)
1360 {
1361 e = 0.0f;
1362
1363 //u *= 1.2f;
1364
1365 //if (u > 1.0f)
1366 // u = 1.0f;
1367 }
1368 else
1369 {
1370 //e = (eA + eB) * 0.5f;
1371 if (eA < eB)
1372 e = eA;
1373 else
1374 e = eB;
1375 }
1376
1377 et = 0.0f;//u * 2.0f - 1.0f;
1378
1379 neV3 candidate = pI * (1.0f + e);
1380
1381 candidate += (1.0f + et) * pDiff;
1382
1383 f32 t1 = candidate[0] * candidate[0] + candidate[1] * candidate[1];
1384
1385 f32 t2 = u * candidate[2];
1386
1387 t2 = t2 * t2;
1388
1389 if (t1 > t2)
1390 {
1391 f32 kap,
1392 temp;
1393
1394 kap = u * (1.0f + e) * pI[2];
1395
1396 neV3 tempv; tempv.Set( pII[0], pII[1], 0.0f );
1397
1398 temp = sqrtf(tempv.Dot(tempv ));
1399
1400 temp -= u * pDiff[2];
1401
1402 if ( neAbs( temp ) > NE_ZERO )
1403 {
1404 kap /= temp;
1405
1406 impulse = pI * (1.0f + e);
1407
1408 tempv = pDiff * kap;
1409
1410 impulse = impulse + tempv;
1411 }
1412 else
1413 {
1414 impulse.SetZero();
1415 }
1416 }
1417 else
1418 {
1419 impulse = candidate;
1420 }
1421
1422 return impulse;
1423 }
1424
HandleCollision(neRigidBodyBase * bodyA,neRigidBodyBase * bodyB,neCollisionResult & cresult,neImpulseType impulseType,f32 scale)1425 f32 neFixedTimeStepSimulator::HandleCollision(neRigidBodyBase * bodyA,
1426 neRigidBodyBase * bodyB,
1427 neCollisionResult & cresult, neImpulseType impulseType,
1428 f32 scale)
1429 {
1430 // ASSERT(impulseType == IMPULSE_NORMAL);
1431
1432 neM3 w2c;
1433
1434 w2c.SetTranspose(cresult.collisionFrame);
1435
1436 ASSERT(w2c.IsFinite());
1437
1438 neV3 velA;
1439
1440 neV3 velB;
1441
1442 neRigidBody_ * ba = NULL;
1443
1444 neRigidBody_ * bb = NULL;
1445
1446 if (bodyA && bodyA->AsRigidBody())
1447 {
1448 ba = bodyA->AsRigidBody();
1449 }
1450 if (bodyB && bodyB->AsRigidBody())
1451 {
1452 bb = bodyB->AsRigidBody();
1453 }
1454
1455 if (!ba && !bb)
1456 return 0;
1457
1458 if (ba)
1459 {
1460 velA = ba->VelocityAtPoint(cresult.contactA);
1461 }
1462 else
1463 {
1464 velA.SetZero();
1465 }
1466 if (bb)
1467 {
1468 velB = bb->VelocityAtPoint(cresult.contactB);
1469 }
1470 else
1471 {
1472 velB.SetZero();
1473 }
1474
1475 neV3 relVel = velA - velB;
1476
1477 cresult.initRelVelWorld = relVel;
1478
1479 cresult.initRelVel = w2c * relVel;
1480
1481 f32 initSpeed = cresult.relativeSpeed = cresult.initRelVel.Length();
1482
1483 f32 theshold = -1.0f;
1484
1485 // neM3 k, kInv;
1486
1487 neV3 impulse;
1488
1489 f32 ret = 0.0f;
1490
1491 switch (impulseType)
1492 {
1493 case IMPULSE_NORMAL:
1494 {
1495 if (cresult.initRelVel[2] >= 0.0f)
1496 {
1497 return 0.0f;
1498 }
1499
1500 impulse = CalcNormalImpulse(cresult, FALSE);
1501 }
1502 break;
1503
1504 case IMPULSE_CONTACT:
1505 {
1506 cresult.SolveContact(this);
1507 return 0.0f;
1508 }
1509 break;
1510
1511 default:
1512 ASSERT(0);
1513 break;
1514 }
1515
1516 if (!impulse.IsFinite())
1517 {
1518 //ASSERT(0);
1519
1520 return 0.0f;
1521 }
1522
1523 impulse = cresult.collisionFrame * impulse * scale;
1524
1525 neV3 bimpulse;
1526
1527 if (impulseType == IMPULSE_NORMAL)
1528 {
1529 neBool doBreakCheck = (impulseType == IMPULSE_NORMAL && breakageCallback);
1530
1531 if (doBreakCheck)
1532 {
1533 if (!ba) // meaning either ca and bb
1534 {
1535 if (cresult.convexA->breakInfo.flag != neGeometry::NE_BREAK_DISABLE)
1536 {
1537 CheckBreakage(bodyA, cresult.convexA, cresult.contactAWorld, impulse);
1538 }
1539 bimpulse = impulse * -1.0f;
1540
1541 if (cresult.convexB->breakInfo.flag != neGeometry::NE_BREAK_DISABLE)
1542 {
1543 CheckBreakage(bb, cresult.convexB, cresult.contactBWorld, bimpulse);
1544 }
1545 bb->ApplyCollisionImpulse(bimpulse, cresult.contactB, impulseType);
1546
1547 bb->needRecalc = true;
1548 }
1549 else // meaning ba and bb, or ba and cb
1550 {
1551 if (!bb) //ba and cb
1552 {
1553 if (cresult.convexB->breakInfo.flag != neGeometry::NE_BREAK_DISABLE)
1554 {
1555 bimpulse = impulse * -1.0f;
1556
1557 CheckBreakage(bodyB, cresult.convexB, cresult.contactBWorld, bimpulse);
1558
1559 impulse = bimpulse * -1.0f;
1560 }
1561 if (cresult.convexA->breakInfo.flag != neGeometry::NE_BREAK_DISABLE)
1562 {
1563 CheckBreakage(ba, cresult.convexA, cresult.contactAWorld, impulse);
1564 }
1565 ba->ApplyCollisionImpulse(impulse, cresult.contactA, impulseType);
1566
1567 ba->needRecalc = true;
1568 }
1569 else // meaning ba and bb
1570 {
1571 if (cresult.convexA->breakInfo.flag != neGeometry::NE_BREAK_DISABLE)
1572 {
1573 CheckBreakage(ba, cresult.convexA, cresult.contactAWorld, impulse);
1574 }
1575 bimpulse = impulse * -1.0f;
1576
1577 if (cresult.convexB->breakInfo.flag != neGeometry::NE_BREAK_DISABLE)
1578 {
1579 CheckBreakage(bb, cresult.convexB, cresult.contactBWorld, bimpulse);
1580 }
1581 ba->ApplyCollisionImpulse(impulse, cresult.contactA, impulseType);
1582
1583 ba->needRecalc = true;
1584
1585 bb->ApplyCollisionImpulse(bimpulse, cresult.contactB, impulseType);
1586
1587 bb->needRecalc = true;
1588 }
1589 }
1590 }
1591 else
1592 {
1593 if (ba)
1594 {
1595 ba->ApplyCollisionImpulse(impulse, cresult.contactA, impulseType);
1596
1597 ba->needRecalc = true;
1598 }
1599 if (bb)
1600 {
1601 bimpulse = impulse * -1.0f;
1602
1603 bb->ApplyCollisionImpulse(bimpulse, cresult.contactB, impulseType);
1604
1605 bb->needRecalc = true;
1606 }
1607 }
1608 }
1609 else
1610 {
1611 if (ba)
1612 {
1613 ba->ApplyCollisionImpulse(impulse, cresult.contactA, impulseType);
1614
1615 ba->needRecalc = true;
1616 }
1617 if (bb)
1618 {
1619 bimpulse = impulse * -1.0f;
1620
1621 bb->ApplyCollisionImpulse(bimpulse, cresult.contactB, impulseType);
1622
1623 bb->needRecalc = true;
1624 }
1625 }
1626 return ret;
1627 }
1628
1629 #define AUTO_SLEEP_ON
1630
SolveAllConstrain()1631 void neFixedTimeStepSimulator::SolveAllConstrain()
1632 {
1633 if (constraintHeaders.GetUsedCount() == 0)
1634 return;
1635
1636 neDLinkList<neConstraintHeader>::iterator chiter;
1637
1638 for (chiter = constraintHeaders.BeginUsed(); chiter.Valid(); chiter++)
1639 {
1640 (*chiter)->solved = false;
1641 }
1642
1643 for (chiter = constraintHeaders.BeginUsed(); chiter.Valid(); chiter++)
1644 {
1645 if ((*chiter)->solved)
1646 continue;
1647
1648 pointerBuffer2.Clear(); // stack headers
1649
1650 pointerBuffer1.Clear(); // constraint headers
1651
1652 contactConstraintHeader.RemoveAll();
1653
1654 cresultHeap.Clear();
1655
1656 cresultHeap2.Clear();
1657
1658 neByte ** pt = pointerBuffer1.Alloc();
1659
1660 *pt = (neByte*)(*chiter);
1661
1662 neBodyHandle * bodyHandle = (*chiter)->bodies.GetHead();
1663
1664 s32 allIdle = 0;
1665 s32 nbody = 0;
1666 neRigidBody_ * lastrb = NULL;
1667
1668 while (bodyHandle)
1669 {
1670 neRigidBody_ * rb = bodyHandle->thing->AsRigidBody();
1671
1672 if (rb)
1673 {
1674 rb->maxErrCResult = NULL;
1675
1676 nbody++;
1677 }
1678
1679 if (rb && rb->status != neRigidBody_::NE_RBSTATUS_IDLE)
1680 {
1681 allIdle++;
1682 }
1683 lastrb = rb;
1684 bodyHandle = bodyHandle->next;
1685 }
1686 if (allIdle == 0)
1687 continue;
1688
1689 //lastrb->WakeUpAllJoint();
1690
1691 f32 epsilon = -1.0f;//DEFAULT_CONSTRAINT_EPSILON;
1692
1693 s32 iteration = -1;
1694
1695 (*chiter)->AddToSolver(epsilon, iteration); // pointerBuffer2 will be filled after this call
1696
1697 AddContactConstraint(epsilon, iteration);
1698
1699 SolveOneConstrainChain(epsilon, iteration);
1700
1701 CheckIfStationary();
1702 }
1703 contactConstraintHeader.RemoveAll();
1704 }
1705
SolveOneConstrainChain(f32 epsilon,s32 iteration)1706 void neFixedTimeStepSimulator::SolveOneConstrainChain(f32 epsilon, s32 iteration)
1707 {
1708 solverStage = 0;
1709
1710 if (cresultHeap.GetUsedCount() == 0 && cresultHeap2.GetUsedCount() == 0)
1711 {
1712 return;
1713 }
1714
1715 if (epsilon == -1.0f)
1716 epsilon = DEFAULT_CONSTRAINT_EPSILON;
1717
1718 if (iteration == -1)
1719 {
1720 iteration = (s32) (DEFAULT_CONSTRAINT_ITERATION);// * cresultHeap.GetUsedCount());
1721
1722 if (iteration == 0)
1723 iteration = 1;
1724 }
1725 // pp == 0 is friction stage
1726 // pp == 1 is penetration stage
1727
1728
1729 s32 checkSleep = iteration >> 1;
1730
1731 solverLastIteration = false;
1732
1733 for (s32 pp = 0; pp < 2; pp++)
1734 {
1735 if (pp == 1)
1736 {
1737 solverStage = 1;
1738 /*
1739 for (s32 tt = 0; tt < cresultHeap2.GetUsedCount(); tt++)
1740 {
1741 neCollisionResult * cr = &cresultHeap2[tt];
1742
1743 ASSERT(cr->impulseType == IMPULSE_CONTACT);
1744
1745 cr->StartStage2();
1746 }
1747 */ }
1748 s32 it;
1749
1750 it = iteration;
1751
1752 for (s32 i = 0; i < it; i++)
1753 {
1754 neBool doCheckSleep = false;
1755
1756 if (pp == 1 && i == checkSleep)
1757 {
1758 doCheckSleep = true;
1759 }
1760 if (pp == 1 && i == (it -1))
1761 {
1762 solverLastIteration = true;
1763 }
1764 f32 maxError = 0.0f;
1765
1766 s32 nConstraint = 0;
1767
1768 neCollisionResult * cresult = &cresultHeap[0]; //*cresultHeap.BeginUsed();
1769
1770 s32 tt;
1771
1772 for (tt = 0; tt < cresultHeap.GetUsedCount(); tt++)
1773 {
1774 neCollisionResult * cr = &cresultHeap[tt];
1775
1776 f32 err = 0.0f;
1777
1778 err = SolveLocal(cr);
1779
1780 if (err > maxError)
1781 maxError = err;
1782 }
1783 //for (tt = 0; tt < cresultHeap2.GetUsedCount(); tt++)
1784 for (tt = cresultHeap2.GetUsedCount()-1; tt >= 0 ; tt--)
1785 {
1786 neCollisionResult * cr = &cresultHeap2[tt];
1787
1788 f32 err = 0.0f;
1789
1790 err = SolveLocal(cr);
1791
1792 if (err > maxError)
1793 maxError = err;
1794 }
1795 //maxError = 0.0f;
1796
1797 s32 jj;
1798
1799 for (jj = 0; jj < pointerBuffer1.GetUsedCount(); jj++) // in this loop we apply the total impulse from the
1800 // solving stage to the rigid bodies
1801 {
1802 neConstraintHeader * ch = (neConstraintHeader*)pointerBuffer1[jj];
1803
1804 ch->TraverseApplyConstraint(doCheckSleep);
1805 }// next jj, next constraint
1806
1807 contactConstraintHeader.TraverseApplyConstraint(doCheckSleep);
1808
1809 #if 1//AUTO_SLEEP_ON
1810
1811 //if (doCheckSleep)
1812 if (pp == 1 && i == (it - 2)) // the second last iteration
1813 {
1814 for (tt = 0; tt < cresultHeap2.GetUsedCount(); tt++)
1815 {
1816 neCollisionResult * cr = &cresultHeap2[tt];
1817
1818 if (cr->impulseType == IMPULSE_CONTACT)
1819 {
1820 cr->PrepareForSolver(false, true);
1821 }
1822
1823 }
1824 }
1825 #endif //AUTO_SLEEP_ON
1826
1827 }
1828 }
1829
1830 cresultHeap.Clear();
1831
1832 cresultHeap2.Clear();
1833
1834 ASSERT(cresultHeap.GetUsedCount() == 0);
1835 }
AddContactConstraint(f32 & epsilon,s32 & iteration)1836 void neFixedTimeStepSimulator::AddContactConstraint(f32 & epsilon, s32 & iteration)
1837 {
1838 for (s32 i = 0; i < pointerBuffer2.GetUsedCount(); i++)
1839 {
1840 neStackHeader * sheader = (neStackHeader *) pointerBuffer2[i];
1841
1842 sheader->AddToSolver(/*true*/);
1843
1844 neStackInfoItem * sitem = (neStackInfoItem *)sheader->head;
1845
1846 while (sitem)
1847 {
1848 neStackInfo * sinfo = (neStackInfo*) sitem;
1849
1850 sitem = sitem->next;
1851
1852 if (sinfo->body->_constraintHeader &&
1853 !sinfo->body->_constraintHeader->solved)
1854 {
1855 sinfo->body->_constraintHeader->AddToSolver(epsilon, iteration);
1856
1857 *pointerBuffer1.Alloc() = (neByte *)(sinfo->body->_constraintHeader);
1858 }
1859 }
1860 }
1861 }
1862
1863 /****************************************************************************
1864 *
1865 * ChooseAxis
1866 *
1867 ****************************************************************************/
1868
ChooseAxis(neV3 & x,neV3 & y,const neV3 & normal)1869 void ChooseAxis(neV3 & x, neV3 & y, const neV3 & normal)
1870 {
1871 neV3 mag;
1872
1873 mag[0] = neAbs(normal[0]);
1874 mag[1] = neAbs(normal[1]);
1875 mag[2] = neAbs(normal[2]);
1876
1877 if (mag[0] > mag[1])
1878 {
1879 if (mag[0] > mag[2])
1880 {
1881 x[0] = (normal[1] + normal[2]) / normal[0] * -1.0f;
1882 x[1] = 1.0f;
1883 x[2] = 1.0f;
1884 }
1885 else
1886 {
1887 x[2] = (normal[0] + normal[1]) / normal[2] * -1.0f;
1888 x[0] = 1.0f;
1889 x[1] = 1.0f;
1890 }
1891 }
1892 else if (mag[1] > mag[2])
1893 {
1894 x[1] = (normal[0] + normal[2]) / normal[1] * -1.0f;
1895 x[0] = 1.0f;
1896 x[2] = 1.0f;
1897 }
1898 else
1899 {
1900 x[2] = (normal[0] + normal[1]) / normal[2] * -1.0f;
1901 x[0] = 1.0f;
1902 x[1] = 1.0f;
1903 }
1904 x.Normalize();
1905 y = normal.Cross(x);
1906 }
1907