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