OLD | NEW |
1 /******************************************************************************* | 1 /******************************************************************************* |
2 * Copyright (c) 2015, Daniel Murphy, Google | 2 * Copyright (c) 2015, Daniel Murphy, Google |
3 * All rights reserved. | 3 * All rights reserved. |
4 * | 4 * |
5 * Redistribution and use in source and binary forms, with or without modificati
on, | 5 * Redistribution and use in source and binary forms, with or without modificati
on, |
6 * are permitted provided that the following conditions are met: | 6 * are permitted provided that the following conditions are met: |
7 * * Redistributions of source code must retain the above copyright notice, | 7 * * Redistributions of source code must retain the above copyright notice, |
8 * this list of conditions and the following disclaimer. | 8 * this list of conditions and the following disclaimer. |
9 * * Redistributions in binary form must reproduce the above copyright notice, | 9 * * Redistributions in binary form must reproduce the above copyright notice, |
10 * this list of conditions and the following disclaimer in the documentation | 10 * this list of conditions and the following disclaimer in the documentation |
(...skipping 466 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
477 | 477 |
478 if (data.step.warmStarting) { | 478 if (data.step.warmStarting) { |
479 // Account for variable time step. | 479 // Account for variable time step. |
480 _impulse.scale(data.step.dtRatio); | 480 _impulse.scale(data.step.dtRatio); |
481 _motorImpulse *= data.step.dtRatio; | 481 _motorImpulse *= data.step.dtRatio; |
482 | 482 |
483 final Vector2 P = pool.popVec2(); | 483 final Vector2 P = pool.popVec2(); |
484 temp.setFrom(_axis).scale(_motorImpulse + _impulse.z); | 484 temp.setFrom(_axis).scale(_motorImpulse + _impulse.z); |
485 P.setFrom(_perp).scale(_impulse.x).add(temp); | 485 P.setFrom(_perp).scale(_impulse.x).add(temp); |
486 | 486 |
487 double LA = _impulse.x * _s1 + | 487 double LA = |
488 _impulse.y + | 488 _impulse.x * _s1 + _impulse.y + (_motorImpulse + _impulse.z) * _a1; |
489 (_motorImpulse + _impulse.z) * _a1; | 489 double LB = |
490 double LB = _impulse.x * _s2 + | 490 _impulse.x * _s2 + _impulse.y + (_motorImpulse + _impulse.z) * _a2; |
491 _impulse.y + | |
492 (_motorImpulse + _impulse.z) * _a2; | |
493 | 491 |
494 vA.x -= mA * P.x; | 492 vA.x -= mA * P.x; |
495 vA.y -= mA * P.y; | 493 vA.y -= mA * P.y; |
496 wA -= iA * LA; | 494 wA -= iA * LA; |
497 | 495 |
498 vB.x += mB * P.x; | 496 vB.x += mB * P.x; |
499 vB.y += mB * P.y; | 497 vB.y += mB * P.y; |
500 wB += iB * LB; | 498 wB += iB * LB; |
501 | 499 |
502 pool.pushVec2(1); | 500 pool.pushVec2(1); |
(...skipping 79 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
582 _impulse.z = Math.max(_impulse.z, 0.0); | 580 _impulse.z = Math.max(_impulse.z, 0.0); |
583 } else if (_limitState == LimitState.AT_UPPER) { | 581 } else if (_limitState == LimitState.AT_UPPER) { |
584 _impulse.z = Math.min(_impulse.z, 0.0); | 582 _impulse.z = Math.min(_impulse.z, 0.0); |
585 } | 583 } |
586 | 584 |
587 // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + | 585 // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + |
588 // f1(1:2) | 586 // f1(1:2) |
589 final Vector2 b = pool.popVec2(); | 587 final Vector2 b = pool.popVec2(); |
590 final Vector2 f2r = pool.popVec2(); | 588 final Vector2 f2r = pool.popVec2(); |
591 | 589 |
592 temp | 590 temp.setValues(_K.entry(0, 2), _K.entry(1, 2)).scale(_impulse.z - f1.z); |
593 .setValues(_K.entry(0, 2), _K.entry(1, 2)) | |
594 .scale(_impulse.z - f1.z); | |
595 b.setFrom(Cdot1).negate().sub(temp); | 591 b.setFrom(Cdot1).negate().sub(temp); |
596 | 592 |
597 Matrix3.solve2(_K, f2r, b); | 593 Matrix3.solve2(_K, f2r, b); |
598 f2r.add(new Vector2(f1.x, f1.y)); | 594 f2r.add(new Vector2(f1.x, f1.y)); |
599 _impulse.x = f2r.x; | 595 _impulse.x = f2r.x; |
600 _impulse.y = f2r.y; | 596 _impulse.y = f2r.y; |
601 | 597 |
602 df.setFrom(_impulse).sub(f1); | 598 df.setFrom(_impulse).sub(f1); |
603 | 599 |
604 final Vector2 P = pool.popVec2(); | 600 final Vector2 P = pool.popVec2(); |
(...skipping 66 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
671 | 667 |
672 qA.setAngle(aA); | 668 qA.setAngle(aA); |
673 qB.setAngle(aB); | 669 qB.setAngle(aB); |
674 | 670 |
675 double mA = _invMassA, | 671 double mA = _invMassA, |
676 mB = _invMassB; | 672 mB = _invMassB; |
677 double iA = _invIA, | 673 double iA = _invIA, |
678 iB = _invIB; | 674 iB = _invIB; |
679 | 675 |
680 // Compute fresh Jacobians | 676 // Compute fresh Jacobians |
681 Rot.mulToOutUnsafe( | 677 Rot.mulToOutUnsafe(qA, temp.setFrom(_localAnchorA).sub(_localCenterA), rA); |
682 qA, temp.setFrom(_localAnchorA).sub(_localCenterA), rA); | 678 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), rB); |
683 Rot.mulToOutUnsafe( | |
684 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), rB); | |
685 d.setFrom(cB).add(rB).sub(cA).sub(rA); | 679 d.setFrom(cB).add(rB).sub(cA).sub(rA); |
686 | 680 |
687 Rot.mulToOutUnsafe(qA, _localXAxisA, axis); | 681 Rot.mulToOutUnsafe(qA, _localXAxisA, axis); |
688 double a1 = temp.setFrom(d).add(rA).cross(axis); | 682 double a1 = temp.setFrom(d).add(rA).cross(axis); |
689 double a2 = rB.cross(axis); | 683 double a2 = rB.cross(axis); |
690 Rot.mulToOutUnsafe(qA, _localYAxisA, perp); | 684 Rot.mulToOutUnsafe(qA, _localYAxisA, perp); |
691 | 685 |
692 double s1 = temp.setFrom(d).add(rA).cross(perp); | 686 double s1 = temp.setFrom(d).add(rA).cross(perp); |
693 double s2 = rB.cross(perp); | 687 double s2 = rB.cross(perp); |
694 | 688 |
(...skipping 94 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
789 data.positions[_indexB].a = aB; | 783 data.positions[_indexB].a = aB; |
790 | 784 |
791 pool.pushVec2(7); | 785 pool.pushVec2(7); |
792 pool.pushVec3(1); | 786 pool.pushVec3(1); |
793 pool.pushRot(2); | 787 pool.pushRot(2); |
794 | 788 |
795 return linearError <= Settings.linearSlop && | 789 return linearError <= Settings.linearSlop && |
796 angularError <= Settings.angularSlop; | 790 angularError <= Settings.angularSlop; |
797 } | 791 } |
798 } | 792 } |
OLD | NEW |