| 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 118 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 129 double wB = data.velocities[_indexB].w; | 129 double wB = data.velocities[_indexB].w; |
| 130 | 130 |
| 131 final Rot qA = pool.popRot(); | 131 final Rot qA = pool.popRot(); |
| 132 final Rot qB = pool.popRot(); | 132 final Rot qB = pool.popRot(); |
| 133 final Vector2 temp = pool.popVec2(); | 133 final Vector2 temp = pool.popVec2(); |
| 134 | 134 |
| 135 qA.setAngle(aA); | 135 qA.setAngle(aA); |
| 136 qB.setAngle(aB); | 136 qB.setAngle(aB); |
| 137 | 137 |
| 138 // Compute the effective masses. | 138 // Compute the effective masses. |
| 139 Rot.mulToOutUnsafe( | 139 Rot.mulToOutUnsafe(qA, temp.setFrom(_localAnchorA).sub(_localCenterA), _rA); |
| 140 qA, temp.setFrom(_localAnchorA).sub(_localCenterA), _rA); | 140 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); |
| 141 Rot.mulToOutUnsafe( | |
| 142 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); | |
| 143 | 141 |
| 144 // J = [-I -r1_skew I r2_skew] | 142 // J = [-I -r1_skew I r2_skew] |
| 145 // [ 0 -1 0 1] | 143 // [ 0 -1 0 1] |
| 146 // r_skew = [-ry; rx] | 144 // r_skew = [-ry; rx] |
| 147 | 145 |
| 148 // Matlab | 146 // Matlab |
| 149 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] | 147 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] |
| 150 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] | 148 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] |
| 151 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] | 149 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] |
| 152 | 150 |
| (...skipping 166 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 319 final Vector2 rB = pool.popVec2(); | 317 final Vector2 rB = pool.popVec2(); |
| 320 | 318 |
| 321 qA.setAngle(aA); | 319 qA.setAngle(aA); |
| 322 qB.setAngle(aB); | 320 qB.setAngle(aB); |
| 323 | 321 |
| 324 double mA = _invMassA, | 322 double mA = _invMassA, |
| 325 mB = _invMassB; | 323 mB = _invMassB; |
| 326 double iA = _invIA, | 324 double iA = _invIA, |
| 327 iB = _invIB; | 325 iB = _invIB; |
| 328 | 326 |
| 329 Rot.mulToOutUnsafe( | 327 Rot.mulToOutUnsafe(qA, temp.setFrom(_localAnchorA).sub(_localCenterA), rA); |
| 330 qA, temp.setFrom(_localAnchorA).sub(_localCenterA), rA); | 328 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), rB); |
| 331 Rot.mulToOutUnsafe( | |
| 332 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), rB); | |
| 333 double positionError, angularError; | 329 double positionError, angularError; |
| 334 | 330 |
| 335 final Matrix3 K = pool.popMat33(); | 331 final Matrix3 K = pool.popMat33(); |
| 336 final Vector2 C1 = pool.popVec2(); | 332 final Vector2 C1 = pool.popVec2(); |
| 337 final Vector2 P = pool.popVec2(); | 333 final Vector2 P = pool.popVec2(); |
| 338 | 334 |
| 339 double ex_x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; | 335 double ex_x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; |
| 340 double ey_x = -rA.y * rA.x * iA - rB.y * rB.x * iB; | 336 double ey_x = -rA.y * rA.x * iA - rB.y * rB.x * iB; |
| 341 double ez_x = -rA.y * iA - rB.y * iB; | 337 double ez_x = -rA.y * iA - rB.y * iB; |
| 342 double ex_y = K.entry(0, 1); | 338 double ex_y = K.entry(0, 1); |
| (...skipping 52 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 395 data.positions[_indexB].a = aB; | 391 data.positions[_indexB].a = aB; |
| 396 | 392 |
| 397 pool.pushVec2(5); | 393 pool.pushVec2(5); |
| 398 pool.pushRot(2); | 394 pool.pushRot(2); |
| 399 pool.pushMat33(1); | 395 pool.pushMat33(1); |
| 400 | 396 |
| 401 return positionError <= Settings.linearSlop && | 397 return positionError <= Settings.linearSlop && |
| 402 angularError <= Settings.angularSlop; | 398 angularError <= Settings.angularSlop; |
| 403 } | 399 } |
| 404 } | 400 } |
| OLD | NEW |