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 |