| 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 119 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 130 assert(d + h * k > Settings.EPSILON); | 130 assert(d + h * k > Settings.EPSILON); |
| 131 _gamma = h * (d + h * k); | 131 _gamma = h * (d + h * k); |
| 132 if (_gamma != 0.0) { | 132 if (_gamma != 0.0) { |
| 133 _gamma = 1.0 / _gamma; | 133 _gamma = 1.0 / _gamma; |
| 134 } | 134 } |
| 135 _beta = h * k * _gamma; | 135 _beta = h * k * _gamma; |
| 136 | 136 |
| 137 Vector2 temp = pool.popVec2(); | 137 Vector2 temp = pool.popVec2(); |
| 138 | 138 |
| 139 // Compute the effective mass matrix. | 139 // Compute the effective mass matrix. |
| 140 Rot.mulToOutUnsafe( | 140 Rot.mulToOutUnsafe(qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); |
| 141 qB, temp.setFrom(_localAnchorB).sub(_localCenterB), _rB); | |
| 142 | 141 |
| 143 // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * in
vI2 * skew(r2)] | 142 // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * in
vI2 * skew(r2)] |
| 144 // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r
1.x*r1.y] | 143 // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r
1.x*r1.y] |
| 145 // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] | 144 // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] |
| 146 final Matrix2 K = pool.popMat22(); | 145 final Matrix2 K = pool.popMat22(); |
| 147 double a11 = _invMassB + _invIB * _rB.y * _rB.y + _gamma; | 146 double a11 = _invMassB + _invIB * _rB.y * _rB.y + _gamma; |
| 148 double a21 = -_invIB * _rB.x * _rB.y; | 147 double a21 = -_invIB * _rB.x * _rB.y; |
| 149 double a12 = a21; | 148 double a12 = a21; |
| 150 double a22 = _invMassB + _invIB * _rB.x * _rB.x + _gamma; | 149 double a22 = _invMassB + _invIB * _rB.x * _rB.x + _gamma; |
| 151 | 150 |
| (...skipping 55 matching lines...) Expand 10 before | Expand all | Expand 10 after Loading... |
| 207 vB.x += _invMassB * impulse.x; | 206 vB.x += _invMassB * impulse.x; |
| 208 vB.y += _invMassB * impulse.y; | 207 vB.y += _invMassB * impulse.y; |
| 209 wB += _invIB * _rB.cross(impulse); | 208 wB += _invIB * _rB.cross(impulse); |
| 210 | 209 |
| 211 // data.velocities[_indexB].v.set(vB); | 210 // data.velocities[_indexB].v.set(vB); |
| 212 data.velocities[_indexB].w = wB; | 211 data.velocities[_indexB].w = wB; |
| 213 | 212 |
| 214 pool.pushVec2(3); | 213 pool.pushVec2(3); |
| 215 } | 214 } |
| 216 } | 215 } |
| OLD | NEW |