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 106 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
117 Vector2 vB = data.velocities[_indexB].v; | 117 Vector2 vB = data.velocities[_indexB].v; |
118 double wB = data.velocities[_indexB].w; | 118 double wB = data.velocities[_indexB].w; |
119 | 119 |
120 final Rot qA = pool.popRot(); | 120 final Rot qA = pool.popRot(); |
121 final Rot qB = pool.popRot(); | 121 final Rot qB = pool.popRot(); |
122 | 122 |
123 qA.setAngle(aA); | 123 qA.setAngle(aA); |
124 qB.setAngle(aB); | 124 qB.setAngle(aB); |
125 | 125 |
126 // use _u as temporary variable | 126 // use _u as temporary variable |
127 Rot.mulToOutUnsafe( | 127 Rot.mulToOutUnsafe(qA, _u.setFrom(_localAnchorA).sub(_localCenterA), _rA); |
128 qA, _u.setFrom(_localAnchorA).sub(_localCenterA), _rA); | 128 Rot.mulToOutUnsafe(qB, _u.setFrom(_localAnchorB).sub(_localCenterB), _rB); |
129 Rot.mulToOutUnsafe( | |
130 qB, _u.setFrom(_localAnchorB).sub(_localCenterB), _rB); | |
131 _u.setFrom(cB).add(_rB).sub(cA).sub(_rA); | 129 _u.setFrom(cB).add(_rB).sub(cA).sub(_rA); |
132 | 130 |
133 pool.pushRot(2); | 131 pool.pushRot(2); |
134 | 132 |
135 // Handle singularity. | 133 // Handle singularity. |
136 double length = _u.length; | 134 double length = _u.length; |
137 if (length > Settings.linearSlop) { | 135 if (length > Settings.linearSlop) { |
138 _u.x *= 1.0 / length; | 136 _u.x *= 1.0 / length; |
139 _u.y *= 1.0 / length; | 137 _u.y *= 1.0 / length; |
140 } else { | 138 } else { |
141 _u.setValues(0.0, 0.0); | 139 _u.setValues(0.0, 0.0); |
142 } | 140 } |
143 | 141 |
144 double crAu = _rA.cross(_u); | 142 double crAu = _rA.cross(_u); |
145 double crBu = _rB.cross(_u); | 143 double crBu = _rB.cross(_u); |
146 double invMass = _invMassA + | 144 double invMass = |
147 _invIA * crAu * crAu + | 145 _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu; |
148 _invMassB + | |
149 _invIB * crBu * crBu; | |
150 | 146 |
151 // Compute the effective mass matrix. | 147 // Compute the effective mass matrix. |
152 _mass = invMass != 0.0 ? 1.0 / invMass : 0.0; | 148 _mass = invMass != 0.0 ? 1.0 / invMass : 0.0; |
153 | 149 |
154 if (_frequencyHz > 0.0) { | 150 if (_frequencyHz > 0.0) { |
155 double C = length - _length; | 151 double C = length - _length; |
156 | 152 |
157 // Frequency | 153 // Frequency |
158 double omega = 2.0 * Math.PI * _frequencyHz; | 154 double omega = 2.0 * Math.PI * _frequencyHz; |
159 | 155 |
(...skipping 120 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
280 data.positions[_indexA].a = aA; | 276 data.positions[_indexA].a = aA; |
281 // data.positions[_indexB].c.set(cB); | 277 // data.positions[_indexB].c.set(cB); |
282 data.positions[_indexB].a = aB; | 278 data.positions[_indexB].a = aB; |
283 | 279 |
284 pool.pushVec2(3); | 280 pool.pushVec2(3); |
285 pool.pushRot(2); | 281 pool.pushRot(2); |
286 | 282 |
287 return C.abs() < Settings.linearSlop; | 283 return C.abs() < Settings.linearSlop; |
288 } | 284 } |
289 } | 285 } |
OLD | NEW |