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 |