| 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 57 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 68 final Vector2 _rA = new Vector2.zero(); | 68 final Vector2 _rA = new Vector2.zero(); |
| 69 final Vector2 _rB = new Vector2.zero(); | 69 final Vector2 _rB = new Vector2.zero(); |
| 70 final Vector2 _localCenterA = new Vector2.zero(); | 70 final Vector2 _localCenterA = new Vector2.zero(); |
| 71 final Vector2 _localCenterB = new Vector2.zero(); | 71 final Vector2 _localCenterB = new Vector2.zero(); |
| 72 double _invMassA = 0.0; | 72 double _invMassA = 0.0; |
| 73 double _invMassB = 0.0; | 73 double _invMassB = 0.0; |
| 74 double _invIA = 0.0; | 74 double _invIA = 0.0; |
| 75 double _invIB = 0.0; | 75 double _invIB = 0.0; |
| 76 final Matrix3 _mass = | 76 final Matrix3 _mass = |
| 77 new Matrix3.zero(); // effective mass for point-to-point constraint. | 77 new Matrix3.zero(); // effective mass for point-to-point constraint. |
| 78 double _motorMass = | 78 double _motorMass = 0.0; // effective mass for motor/limit angular constraint. |
| 79 0.0; // effective mass for motor/limit angular constraint. | |
| 80 LimitState _limitState = LimitState.INACTIVE; | 79 LimitState _limitState = LimitState.INACTIVE; |
| 81 | 80 |
| 82 RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def) | 81 RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def) |
| 83 : super(argWorld, def) { | 82 : super(argWorld, def) { |
| 84 _localAnchorA.setFrom(def.localAnchorA); | 83 _localAnchorA.setFrom(def.localAnchorA); |
| 85 _localAnchorB.setFrom(def.localAnchorB); | 84 _localAnchorB.setFrom(def.localAnchorB); |
| 86 _referenceAngle = def.referenceAngle; | 85 _referenceAngle = def.referenceAngle; |
| 87 | 86 |
| 88 _lowerAngle = def.lowerAngle; | 87 _lowerAngle = def.lowerAngle; |
| 89 _upperAngle = def.upperAngle; | 88 _upperAngle = def.upperAngle; |
| (...skipping 23 matching lines...) Expand all Loading... |
| 113 Vector2 vB = data.velocities[_indexB].v; | 112 Vector2 vB = data.velocities[_indexB].v; |
| 114 double wB = data.velocities[_indexB].w; | 113 double wB = data.velocities[_indexB].w; |
| 115 final Rot qA = pool.popRot(); | 114 final Rot qA = pool.popRot(); |
| 116 final Rot qB = pool.popRot(); | 115 final Rot qB = pool.popRot(); |
| 117 final Vector2 temp = pool.popVec2(); | 116 final Vector2 temp = pool.popVec2(); |
| 118 | 117 |
| 119 qA.setAngle(aA); | 118 qA.setAngle(aA); |
| 120 qB.setAngle(aB); | 119 qB.setAngle(aB); |
| 121 | 120 |
| 122 // Compute the effective masses. | 121 // Compute the effective masses. |
| 123 Rot.mulToOutUnsafe( | 122 Rot.mulToOutUnsafe(qA, temp.setFrom(_localAnchorA).sub(_localCenterA), _rA); |
| 124 qA, temp.setFrom(_localAnchorA).sub(_localCenterA), _rA); | 123 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); |
| 125 Rot.mulToOutUnsafe( | |
| 126 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); | |
| 127 | 124 |
| 128 // J = [-I -r1_skew I r2_skew] | 125 // J = [-I -r1_skew I r2_skew] |
| 129 // [ 0 -1 0 1] | 126 // [ 0 -1 0 1] |
| 130 // r_skew = [-ry; rx] | 127 // r_skew = [-ry; rx] |
| 131 | 128 |
| 132 // Matlab | 129 // Matlab |
| 133 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] | 130 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] |
| 134 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] | 131 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] |
| 135 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] | 132 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] |
| 136 | 133 |
| (...skipping 415 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 552 assert(lower <= upper); | 549 assert(lower <= upper); |
| 553 if (lower != _lowerAngle || upper != _upperAngle) { | 550 if (lower != _lowerAngle || upper != _upperAngle) { |
| 554 _bodyA.setAwake(true); | 551 _bodyA.setAwake(true); |
| 555 _bodyB.setAwake(true); | 552 _bodyB.setAwake(true); |
| 556 _impulse.z = 0.0; | 553 _impulse.z = 0.0; |
| 557 _lowerAngle = lower; | 554 _lowerAngle = lower; |
| 558 _upperAngle = upper; | 555 _upperAngle = upper; |
| 559 } | 556 } |
| 560 } | 557 } |
| 561 } | 558 } |
| OLD | NEW |