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 |