| 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 |