internal override void InitVelocityConstraints(TimeStep step) { Body b = _body2; float mass = b.GetMass(); // Frequency float omega = 2.0f * Settings.Pi * _frequencyHz; // Damping coefficient float d = 2.0f * mass * _dampingRatio * omega; // Spring stiffness float k = mass * (omega * omega); // magic formulas // gamma has units of inverse mass. // beta has units of inverse time. Box2DXDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON); _gamma = 1.0f / (step.Dt * (d + step.Dt * k)); _beta = step.Dt * k * _gamma; // Compute the effective mass matrix. Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter()); // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] float invMass = b._invMass; float invI = b._invI; Mat22 K1 = new Mat22(); K1.Col1.X = invMass; K1.Col2.X = 0.0f; K1.Col1.Y = 0.0f; K1.Col2.Y = invMass; Mat22 K2 = new Mat22(); K2.Col1.X = invI * r.Y * r.Y; K2.Col2.X = -invI * r.X * r.Y; K2.Col1.Y = -invI * r.X * r.Y; K2.Col2.Y = invI * r.X * r.X; Mat22 K = K1 + K2; K.Col1.X += _gamma; K.Col2.Y += _gamma; _mass = K.GetInverse(); _C = b._sweep.C + r - _target; // Cheat with some damping b._angularVelocity *= 0.98f; // Warm starting. _impulse *= step.DtRatio; b._linearVelocity += invMass * _impulse; b._angularVelocity += invI * r.Cross(_impulse); }
internal override void InitVelocityConstraints(TimeStep step) { Body body = this._body2; float mass = body.GetMass(); float num = 2f * Settings.Pi * this._frequencyHz; float num2 = 2f * mass * this._dampingRatio * num; float num3 = mass * (num * num); Box2DXDebug.Assert(num2 + step.Dt * num3 > Settings.FLT_EPSILON); this._gamma = 1f / (step.Dt * (num2 + step.Dt * num3)); this._beta = step.Dt * num3 * this._gamma; Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor - body.GetLocalCenter()); float invMass = body._invMass; float invI = body._invI; Mat22 a = default(Mat22); a.Col1.X = invMass; a.Col2.X = 0f; a.Col1.Y = 0f; a.Col2.Y = invMass; Mat22 b = default(Mat22); b.Col1.X = invI * vec.Y * vec.Y; b.Col2.X = -invI * vec.X * vec.Y; b.Col1.Y = -invI * vec.X * vec.Y; b.Col2.Y = invI * vec.X * vec.X; Mat22 mat = a + b; mat.Col1.X = mat.Col1.X + this._gamma; mat.Col2.Y = mat.Col2.Y + this._gamma; this._mass = mat.Invert(); this._C = body._sweep.C + vec - this._target; body._angularVelocity *= 0.98f; this._impulse *= step.DtRatio; Body expr_21D = body; expr_21D._linearVelocity += invMass * this._impulse; body._angularVelocity += invI * Vec2.Cross(vec, this._impulse); }