| 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 216 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 227 double wB = data.velocities[_indexB].w; | 227 double wB = data.velocities[_indexB].w; |
| 228 | 228 |
| 229 final Rot qA = pool.popRot(); | 229 final Rot qA = pool.popRot(); |
| 230 final Rot qB = pool.popRot(); | 230 final Rot qB = pool.popRot(); |
| 231 final Vector2 temp = pool.popVec2(); | 231 final Vector2 temp = pool.popVec2(); |
| 232 | 232 |
| 233 qA.setAngle(aA); | 233 qA.setAngle(aA); |
| 234 qB.setAngle(aB); | 234 qB.setAngle(aB); |
| 235 | 235 |
| 236 // Compute the effective masses. | 236 // Compute the effective masses. |
| 237 Rot.mulToOutUnsafe( | 237 Rot.mulToOutUnsafe(qA, temp.setFrom(_localAnchorA).sub(_localCenterA), rA); |
| 238 qA, temp.setFrom(_localAnchorA).sub(_localCenterA), rA); | 238 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), rB); |
| 239 Rot.mulToOutUnsafe( | |
| 240 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), rB); | |
| 241 d.setFrom(cB).add(rB).sub(cA).sub(rA); | 239 d.setFrom(cB).add(rB).sub(cA).sub(rA); |
| 242 | 240 |
| 243 // Point to line constraint | 241 // Point to line constraint |
| 244 { | 242 { |
| 245 Rot.mulToOut(qA, _localYAxisA, _ay); | 243 Rot.mulToOut(qA, _localYAxisA, _ay); |
| 246 _sAy = temp.setFrom(d).add(rA).cross(_ay); | 244 _sAy = temp.setFrom(d).add(rA).cross(_ay); |
| 247 _sBy = rB.cross(_ay); | 245 _sBy = rB.cross(_ay); |
| 248 | 246 |
| 249 _mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy; | 247 _mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy; |
| 250 | 248 |
| (...skipping 99 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 350 Vector2 vA = data.velocities[_indexA].v; | 348 Vector2 vA = data.velocities[_indexA].v; |
| 351 double wA = data.velocities[_indexA].w; | 349 double wA = data.velocities[_indexA].w; |
| 352 Vector2 vB = data.velocities[_indexB].v; | 350 Vector2 vB = data.velocities[_indexB].v; |
| 353 double wB = data.velocities[_indexB].w; | 351 double wB = data.velocities[_indexB].w; |
| 354 | 352 |
| 355 final Vector2 temp = pool.popVec2(); | 353 final Vector2 temp = pool.popVec2(); |
| 356 final Vector2 P = pool.popVec2(); | 354 final Vector2 P = pool.popVec2(); |
| 357 | 355 |
| 358 // Solve spring constraint | 356 // Solve spring constraint |
| 359 { | 357 { |
| 360 double Cdot = | 358 double Cdot = _ax.dot(temp.setFrom(vB).sub(vA)) + _sBx * wB - _sAx * wA; |
| 361 _ax.dot(temp.setFrom(vB).sub(vA)) + _sBx * wB - _sAx * wA; | 359 double impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse); |
| 362 double impulse = | |
| 363 -_springMass * (Cdot + _bias + _gamma * _springImpulse); | |
| 364 _springImpulse += impulse; | 360 _springImpulse += impulse; |
| 365 | 361 |
| 366 P.x = impulse * _ax.x; | 362 P.x = impulse * _ax.x; |
| 367 P.y = impulse * _ax.y; | 363 P.y = impulse * _ax.y; |
| 368 double LA = impulse * _sAx; | 364 double LA = impulse * _sAx; |
| 369 double LB = impulse * _sBx; | 365 double LB = impulse * _sBx; |
| 370 | 366 |
| 371 vA.x -= mA * P.x; | 367 vA.x -= mA * P.x; |
| 372 vA.y -= mA * P.y; | 368 vA.y -= mA * P.y; |
| 373 wA -= iA * LA; | 369 wA -= iA * LA; |
| (...skipping 13 matching lines...) Expand all Loading... |
| 387 _motorImpulse = MathUtils.clampDouble( | 383 _motorImpulse = MathUtils.clampDouble( |
| 388 _motorImpulse + impulse, -maxImpulse, maxImpulse); | 384 _motorImpulse + impulse, -maxImpulse, maxImpulse); |
| 389 impulse = _motorImpulse - oldImpulse; | 385 impulse = _motorImpulse - oldImpulse; |
| 390 | 386 |
| 391 wA -= iA * impulse; | 387 wA -= iA * impulse; |
| 392 wB += iB * impulse; | 388 wB += iB * impulse; |
| 393 } | 389 } |
| 394 | 390 |
| 395 // Solve point to line constraint | 391 // Solve point to line constraint |
| 396 { | 392 { |
| 397 double Cdot = | 393 double Cdot = _ay.dot(temp.setFrom(vB).sub(vA)) + _sBy * wB - _sAy * wA; |
| 398 _ay.dot(temp.setFrom(vB).sub(vA)) + _sBy * wB - _sAy * wA; | |
| 399 double impulse = -_mass * Cdot; | 394 double impulse = -_mass * Cdot; |
| 400 _impulse += impulse; | 395 _impulse += impulse; |
| 401 | 396 |
| 402 P.x = impulse * _ay.x; | 397 P.x = impulse * _ay.x; |
| 403 P.y = impulse * _ay.y; | 398 P.y = impulse * _ay.y; |
| 404 double LA = impulse * _sAy; | 399 double LA = impulse * _sAy; |
| 405 double LB = impulse * _sBy; | 400 double LB = impulse * _sBy; |
| 406 | 401 |
| 407 vA.x -= mA * P.x; | 402 vA.x -= mA * P.x; |
| 408 vA.y -= mA * P.y; | 403 vA.y -= mA * P.y; |
| (...skipping 29 matching lines...) Expand all Loading... |
| 438 d.setFrom(cB).sub(cA).add(rB).sub(rA); | 433 d.setFrom(cB).sub(cA).add(rB).sub(rA); |
| 439 | 434 |
| 440 Vector2 ay = pool.popVec2(); | 435 Vector2 ay = pool.popVec2(); |
| 441 Rot.mulToOut(qA, _localYAxisA, ay); | 436 Rot.mulToOut(qA, _localYAxisA, ay); |
| 442 | 437 |
| 443 double sAy = temp.setFrom(d).add(rA).cross(ay); | 438 double sAy = temp.setFrom(d).add(rA).cross(ay); |
| 444 double sBy = rB.cross(ay); | 439 double sBy = rB.cross(ay); |
| 445 | 440 |
| 446 double C = d.dot(ay); | 441 double C = d.dot(ay); |
| 447 | 442 |
| 448 double k = _invMassA + | 443 double k = |
| 449 _invMassB + | 444 _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy; |
| 450 _invIA * _sAy * _sAy + | |
| 451 _invIB * _sBy * _sBy; | |
| 452 | 445 |
| 453 double impulse; | 446 double impulse; |
| 454 if (k != 0.0) { | 447 if (k != 0.0) { |
| 455 impulse = -C / k; | 448 impulse = -C / k; |
| 456 } else { | 449 } else { |
| 457 impulse = 0.0; | 450 impulse = 0.0; |
| 458 } | 451 } |
| 459 | 452 |
| 460 final Vector2 P = pool.popVec2(); | 453 final Vector2 P = pool.popVec2(); |
| 461 P.x = impulse * ay.x; | 454 P.x = impulse * ay.x; |
| (...skipping 11 matching lines...) Expand all Loading... |
| 473 pool.pushVec2(3); | 466 pool.pushVec2(3); |
| 474 pool.pushRot(2); | 467 pool.pushRot(2); |
| 475 // data.positions[_indexA].c = cA; | 468 // data.positions[_indexA].c = cA; |
| 476 data.positions[_indexA].a = aA; | 469 data.positions[_indexA].a = aA; |
| 477 // data.positions[_indexB].c = cB; | 470 // data.positions[_indexB].c = cB; |
| 478 data.positions[_indexB].a = aB; | 471 data.positions[_indexB].a = aB; |
| 479 | 472 |
| 480 return C.abs() <= Settings.linearSlop; | 473 return C.abs() <= Settings.linearSlop; |
| 481 } | 474 } |
| 482 } | 475 } |
| OLD | NEW |