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