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 |