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 |
11 * and/or other materials provided with the distribution. | 11 * and/or other materials provided with the distribution. |
12 * | 12 * |
13 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" A
ND | 13 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" A
ND |
14 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | 14 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED |
15 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLA
IMED. | 15 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLA
IMED. |
16 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRE
CT, | 16 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRE
CT, |
17 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING
, BUT | 17 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING
, BUT |
18 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DAT
A, OR | 18 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DAT
A, OR |
19 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABIL
ITY, | 19 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABIL
ITY, |
20 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHER
WISE) | 20 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHER
WISE) |
21 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | 21 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
22 * POSSIBILITY OF SUCH DAMAGE. | 22 * POSSIBILITY OF SUCH DAMAGE. |
23 ******************************************************************************/ | 23 ******************************************************************************/ |
24 | 24 |
25 part of box2d; | 25 part of box2d; |
26 | 26 |
27 class FrictionJoint extends Joint { | 27 class FrictionJoint extends Joint { |
28 | |
29 final Vector2 _localAnchorA; | 28 final Vector2 _localAnchorA; |
30 final Vector2 _localAnchorB; | 29 final Vector2 _localAnchorB; |
31 | 30 |
32 // Solver shared | 31 // Solver shared |
33 final Vector2 _linearImpulse; | 32 final Vector2 _linearImpulse; |
34 double _angularImpulse = 0.0; | 33 double _angularImpulse = 0.0; |
35 double _maxForce = 0.0; | 34 double _maxForce = 0.0; |
36 double _maxTorque = 0.0; | 35 double _maxTorque = 0.0; |
37 | 36 |
38 // Solver temp | 37 // Solver temp |
(...skipping 84 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
123 double wB = data.velocities[_indexB].w; | 122 double wB = data.velocities[_indexB].w; |
124 | 123 |
125 final Vector2 temp = pool.popVec2(); | 124 final Vector2 temp = pool.popVec2(); |
126 final Rot qA = pool.popRot(); | 125 final Rot qA = pool.popRot(); |
127 final Rot qB = pool.popRot(); | 126 final Rot qB = pool.popRot(); |
128 | 127 |
129 qA.setAngle(aA); | 128 qA.setAngle(aA); |
130 qB.setAngle(aB); | 129 qB.setAngle(aB); |
131 | 130 |
132 // Compute the effective mass matrix. | 131 // Compute the effective mass matrix. |
133 Rot.mulToOutUnsafe( | 132 Rot.mulToOutUnsafe(qA, temp.setFrom(_localAnchorA).sub(_localCenterA), _rA); |
134 qA, temp.setFrom(_localAnchorA).sub(_localCenterA), _rA); | 133 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); |
135 Rot.mulToOutUnsafe( | |
136 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); | |
137 | 134 |
138 // J = [-I -r1_skew I r2_skew] | 135 // J = [-I -r1_skew I r2_skew] |
139 // [ 0 -1 0 1] | 136 // [ 0 -1 0 1] |
140 // r_skew = [-ry; rx] | 137 // r_skew = [-ry; rx] |
141 | 138 |
142 // Matlab | 139 // Matlab |
143 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] | 140 // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] |
144 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] | 141 // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] |
145 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] | 142 // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] |
146 | 143 |
(...skipping 123 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
270 // data.velocities[_indexB].v.set(vB); | 267 // data.velocities[_indexB].v.set(vB); |
271 data.velocities[_indexB].w = wB; | 268 data.velocities[_indexB].w = wB; |
272 | 269 |
273 pool.pushVec2(4); | 270 pool.pushVec2(4); |
274 } | 271 } |
275 | 272 |
276 bool solvePositionConstraints(final SolverData data) { | 273 bool solvePositionConstraints(final SolverData data) { |
277 return true; | 274 return true; |
278 } | 275 } |
279 } | 276 } |
OLD | NEW |