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 |