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 |