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 73 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
84 double wB = data.velocities[_indexB].w; | 84 double wB = data.velocities[_indexB].w; |
85 | 85 |
86 final Rot qA = pool.popRot(); | 86 final Rot qA = pool.popRot(); |
87 final Rot qB = pool.popRot(); | 87 final Rot qB = pool.popRot(); |
88 final Vector2 temp = pool.popVec2(); | 88 final Vector2 temp = pool.popVec2(); |
89 | 89 |
90 qA.setAngle(aA); | 90 qA.setAngle(aA); |
91 qB.setAngle(aB); | 91 qB.setAngle(aB); |
92 | 92 |
93 // Compute the effective masses. | 93 // Compute the effective masses. |
94 Rot.mulToOutUnsafe( | 94 Rot.mulToOutUnsafe(qA, temp.setFrom(_localAnchorA).sub(_localCenterA), _rA); |
95 qA, temp.setFrom(_localAnchorA).sub(_localCenterA), _rA); | 95 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); |
96 Rot.mulToOutUnsafe( | |
97 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); | |
98 | 96 |
99 _u.setFrom(cB).add(_rB).sub(cA).sub(_rA); | 97 _u.setFrom(cB).add(_rB).sub(cA).sub(_rA); |
100 | 98 |
101 _length = _u.length; | 99 _length = _u.length; |
102 | 100 |
103 double C = _length - _maxLength; | 101 double C = _length - _maxLength; |
104 if (C > 0.0) { | 102 if (C > 0.0) { |
105 _state = LimitState.AT_UPPER; | 103 _state = LimitState.AT_UPPER; |
106 } else { | 104 } else { |
107 _state = LimitState.INACTIVE; | 105 _state = LimitState.INACTIVE; |
(...skipping 98 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
206 final Rot qB = pool.popRot(); | 204 final Rot qB = pool.popRot(); |
207 final Vector2 u = pool.popVec2(); | 205 final Vector2 u = pool.popVec2(); |
208 final Vector2 rA = pool.popVec2(); | 206 final Vector2 rA = pool.popVec2(); |
209 final Vector2 rB = pool.popVec2(); | 207 final Vector2 rB = pool.popVec2(); |
210 final Vector2 temp = pool.popVec2(); | 208 final Vector2 temp = pool.popVec2(); |
211 | 209 |
212 qA.setAngle(aA); | 210 qA.setAngle(aA); |
213 qB.setAngle(aB); | 211 qB.setAngle(aB); |
214 | 212 |
215 // Compute the effective masses. | 213 // Compute the effective masses. |
216 Rot.mulToOutUnsafe( | 214 Rot.mulToOutUnsafe(qA, temp.setFrom(_localAnchorA).sub(_localCenterA), rA); |
217 qA, temp.setFrom(_localAnchorA).sub(_localCenterA), rA); | 215 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), rB); |
218 Rot.mulToOutUnsafe( | |
219 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), rB); | |
220 u.setFrom(cB).add(rB).sub(cA).sub(rA); | 216 u.setFrom(cB).add(rB).sub(cA).sub(rA); |
221 | 217 |
222 double length = u.normalizeLength(); | 218 double length = u.normalizeLength(); |
223 double C = length - _maxLength; | 219 double C = length - _maxLength; |
224 | 220 |
225 C = MathUtils.clampDouble(C, 0.0, Settings.maxLinearCorrection); | 221 C = MathUtils.clampDouble(C, 0.0, Settings.maxLinearCorrection); |
226 | 222 |
227 double impulse = -_mass * C; | 223 double impulse = -_mass * C; |
228 double Px = impulse * u.x; | 224 double Px = impulse * u.x; |
229 double Py = impulse * u.y; | 225 double Py = impulse * u.y; |
(...skipping 45 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
275 } | 271 } |
276 | 272 |
277 void setMaxLength(double maxLength) { | 273 void setMaxLength(double maxLength) { |
278 this._maxLength = maxLength; | 274 this._maxLength = maxLength; |
279 } | 275 } |
280 | 276 |
281 LimitState getLimitState() { | 277 LimitState getLimitState() { |
282 return _state; | 278 return _state; |
283 } | 279 } |
284 } | 280 } |
OLD | NEW |