| 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 172 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 183 double wB = data.velocities[_indexB].w; | 183 double wB = data.velocities[_indexB].w; |
| 184 | 184 |
| 185 final Rot qA = pool.popRot(); | 185 final Rot qA = pool.popRot(); |
| 186 final Rot qB = pool.popRot(); | 186 final Rot qB = pool.popRot(); |
| 187 final Vector2 temp = pool.popVec2(); | 187 final Vector2 temp = pool.popVec2(); |
| 188 | 188 |
| 189 qA.setAngle(aA); | 189 qA.setAngle(aA); |
| 190 qB.setAngle(aB); | 190 qB.setAngle(aB); |
| 191 | 191 |
| 192 // Compute the effective masses. | 192 // Compute the effective masses. |
| 193 Rot.mulToOutUnsafe( | 193 Rot.mulToOutUnsafe(qA, temp.setFrom(_localAnchorA).sub(_localCenterA), _rA); |
| 194 qA, temp.setFrom(_localAnchorA).sub(_localCenterA), _rA); | 194 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); |
| 195 Rot.mulToOutUnsafe( | |
| 196 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); | |
| 197 | 195 |
| 198 _uA.setFrom(cA).add(_rA).sub(_groundAnchorA); | 196 _uA.setFrom(cA).add(_rA).sub(_groundAnchorA); |
| 199 _uB.setFrom(cB).add(_rB).sub(_groundAnchorB); | 197 _uB.setFrom(cB).add(_rB).sub(_groundAnchorB); |
| 200 | 198 |
| 201 double lengthA = _uA.length; | 199 double lengthA = _uA.length; |
| 202 double lengthB = _uB.length; | 200 double lengthB = _uB.length; |
| 203 | 201 |
| 204 if (lengthA > 10.0 * Settings.linearSlop) { | 202 if (lengthA > 10.0 * Settings.linearSlop) { |
| 205 _uA.scale(1.0 / lengthA); | 203 _uA.scale(1.0 / lengthA); |
| 206 } else { | 204 } else { |
| (...skipping 100 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 307 final Vector2 PB = pool.popVec2(); | 305 final Vector2 PB = pool.popVec2(); |
| 308 | 306 |
| 309 Vector2 cA = data.positions[_indexA].c; | 307 Vector2 cA = data.positions[_indexA].c; |
| 310 double aA = data.positions[_indexA].a; | 308 double aA = data.positions[_indexA].a; |
| 311 Vector2 cB = data.positions[_indexB].c; | 309 Vector2 cB = data.positions[_indexB].c; |
| 312 double aB = data.positions[_indexB].a; | 310 double aB = data.positions[_indexB].a; |
| 313 | 311 |
| 314 qA.setAngle(aA); | 312 qA.setAngle(aA); |
| 315 qB.setAngle(aB); | 313 qB.setAngle(aB); |
| 316 | 314 |
| 317 Rot.mulToOutUnsafe( | 315 Rot.mulToOutUnsafe(qA, temp.setFrom(_localAnchorA).sub(_localCenterA), rA); |
| 318 qA, temp.setFrom(_localAnchorA).sub(_localCenterA), rA); | 316 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), rB); |
| 319 Rot.mulToOutUnsafe( | |
| 320 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), rB); | |
| 321 | 317 |
| 322 uA.setFrom(cA).add(rA).sub(_groundAnchorA); | 318 uA.setFrom(cA).add(rA).sub(_groundAnchorA); |
| 323 uB.setFrom(cB).add(rB).sub(_groundAnchorB); | 319 uB.setFrom(cB).add(rB).sub(_groundAnchorB); |
| 324 | 320 |
| 325 double lengthA = uA.length; | 321 double lengthA = uA.length; |
| 326 double lengthB = uB.length; | 322 double lengthB = uB.length; |
| 327 | 323 |
| 328 if (lengthA > 10.0 * Settings.linearSlop) { | 324 if (lengthA > 10.0 * Settings.linearSlop) { |
| 329 uA.scale(1.0 / lengthA); | 325 uA.scale(1.0 / lengthA); |
| 330 } else { | 326 } else { |
| (...skipping 38 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 369 data.positions[_indexA].a = aA; | 365 data.positions[_indexA].a = aA; |
| 370 // data.positions[_indexB].c.set(cB); | 366 // data.positions[_indexB].c.set(cB); |
| 371 data.positions[_indexB].a = aB; | 367 data.positions[_indexB].a = aB; |
| 372 | 368 |
| 373 pool.pushRot(2); | 369 pool.pushRot(2); |
| 374 pool.pushVec2(7); | 370 pool.pushVec2(7); |
| 375 | 371 |
| 376 return linearError < Settings.linearSlop; | 372 return linearError < Settings.linearSlop; |
| 377 } | 373 } |
| 378 } | 374 } |
| OLD | NEW |