| 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 38 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 49 * constant The ratio can be negative or positive. If one joint is a revolute jo
int and the other | 49 * constant The ratio can be negative or positive. If one joint is a revolute jo
int and the other |
| 50 * joint is a prismatic joint, then the ratio will have units of length or units
of 1/length. | 50 * joint is a prismatic joint, then the ratio will have units of length or units
of 1/length. |
| 51 * | 51 * |
| 52 * @warning The revolute and prismatic joints must be attached to fixed bodies (
which must be body1 | 52 * @warning The revolute and prismatic joints must be attached to fixed bodies (
which must be body1 |
| 53 * on those joints). | 53 * on those joints). |
| 54 * @warning You have to manually destroy the gear joint if joint1 or joint2 is d
estroyed. | 54 * @warning You have to manually destroy the gear joint if joint1 or joint2 is d
estroyed. |
| 55 * @author Daniel Murphy | 55 * @author Daniel Murphy |
| 56 */ | 56 */ |
| 57 | 57 |
| 58 class GearJoint extends Joint { | 58 class GearJoint extends Joint { |
| 59 | |
| 60 final Joint _joint1; | 59 final Joint _joint1; |
| 61 final Joint _joint2; | 60 final Joint _joint2; |
| 62 | 61 |
| 63 final JointType _typeA; | 62 final JointType _typeA; |
| 64 final JointType _typeB; | 63 final JointType _typeB; |
| 65 | 64 |
| 66 // Body A is connected to body C | 65 // Body A is connected to body C |
| 67 // Body B is connected to body D | 66 // Body B is connected to body D |
| 68 final Body _bodyC; | 67 final Body _bodyC; |
| 69 final Body _bodyD; | 68 final Body _bodyD; |
| (...skipping 229 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 299 } else { | 298 } else { |
| 300 Vector2 u = pool.popVec2(); | 299 Vector2 u = pool.popVec2(); |
| 301 Vector2 rD = pool.popVec2(); | 300 Vector2 rD = pool.popVec2(); |
| 302 Vector2 rB = pool.popVec2(); | 301 Vector2 rB = pool.popVec2(); |
| 303 Rot.mulToOutUnsafe(qD, _localAxisD, u); | 302 Rot.mulToOutUnsafe(qD, _localAxisD, u); |
| 304 Rot.mulToOutUnsafe(qD, temp.setFrom(_localAnchorD).sub(_lcD), rD); | 303 Rot.mulToOutUnsafe(qD, temp.setFrom(_localAnchorD).sub(_lcD), rD); |
| 305 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_lcB), rB); | 304 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_lcB), rB); |
| 306 _JvBD.setFrom(u).scale(_ratio); | 305 _JvBD.setFrom(u).scale(_ratio); |
| 307 _JwD = _ratio * rD.cross(u); | 306 _JwD = _ratio * rD.cross(u); |
| 308 _JwB = _ratio * rB.cross(u); | 307 _JwB = _ratio * rB.cross(u); |
| 309 _mass += _ratio * _ratio * (_mD + _mB) + | 308 _mass += |
| 310 _iD * _JwD * _JwD + | 309 _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB; |
| 311 _iB * _JwB * _JwB; | |
| 312 pool.pushVec2(3); | 310 pool.pushVec2(3); |
| 313 } | 311 } |
| 314 | 312 |
| 315 // Compute effective mass. | 313 // Compute effective mass. |
| 316 _mass = _mass > 0.0 ? 1.0 / _mass : 0.0; | 314 _mass = _mass > 0.0 ? 1.0 / _mass : 0.0; |
| 317 | 315 |
| 318 if (data.step.warmStarting) { | 316 if (data.step.warmStarting) { |
| 319 vA.x += (_mA * _impulse) * _JvAC.x; | 317 vA.x += (_mA * _impulse) * _JvAC.x; |
| 320 vA.y += (_mA * _impulse) * _JvAC.y; | 318 vA.y += (_mA * _impulse) * _JvAC.y; |
| 321 wA += _iA * _impulse * _JwA; | 319 wA += _iA * _impulse * _JwA; |
| (...skipping 145 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 467 Vector2 rD = pool.popVec2(); | 465 Vector2 rD = pool.popVec2(); |
| 468 Vector2 rB = pool.popVec2(); | 466 Vector2 rB = pool.popVec2(); |
| 469 Vector2 pD = pool.popVec2(); | 467 Vector2 pD = pool.popVec2(); |
| 470 Vector2 pB = pool.popVec2(); | 468 Vector2 pB = pool.popVec2(); |
| 471 Rot.mulToOutUnsafe(qD, _localAxisD, u); | 469 Rot.mulToOutUnsafe(qD, _localAxisD, u); |
| 472 Rot.mulToOutUnsafe(qD, temp.setFrom(_localAnchorD).sub(_lcD), rD); | 470 Rot.mulToOutUnsafe(qD, temp.setFrom(_localAnchorD).sub(_lcD), rD); |
| 473 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_lcB), rB); | 471 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_lcB), rB); |
| 474 JvBD.setFrom(u).scale(_ratio); | 472 JvBD.setFrom(u).scale(_ratio); |
| 475 JwD = rD.cross(u); | 473 JwD = rD.cross(u); |
| 476 JwB = rB.cross(u); | 474 JwB = rB.cross(u); |
| 477 mass += _ratio * _ratio * (_mD + _mB) + | 475 mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB; |
| 478 _iD * JwD * JwD + | |
| 479 _iB * JwB * JwB; | |
| 480 | 476 |
| 481 pD.setFrom(_localAnchorD).sub(_lcD); | 477 pD.setFrom(_localAnchorD).sub(_lcD); |
| 482 Rot.mulTransUnsafeVec2(qD, temp.setFrom(rB).add(cB).sub(cD), pB); | 478 Rot.mulTransUnsafeVec2(qD, temp.setFrom(rB).add(cB).sub(cD), pB); |
| 483 coordinateB = pB.sub(pD).dot(_localAxisD); | 479 coordinateB = pB.sub(pD).dot(_localAxisD); |
| 484 pool.pushVec2(5); | 480 pool.pushVec2(5); |
| 485 } | 481 } |
| 486 | 482 |
| 487 double C = (coordinateA + _ratio * coordinateB) - _constant; | 483 double C = (coordinateA + _ratio * coordinateB) - _constant; |
| 488 | 484 |
| 489 double impulse = 0.0; | 485 double impulse = 0.0; |
| (...skipping 25 matching lines...) Expand all Loading... |
| 515 data.positions[_indexB].a = aB; | 511 data.positions[_indexB].a = aB; |
| 516 // data.positions[_indexC].c = cC; | 512 // data.positions[_indexC].c = cC; |
| 517 data.positions[_indexC].a = aC; | 513 data.positions[_indexC].a = aC; |
| 518 // data.positions[_indexD].c = cD; | 514 // data.positions[_indexD].c = cD; |
| 519 data.positions[_indexD].a = aD; | 515 data.positions[_indexD].a = aD; |
| 520 | 516 |
| 521 // TODO_ERIN not implemented | 517 // TODO_ERIN not implemented |
| 522 return linearError < Settings.linearSlop; | 518 return linearError < Settings.linearSlop; |
| 523 } | 519 } |
| 524 } | 520 } |
| OLD | NEW |