public TestbedCamera(Vec2 initPosition, float initScale, float zoomScaleDiff) { this.transform = new OBBViewportTransform(); transform.setCamera(initPosition.x, initPosition.y, initScale); this.initPosition.set(initPosition); this.initScale = initScale; upScale = Mat22.createScaleTransform(1 + zoomScaleDiff); downScale = Mat22.createScaleTransform(1 - zoomScaleDiff); }
public bool Clip(Vertices clipVertices, Vector2 position) { Mat22 mat = new Mat22(0); Transform t = new Transform(ref position, ref mat); //Transform shape Transform thistransform; Body.GetTransform(out thistransform); //Transform the shape Vertices transformedshape = new Vertices(clipVertices.Count); foreach (Vector2 v in clipVertices) { Vector2 newv = v; newv = MathUtils.Multiply(ref t, ref newv); newv = MathUtils.MultiplyT(ref thistransform, ref newv); transformedshape.Add(newv); } PolyClipError error; List<Vertices> result = YuPengClipper.Difference(Vertices, transformedshape, out error); //Need to check if the entire shape was cut, //so we can destroy/erase it if (result.Count == 0) return false; //The shape was split up, //so create a new DestructableBody for each piece if (result.Count > 1) { //Create a new destructable body for each extra shape for (int i = 1; i < result.Count; i++) { DestructableBody db = new DestructableBody(_world, result[i]); db.Body.Position = Body.Position; } } //Set Shape Vertices newshape = result[0]; SetShape(newshape); return true; }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Rot qA = new Rot(aA); Rot qB = new Rot(aB); // Compute the effective mass matrix. _rA = MathUtils.Mul(qA, -_localCenterA); _rB = MathUtils.Mul(qB, -_localCenterB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Mat22 K = new Mat22(); K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y; K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y; K.ey.X = K.ex.Y; K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X; _linearMass = K.Inverse; _angularMass = iA + iB; if (_angularMass > 0.0f) { _angularMass = 1.0f / _angularMass; } _linearError = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset); _angularError = aB - aA - _angularOffset; if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _linearImpulse *= data.step.dtRatio; _angularImpulse *= data.step.dtRatio; Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse); } else { _linearImpulse = Vector2.Zero; _angularImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override bool SolvePositionConstraints(float baumgarte) { Body b1 = _body1; Body b2 = _body2; Vec2 c1 = b1._sweep.C; float a1 = b1._sweep.A; Vec2 c2 = b2._sweep.C; float a2 = b2._sweep.A; // Solve linear limit constraint. float linearError = 0.0f, angularError = 0.0f; bool active = false; float C2 = 0.0f; Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2); Vec2 r1 = Box2DNet.Common.Math.Mul(R1, _localAnchor1 - _localCenter1); Vec2 r2 = Box2DNet.Common.Math.Mul(R2, _localAnchor2 - _localCenter2); Vec2 d = c2 + r2 - c1 - r1; if (_enableLimit) { _axis = Box2DNet.Common.Math.Mul(R1, _localXAxis1); _a1 = Vec2.Cross(d + r1, _axis); _a2 = Vec2.Cross(r2, _axis); float translation = Vec2.Dot(_axis, d); if (Box2DNet.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = Box2DNet.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Box2DNet.Common.Math.Abs(translation); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = Box2DNet.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = _lowerTranslation - translation; active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = Box2DNet.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = translation - _upperTranslation; active = true; } } _perp = Box2DNet.Common.Math.Mul(R1, _localYAxis1); _s1 = Vec2.Cross(d + r1, _perp); _s2 = Vec2.Cross(r2, _perp); Vec2 impulse; float C1; C1 = Vec2.Dot(_perp, d); linearError = Box2DNet.Common.Math.Max(linearError, Box2DNet.Common.Math.Abs(C1)); angularError = 0.0f; if (active) { float m1 = _invMass1, m2 = _invMass2; float i1 = _invI1, i2 = _invI2; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2; float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; _K.Col1.Set(k11, k12); _K.Col2.Set(k12, k22); Vec2 C = new Vec2(); C.X = C1; C.Y = C2; impulse = _K.Solve(-C); } else { float m1 = _invMass1, m2 = _invMass2; float i1 = _invI1, i2 = _invI2; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float impulse1 = (-C1) / k11; impulse.X = impulse1; impulse.Y = 0.0f; } Vec2 P = impulse.X * _perp + impulse.Y * _axis; float L1 = impulse.X * _s1 + impulse.Y * _a1; float L2 = impulse.X * _s2 + impulse.Y * _a2; c1 -= _invMass1 * P; a1 -= _invI1 * L1; c2 += _invMass2 * P; a2 += _invI2 * L2; // TODO_ERIN remove need for this. b1._sweep.C = c1; b1._sweep.A = a1; b2._sweep.C = c2; b2._sweep.A = a2; b1.SynchronizeTransform(); b2.SynchronizeTransform(); return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override bool SolvePositionConstraints() { // TODO_ERIN block solve with limit. COME ON ERIN Body b1 = BodyA; float angularError = 0.0f; float positionError; // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { float angle = 0 - b1.Sweep.A - ReferenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = Math.Abs(C); } else if (_limitState == LimitState.AtLower) { float C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpper) { float C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } b1.Sweep.A -= b1.InvI * limitImpulse; b1.SynchronizeTransform(); } // Solve point-to-point constraint. { Transform xf1; b1.GetTransform(out xf1); Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = _worldAnchor; Vector2 C = Vector2.zero + r2 - b1.Sweep.C - r1; positionError = C.magnitude; float invMass1 = b1.InvMass; const float invMass2 = 0; float invI1 = b1.InvI; const float invI2 = 0; // Handle large detachment. const float k_allowedStretch = 10.0f * Settings.LinearSlop; if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch) { // Use a particle solution (no rotation). Vector2 u = C; u.Normalize(); float k = invMass1 + invMass2; Debug.Assert(k > Settings.Epsilon); float m = 1.0f / k; Vector2 impulse2 = m * (-C); const float k_beta = 0.5f; b1.Sweep.C -= k_beta * invMass1 * impulse2; C = Vector2.zero + r2 - b1.Sweep.C - r1; } Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2)); Mat22 K2 = new Mat22(new Vector2(invI1 * r1.y * r1.y, -invI1 * r1.x * r1.y), new Vector2(-invI1 * r1.x * r1.y, invI1 * r1.x * r1.x)); Mat22 K3 = new Mat22(new Vector2(invI2 * r2.y * r2.y, -invI2 * r2.x * r2.y), new Vector2(-invI2 * r2.x * r2.y, invI2 * r2.x * r2.x)); Mat22 Ka; Mat22.Add(ref K1, ref K2, out Ka); Mat22 K; Mat22.Add(ref Ka, ref K3, out K); Vector2 impulse = K.Solve(-C); b1.Sweep.C -= b1.InvMass * impulse; b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse); b1.SynchronizeTransform(); } return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override bool SolvePositionConstraints() { // TODO_ERIN block solve with limit. COME ON ERIN Body b1 = BodyA; Body b2 = BodyB; float angularError = 0.0f; float positionError; // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { float angle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections float C = MathHelper.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = Math.Abs(C); } else if (_limitState == LimitState.AtLower) { float C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathHelper.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpper) { float C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathHelper.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } b1.Sweep.A -= b1.InvI * limitImpulse; b2.Sweep.A += b2.InvI * limitImpulse; b1.SynchronizeTransform(); b2.SynchronizeTransform(); } // Solve point-to-point constraint. { /*Transform xf1, xf2; b1.GetTransform(out xf1); b2.GetTransform(out xf2);*/ Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); Vector2 C = b2.Sweep.C + r2 - b1.Sweep.C - r1; positionError = C.Length(); float invMass1 = b1.InvMass, invMass2 = b2.InvMass; float invI1 = b1.InvI, invI2 = b2.InvI; // Handle large detachment. const float k_allowedStretch = 10.0f * Settings.LinearSlop; if (C.LengthSquared() > k_allowedStretch * k_allowedStretch) { // Use a particle solution (no rotation). Vector2 u = C; u.Normalize(); float k = invMass1 + invMass2; Debug.Assert(k > Settings.Epsilon); float m = 1.0f / k; Vector2 impulse2 = m * (-C); const float k_beta = 0.5f; b1.Sweep.C -= k_beta * invMass1 * impulse2; b2.Sweep.C += k_beta * invMass2 * impulse2; C = b2.Sweep.C + r2 - b1.Sweep.C - r1; } Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2)); Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y), new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X)); Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y), new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X)); Mat22 Ka; Mat22.Add(ref K1, ref K2, out Ka); Mat22 K; Mat22.Add(ref Ka, ref K3, out K); Vector2 impulse = K.Solve(-C); b1.Sweep.C -= b1.InvMass * impulse; MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1); b1.Sweep.A -= b1.InvI * /* r1 x impulse */ _tmpFloat1; b2.Sweep.C += b2.InvMass * impulse; MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1); b2.Sweep.A += b2.InvI * /* r2 x impulse */ _tmpFloat1; b1.SynchronizeTransform(); b2.SynchronizeTransform(); } return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; }
internal override void InitVelocityConstraints(ref TimeStep step) { Body b = _bodyB; float mass = b.GetMass(); // Frequency float omega = 2.0f * Settings.b2_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. Debug.Assert(d + step.dt * k > Settings.b2_FLT_EPSILON); _gamma = 1.0f / (step.dt * (d + step.dt * k)); _beta = step.dt * k * _gamma; // Compute the effective mass matrix. XForm xf1; b.GetXForm(out xf1); Vector2 r = MathUtils.Multiply(ref xf1.R, _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(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass)); Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y), new Vector2(-invI * r.X * r.Y, invI * r.X * r.X)); Mat22 K; Mat22.Add(ref K1, ref K2, out K); 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 * MathUtils.Cross(r, _impulse); }
internal override bool SolvePositionConstraints() { //Body b1 = BodyA; Body b2 = BodyB; Vector2 c1 = Vector2.Zero; // b1._sweep.Center; float a1 = 0.0f; // b1._sweep.Angle; Vector2 c2 = b2.Sweep.C; float a2 = b2.Sweep.A; // Solve linear limit constraint. float linearError = 0.0f; bool active = false; float C2 = 0.0f; Mat22 R1 = new Mat22(a1); Mat22 R2 = new Mat22(a2); Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA); Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB); Vector2 d = c2 + r2 - c1 - r1; if (_enableLimit) { _axis = MathUtils.Multiply(ref R1, _localXAxis1); _a1 = MathUtils.Cross(d + r1, _axis); _a2 = MathUtils.Cross(r2, _axis); float translation = Vector2.Dot(_axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Math.Abs(translation); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = _lowerTranslation - translation; active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = translation - _upperTranslation; active = true; } } _perp = MathUtils.Multiply(ref R1, _localYAxis1); _s1 = MathUtils.Cross(d + r1, _perp); _s2 = MathUtils.Cross(r2, _perp); Vector3 impulse; Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle); linearError = Math.Max(linearError, Math.Abs(C1.X)); float angularError = Math.Abs(C1.Y); if (active) { float m1 = InvMassA, m2 = InvMassB; float i1 = InvIA, i2 = InvIB; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float k12 = i1 * _s1 + i2 * _s2; float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; float k22 = i1 + i2; float k23 = i1 * _a1 + i2 * _a2; float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; _K.Col1 = new Vector3(k11, k12, k13); _K.Col2 = new Vector3(k12, k22, k23); _K.Col3 = new Vector3(k13, k23, k33); Vector3 C = new Vector3(-C1.X, -C1.Y, -C2); impulse = _K.Solve33(C); // negated above } else { float m1 = InvMassA, m2 = InvMassB; float i1 = InvIA, i2 = InvIB; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float k12 = i1 * _s1 + i2 * _s2; float k22 = i1 + i2; _K.Col1 = new Vector3(k11, k12, 0.0f); _K.Col2 = new Vector3(k12, k22, 0.0f); Vector2 impulse1 = _K.Solve22(-C1); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } Vector2 P = impulse.X * _perp + impulse.Z * _axis; float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2; c2 += InvMassB * P; a2 += InvIB * L2; // TODO_ERIN remove need for this. b2.Sweep.C = c2; b2.Sweep.A = a2; b2.SynchronizeTransform(); return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; float angularError = 0.0f; float positionError; bool fixedRotation = (_invIA + _invIB == 0.0f); // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { float angle = aB - aA - ReferenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = Math.Abs(C); } else if (_limitState == LimitState.AtLower) { float C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpper) { float C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } aA -= _invIA * limitImpulse; aB += _invIB * limitImpulse; } // Solve point-to-point constraint. { Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 C = cB + rB - cA - rA; positionError = C.Length(); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Mat22 K = new Mat22(); K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y; K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y; K.ey.X = K.ex.Y; K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X; Vector2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * MathUtils.Cross(ref rA, ref impulse); cB += mB * impulse; aB += iB * MathUtils.Cross(ref rB, ref impulse); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
/// <summary> /// Multiplies the obb transform by the given transform /// </summary> /// <param name="argTransform"></param> public virtual void mulByTransform(Mat22 argTransform) { box.R.mulLocal(argTransform); }
public OBBViewportTransform() { yFlipMatInv = yFlipMat.invert(); box.R.setIdentity(); }
internal override bool SolvePositionConstraints(float baumgarte) { Body body = this._body1; Body body2 = this._body2; Vec2 vec = body._sweep.C; float num = body._sweep.A; Vec2 vec2 = body2._sweep.C; float num2 = body2._sweep.A; float num3 = 0f; bool flag = false; float z = 0f; Mat22 a = new Mat22(num); Mat22 a2 = new Mat22(num2); Vec2 v = Box2DX.Common.Math.Mul(a, this._localAnchor1 - this._localCenter1); Vec2 vec3 = Box2DX.Common.Math.Mul(a2, this._localAnchor2 - this._localCenter2); Vec2 vec4 = vec2 + vec3 - vec - v; if (this._enableLimit) { this._axis = Box2DX.Common.Math.Mul(a, this._localXAxis1); this._a1 = Vec2.Cross(vec4 + v, this._axis); this._a2 = Vec2.Cross(vec3, this._axis); float num4 = Vec2.Dot(this._axis, vec4); if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop) { z = Box2DX.Common.Math.Clamp(num4, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); num3 = Box2DX.Common.Math.Abs(num4); flag = true; } else { if (num4 <= this._lowerTranslation) { z = Box2DX.Common.Math.Clamp(num4 - this._lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f); num3 = this._lowerTranslation - num4; flag = true; } else { if (num4 >= this._upperTranslation) { z = Box2DX.Common.Math.Clamp(num4 - this._upperTranslation - Settings.LinearSlop, 0f, Settings.MaxLinearCorrection); num3 = num4 - this._upperTranslation; flag = true; } } } } this._perp = Box2DX.Common.Math.Mul(a, this._localYAxis1); this._s1 = Vec2.Cross(vec4 + v, this._perp); this._s2 = Vec2.Cross(vec3, this._perp); Vec2 v2 = default(Vec2); v2.X = Vec2.Dot(this._perp, vec4); v2.Y = num2 - num - this._refAngle; num3 = Box2DX.Common.Math.Max(num3, Box2DX.Common.Math.Abs(v2.X)); float num5 = Box2DX.Common.Math.Abs(v2.Y); Vec3 vec5; if (flag) { float invMass = this._invMass1; float invMass2 = this._invMass2; float invI = this._invI1; float invI2 = this._invI2; float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2; float num6 = invI * this._s1 + invI2 * this._s2; float num7 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2; float y = invI + invI2; float num8 = invI * this._a1 + invI2 * this._a2; float z2 = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2; this._K.Col1.Set(x, num6, num7); this._K.Col2.Set(num6, y, num8); this._K.Col3.Set(num7, num8, z2); vec5 = this._K.Solve33(-new Vec3 { X = v2.X, Y = v2.Y, Z = z }); } else { float invMass = this._invMass1; float invMass2 = this._invMass2; float invI = this._invI1; float invI2 = this._invI2; float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2; float num6 = invI * this._s1 + invI2 * this._s2; float y = invI + invI2; this._K.Col1.Set(x, num6, 0f); this._K.Col2.Set(num6, y, 0f); Vec2 vec6 = this._K.Solve22(-v2); vec5.X = vec6.X; vec5.Y = vec6.Y; vec5.Z = 0f; } Vec2 v3 = vec5.X * this._perp + vec5.Z * this._axis; float num9 = vec5.X * this._s1 + vec5.Y + vec5.Z * this._a1; float num10 = vec5.X * this._s2 + vec5.Y + vec5.Z * this._a2; vec -= this._invMass1 * v3; num -= this._invI1 * num9; vec2 += this._invMass2 * v3; num2 += this._invI2 * num10; body._sweep.C = vec; body._sweep.A = num; body2._sweep.C = vec2; body2._sweep.A = num2; body.SynchronizeTransform(); body2.SynchronizeTransform(); return(num3 <= Settings.LinearSlop && num5 <= Settings.AngularSlop); }
/// <summary> /// Compute the inverse of this matrix, such that inv(A) * A = identity. /// </summary> public Mat22 Invert() { float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y; Mat22 B = new Mat22(); float det = a * d - b * c; if (det != 0.0f) { det = 1.0f / det; } B.Col1.X = det * d; B.Col2.X = -det * b; B.Col1.Y = -det * c; B.Col2.Y = det * a; return B; }
internal override void InitVelocityConstraints(ref SolverData data) { _indexB = BodyA.IslandIndex; _localCenterB = BodyA.Sweep.LocalCenter; _invMassB = BodyA.InvMass; _invIB = 0; FVector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; FVector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Rot qB = new Rot(aB); float mass = BodyA.Mass; // Frequency float omega = 2.0f * FSSettings.Pi * Frequency; // 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. float h = data.step.dt; Debug.Assert(d + h * k > FSSettings.Epsilon); _gamma = h * (d + h * k); if (!Mathf.Approximately(_gamma, 0.0f)) _gamma = 1.0f / _gamma; _beta = h * k * _gamma; // Compute the effective mass matrix. _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // 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] var K = new Mat22(); K.ex.X = _invMassB + _invIB * _rB.Y * _rB.Y + _gamma; K.ex.Y = -_invIB * _rB.X * _rB.Y; K.ey.X = K.ex.Y; K.ey.Y = _invMassB + _invIB * _rB.X * _rB.X + _gamma; _mass = K.Inverse; _C = cB + _rB - _targetA; _C *= _beta; // Cheat with some damping wB *= 0.98f; // if (Settings.EnableWarmstarting) // { _impulse *= data.step.dtRatio; vB += _invMassB * _impulse; wB += _invIB * MathUtils.Cross(_rB, _impulse); // } // else // _impulse = FVector2.Zero; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
/// <summary> /// Initialize using a position vector and a rotation matrix. /// </summary> /// <param name="position"></param> /// <param name="rotation"></param> public Transform(Vec2 position, Mat22 rotation) { Position = position; R = rotation; }
private static Transform Convert(Matrix4x4 mat) { Vector3 T; Matrix3x3 R; Vector3 S; mat.Decompress (out T, out R, out S); var posA = new XnaVector2 (T.X, T.Y); var rotA = new Mat22 (R[0], R[1], R[3], R[4]); return new Transform (ref posA, ref rotA); }
/** * Multiplies the obb transform by the given transform * * @param argTransform */ public void mulByTransform(Mat22 argTransform) { box.R.mulLocal(argTransform); }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _invMassA = BodyA._invMass; _invIA = BodyA._invI; Vector2 cA = data.Positions[_indexA].C; float aA = data.Positions[_indexA].A; Vector2 vA = data.Velocities[_indexA].V; float wA = data.Velocities[_indexA].W; Rot qA = new Rot(aA); float mass = BodyA.Mass; // Frequency float omega = 2.0f * Settings.Pi * Frequency; // 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. float h = data.Step.dt; Debug.Assert(d + h * k > Settings.Epsilon); _gamma = h * (d + h * k); if (_gamma != 0.0f) { _gamma = 1.0f / _gamma; } _beta = h * k * _gamma; // Compute the effective mass matrix. _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); // 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] Mat22 K = new Mat22(); K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma; K.ex.Y = -_invIA * _rA.X * _rA.Y; K.ey.X = K.ex.Y; K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma; _mass = K.Inverse; _C = cA + _rA - _worldAnchor; _C *= _beta; // Cheat with some damping wA *= 0.98f; #pragma warning disable 162 // ReSharper disable once ConditionIsAlwaysTrueOrFalse if (Settings.EnableWarmstarting) { _impulse *= data.Step.dtRatio; vA += _invMassA * _impulse; wA += _invIA * MathUtils.Cross(_rA, _impulse); } else { _impulse = Vector2.Zero; } #pragma warning restore 162 data.Velocities[_indexA].V = vA; data.Velocities[_indexA].W = wA; }
/** * @see IViewportTransform#setCamera(double, double, double) */ public void setCamera(double x, double y, double scale) { box.center.set(x, y); Mat22.createScaleTransform(scale, box.R); }
internal override void InitVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; // Compute the effective mass matrix. Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.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 invMass1 = b1._invMass, invMass2 = b2._invMass; float invI1 = b1._invI, invI2 = b2._invI; Mat22 K1 = new Mat22(); K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f; K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2; Mat22 K2 = new Mat22(); K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y; K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X; Mat22 K3 = new Mat22(); K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y; K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X; Mat22 K = K1 + K2 + K3; _pivotMass = K.Invert(); _motorMass = 1.0f / (invI1 + invI2); if (_enableMotor == false) { _motorForce = 0.0f; } if (_enableLimit) { float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle; if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop) { _limitState = LimitState.EqualLimits; } else if (jointAngle <= _lowerAngle) { if (_limitState != LimitState.AtLowerLimit) { _limitForce = 0.0f; } _limitState = LimitState.AtLowerLimit; } else if (jointAngle >= _upperAngle) { if (_limitState != LimitState.AtUpperLimit) { _limitForce = 0.0f; } _limitState = LimitState.AtUpperLimit; } else { _limitState = LimitState.InactiveLimit; _limitForce = 0.0f; } } else { _limitForce = 0.0f; } if (step.WarmStarting) { b1._linearVelocity -= Settings.FORCE_SCALE(step.Dt) * invMass1 * _pivotForce; b1._angularVelocity -= Settings.FORCE_SCALE(step.Dt) * invI1 * (Vec2.Cross(r1, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce)); b2._linearVelocity += Settings.FORCE_SCALE(step.Dt) * invMass2 * _pivotForce; b2._angularVelocity += Settings.FORCE_SCALE(step.Dt) * invI2 * (Vec2.Cross(r2, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce)); } else { _pivotForce.SetZero(); _motorForce = 0.0f; _limitForce = 0.0f; } _limitPositionImpulse = 0.0f; }
public static Vector2 MulT(ref Mat22 A, Vector2 v) { return(MulT(ref A, ref v)); }
internal override bool SolvePositionConstraints() { Body b2 = BodyB; Vector2 c1 = Vector2.Zero; float a1 = 0.0f; Vector2 c2 = b2.Sweep.c; float a2 = b2.Sweep.a; // Solve linear limit constraint. float linearError = 0.0f; bool active = false; float C2 = 0.0f; Mat22 R1 = new Mat22(a1); Mat22 R2 = new Mat22(a2); Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA); Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB); Vector2 d = c2 + r2 - c1 - r1; if (_enableLimit) { _axis = MathUtils.Multiply(ref R1, _localXAxis1); _a1 = MathUtils.Cross(d + r1, _axis); _a2 = MathUtils.Cross(r2, _axis); float translation = Vector2.Dot(_axis, d); if (Math.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Math.Abs(translation); active = true; } else if (translation <= LowerLimit) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - LowerLimit + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = LowerLimit - translation; active = true; } else if (translation >= UpperLimit) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - UpperLimit - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = translation - UpperLimit; active = true; } } _perp = MathUtils.Multiply(ref R1, _localYAxis1); _s1 = MathUtils.Cross(d + r1, _perp); _s2 = MathUtils.Cross(r2, _perp); Vector2 impulse; float C1 = Vector2.Dot(_perp, d); linearError = Math.Max(linearError, Math.Abs(C1)); const float angularError = 0.0f; if (active) { float m1 = InvMassA, m2 = InvMassB; float i1 = InvIA, i2 = InvIB; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2; float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; _K.col1 = new Vector2(k11, k12); _K.col2 = new Vector2(k12, k22); Vector2 C = new Vector2(-C1, -C2); impulse = _K.Solve(C); // note i inverted above } else { float m1 = InvMassA, m2 = InvMassB; float i1 = InvIA, i2 = InvIB; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float impulse1; if (k11 != 0.0f) { impulse1 = -C1 / k11; } else { impulse1 = 0.0f; } impulse.X = impulse1; impulse.Y = 0.0f; } Vector2 P = impulse.X * _perp + impulse.Y * _axis; float L2 = impulse.X * _s2 + impulse.Y * _a2; c2 += InvMassB * P; a2 += InvIB * L2; // TODO_ERIN remove need for this. b2.Sweep.c = c2; b2.Sweep.a = a2; b2.SynchronizeTransform(); return (linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); // Compute the effective mass matrix. _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Mat22 K = new Mat22(); K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y; K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y; K.ey.X = K.ex.Y; K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X; _linearMass = K.Inverse; _angularMass = iA + iB; if (_angularMass > 0.0f) { _angularMass = 1.0f / _angularMass; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _linearImpulse *= data.step.dtRatio; _angularImpulse *= data.step.dtRatio; Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse); } else { _linearImpulse = Vector2.Zero; _angularImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
/** * Multiplies the obb transform by the given transform */ public void mulByTransform(Mat22 transform) { box.R.mulLocal(transform); }
internal override bool SolvePositionConstraints() { Body b1 = _body1; Body b2 = _body2; float positionError = 0.0f; // Solve point-to-point position error. Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vec2 p1 = b1._sweep.C + r1; Vec2 p2 = b2._sweep.C + r2; Vec2 ptpC = p2 - p1; positionError = ptpC.Length(); // Prevent overly large corrections. //b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection); //ptpC = b2Clamp(ptpC, -dpMax, dpMax); float invMass1 = b1._invMass, invMass2 = b2._invMass; float invI1 = b1._invI, invI2 = b2._invI; Mat22 K1 = new Mat22(); K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f; K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2; Mat22 K2 = new Mat22(); K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y; K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X; Mat22 K3 = new Mat22(); K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y; K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X; Mat22 K = K1 + K2 + K3; Vec2 impulse = K.Solve(-ptpC); b1._sweep.C -= b1._invMass * impulse; b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse); b2._sweep.C += b2._invMass * impulse; b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse); b1.SynchronizeTransform(); b2.SynchronizeTransform(); // Handle limits. float angularError = 0.0f; if (_enableLimit && _limitState != LimitState.InactiveLimit) { float angle = b2._sweep.A - b1._sweep.A - _referenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.EqualLimits) { // Prevent large angular corrections float limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * limitC; angularError = Box2DXMath.Abs(limitC); } else if (_limitState == LimitState.AtLowerLimit) { float limitC = angle - _lowerAngle; angularError = Box2DXMath.Max(0.0f, -limitC); // Prevent large angular corrections and allow some slop. limitC = Box2DXMath.Clamp(limitC + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * limitC; float oldLimitImpulse = _limitPositionImpulse; _limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f); limitImpulse = _limitPositionImpulse - oldLimitImpulse; } else if (_limitState == LimitState.AtUpperLimit) { float limitC = angle - _upperAngle; angularError = Box2DXMath.Max(0.0f, limitC); // Prevent large angular corrections and allow some slop. limitC = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * limitC; float oldLimitImpulse = _limitPositionImpulse; _limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f); limitImpulse = _limitPositionImpulse - oldLimitImpulse; } b1._sweep.A -= b1._invI * limitImpulse; b2._sweep.A += b2._invI * limitImpulse; b1.SynchronizeTransform(); b2.SynchronizeTransform(); } return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
/** * Sets the transform of the viewport. Transforms about the center. */ public void setTransform(Mat22 transform) { box.R.set(transform); }
internal override bool SolvePositionConstraints() { Body bB = BodyB; Vector2 xA = Vector2.Zero; const float angleA = 0.0f; Vector2 xB = bB.Sweep.C; float angleB = bB.Sweep.A; Mat22 RA = new Mat22(angleA); Mat22 RB = new Mat22(angleB); Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA); Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB); Vector2 d = xB + rB - xA - rA; Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA); float sBy = MathUtils.Cross(rB, ay); float C = Vector2.Dot(d, ay); float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy; float impulse; if (k != 0.0f) { impulse = -C / k; } else { impulse = 0.0f; } Vector2 P = impulse * ay; float LB = impulse * sBy; xB += InvMassB * P; angleB += InvIB * LB; // TODO_ERIN remove need for this. bB.Sweep.C = xB; bB.Sweep.A = angleB; bB.SynchronizeTransform(); return Math.Abs(C) <= Settings.LinearSlop; }
internal override void InitVelocityConstraints(ref TimeStep step) { Body bA = BodyA; Transform xfA; bA.GetTransform(out xfA); // Compute the effective mass Matrix4x4. Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] float mA = bA.InvMass; float iA = bA.InvI; Mat22 K1 = new Mat22(); K1.Col1.x = mA; K1.Col2.x = 0.0f; K1.Col1.y = 0.0f; K1.Col2.y = mA; Mat22 K2 = new Mat22(); K2.Col1.x = iA * rA.y * rA.y; K2.Col2.x = -iA * rA.x * rA.y; K2.Col1.y = -iA * rA.x * rA.y; K2.Col2.y = iA * rA.x * rA.x; Mat22 K12; Mat22.Add(ref K1, ref K2, out K12); _linearMass = K12.Inverse; _angularMass = iA; if (_angularMass > 0.0f) { _angularMass = 1.0f / _angularMass; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _linearImpulse *= step.dtRatio; _angularImpulse *= step.dtRatio; Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y); bA.LinearVelocityInternal -= mA * P; bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse); } else { _linearImpulse = Vector2.zero; _angularImpulse = 0.0f; } }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _invMassA = BodyA._invMass; _invIA = BodyA._invI; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Rot qA = new Rot(aA); float mass = BodyA.Mass; // Frequency float omega = 2.0f * Settings.Pi * Frequency; // 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. float h = data.step.dt; Debug.Assert(d + h * k > Settings.Epsilon); _gamma = h * (d + h * k); if (_gamma != 0.0f) { _gamma = 1.0f / _gamma; } _beta = h * k * _gamma; // Compute the effective mass matrix. _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); // 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] Mat22 K = new Mat22(); K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma; K.ex.Y = -_invIA * _rA.X * _rA.Y; K.ey.X = K.ex.Y; K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma; _mass = K.Inverse; _C = cA + _rA - _worldAnchor; _C *= _beta; // Cheat with some damping wA *= 0.98f; if (Settings.EnableWarmstarting) { _impulse *= data.step.dtRatio; vA += _invMassA * _impulse; wA += _invIA * MathUtils.Cross(_rA, _impulse); } else { _impulse = Vector2.Zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; }
private void ConvertToObj(MeshData mesh, string filename = "object", params bool[] flags) { mesh = mesh.Clone(); try { Queue <float> uvsq = new Queue <float>(); for (int i = 0; i < mesh.Uv.Length; i++) { if (i + 4 > mesh.UvCount) { continue; } float[] transform = new float[] { mesh.Uv[i], mesh.Uv[++i], mesh.Uv[++i], mesh.Uv[++i] }; if (flags[0]) { Mat22.Scale(transform, transform, new float[] { capi.BlockTextureAtlas.Size.Width / 32, -(capi.BlockTextureAtlas.Size.Height / 32) }); Mat22X.Translate(transform, transform, new float[] { 0.0f, 1.0f }); } if (flags[1]) { Mat22.Scale(transform, transform, new float[] { 1.0f, -1.0f }); Mat22X.Translate(transform, transform, new float[] { 0.0f, 1.0f }); } for (int j = 0; j < transform.Length; j++) { uvsq.Enqueue(transform[j]); } } mesh.Translate(-0.5f, -0.5f, -0.5f); float[] uvs = uvsq.ToArray(); using (TextWriter tw = new StreamWriter(Path.Combine(GamePaths.Binaries, filename + ".obj"))) { tw.WriteLine("o " + filename); for (int i = 0; i < mesh.xyz.Length; i++) { if (i % 3 == 0) { if (i != 0) { tw.WriteLine(); } tw.Write("v " + mesh.xyz[i].ToString("F6")); } else { tw.Write(" " + mesh.xyz[i].ToString("F6")); } } tw.WriteLine(); for (int i = 0; i < uvs.Length; i++) { if (i % 2 == 0) { if (i != 0) { tw.WriteLine(); } tw.Write("vt " + uvs[i].ToString("F6")); } else { tw.Write(" " + uvs[i].ToString("F6")); } } tw.WriteLine(); for (int i = 0; i < mesh.Indices.Length; i++) { tw.WriteLine( "f " + (mesh.Indices[i] + 1) + "/" + (mesh.Indices[i] + 1) + " " + (mesh.Indices[++i] + 1) + "/" + (mesh.Indices[i] + 1) + " " + (mesh.Indices[++i] + 1) + "/" + (mesh.Indices[i] + 1)); } tw.Close(); } } catch (Exception) { } }
internal override bool SolvePositionConstraints(ref SolverData data) { FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float angularError = 0.0f; float positionError; bool fixedRotation = (m_invIA + m_invIB == 0.0f); // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { float angle = aB - aA - ReferenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -m_motorMass * C; angularError = Math.Abs(C); } else if (_limitState == LimitState.AtLower) { float C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -m_motorMass * C; } else if (_limitState == LimitState.AtUpper) { float C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -m_motorMass * C; } aA -= m_invIA * limitImpulse; aB += m_invIB * limitImpulse; } // Solve point-to-point constraint. { qA.Set(aA); qB.Set(aB); FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); FVector2 C = cB + rB - cA - rA; positionError = C.Length(); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Mat22 K = new Mat22(); K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y; K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y; K.ey.X = K.ex.Y; K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X; FVector2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * MathUtils.Cross(rA, impulse); cB += mB * impulse; aB += iB * MathUtils.Cross(rB, impulse); } data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; }
/// <summary> /// Multiplies the obb transform by the given transform /// </summary> /// <param name="argTransform"></param> public void MulByTransform(Mat22 argTransform) { Box.R.MulLocal(argTransform); }
internal override bool SolvePositionConstraints() { // TODO_ERIN block solve with limit. COME ON ERIN Body b1 = BodyA; float angularError = 0.0f; float positionError; // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { float angle = 0 - b1.Sweep.A - ReferenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = Math.Abs(C); } else if (_limitState == LimitState.AtLower) { float C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpper) { float C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } b1.Sweep.A -= b1.InvI * limitImpulse; b1.SynchronizeTransform(); } // Solve point-to-point constraint. { Transform xf1; b1.GetTransform(out xf1); Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = _worldAnchor; Vector2 C = Vector2.zero + r2 - b1.Sweep.C - r1; positionError = C.magnitude; float invMass1 = b1.InvMass; const float invMass2 = 0; float invI1 = b1.InvI; const float invI2 = 0; // Handle large detachment. const float k_allowedStretch = 10.0f * Settings.LinearSlop; if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch) { // Use a particle solution (no rotation). Vector2 u = C; u.Normalize(); float k = invMass1 + invMass2; Debug.Assert(k > Settings.Epsilon); float m = 1.0f / k; Vector2 impulse2 = m * (-C); const float k_beta = 0.5f; b1.Sweep.C -= k_beta * invMass1 * impulse2; C = Vector2.zero + r2 - b1.Sweep.C - r1; } Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2)); Mat22 K2 = new Mat22(new Vector2(invI1 * r1.y * r1.y, -invI1 * r1.x * r1.y), new Vector2(-invI1 * r1.x * r1.y, invI1 * r1.x * r1.x)); Mat22 K3 = new Mat22(new Vector2(invI2 * r2.y * r2.y, -invI2 * r2.x * r2.y), new Vector2(-invI2 * r2.x * r2.y, invI2 * r2.x * r2.x)); Mat22 Ka; Mat22.Add(ref K1, ref K2, out Ka); Mat22 K; Mat22.Add(ref Ka, ref K3, out K); Vector2 impulse = K.Solve(-C); b1.Sweep.C -= b1.InvMass * impulse; b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse); b1.SynchronizeTransform(); } return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; }
public OBBViewportTransform() { YFlip = false; yFlipMatInv = yFlipMat.Invert(); Box.R.SetIdentity(); }
/** * Sets the transform of the viewport. Transforms about the center. * * @param transform */ public void setTransform(Mat22 transform) { box.R.set(transform); }
internal override void InitVelocityConstraints(ref TimeStep step) { Body bA = BodyA; Transform xfA; bA.GetTransform(out xfA); // Compute the effective mass matrix. Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] float mA = bA.InvMass; float iA = bA.InvI; Mat22 K1 = new Mat22(); K1.Col1.X = mA; K1.Col2.X = 0.0f; K1.Col1.Y = 0.0f; K1.Col2.Y = mA; Mat22 K2 = new Mat22(); K2.Col1.X = iA * rA.Y * rA.Y; K2.Col2.X = -iA * rA.X * rA.Y; K2.Col1.Y = -iA * rA.X * rA.Y; K2.Col2.Y = iA * rA.X * rA.X; Mat22 K12; Mat22.Add(ref K1, ref K2, out K12); _linearMass = K12.Inverse; _angularMass = iA; if (_angularMass > 0.0f) { _angularMass = 1.0f / _angularMass; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _linearImpulse *= step.dtRatio; _angularImpulse *= step.dtRatio; Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); bA.LinearVelocityInternal -= mA * P; bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse); } else { _linearImpulse = Vector2.Zero; _angularImpulse = 0.0f; } }
public OBBViewportTransform() { box.R.setIdentity(); yFlipMatInv = yFlipMat.invert(); }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; FP aA = data.positions[_indexA].a; TSVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FP aB = data.positions[_indexB].a; TSVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); // Compute the effective mass matrix. _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; Mat22 K = new Mat22(); K.ex.x = mA + mB + iA * _rA.y * _rA.y + iB * _rB.y * _rB.y; K.ex.y = -iA * _rA.x * _rA.y - iB * _rB.x * _rB.y; K.ey.x = K.ex.y; K.ey.y = mA + mB + iA * _rA.x * _rA.x + iB * _rB.x * _rB.x; _linearMass = K.Inverse; _angularMass = iA + iB; if (_angularMass > 0.0f) { _angularMass = 1.0f / _angularMass; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _linearImpulse *= data.step.dtRatio; _angularImpulse *= data.step.dtRatio; TSVector2 P = new TSVector2(_linearImpulse.x, _linearImpulse.y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse); } else { _linearImpulse = TSVector2.zero; _angularImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override bool solvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Compute fresh Jacobians Vector2 rA = MathUtils.mul(qA, localAnchorA - _localCenterA); Vector2 rB = MathUtils.mul(qB, localAnchorB - _localCenterB); Vector2 d = cB + rB - cA - rA; Vector2 axis = MathUtils.mul(qA, localXAxis); float a1 = MathUtils.cross(d + rA, axis); float a2 = MathUtils.cross(rB, axis); Vector2 perp = MathUtils.mul(qA, _localYAxisA); float s1 = MathUtils.cross(d + rA, perp); float s2 = MathUtils.cross(rB, perp); Vector3 impulse; Vector2 C1 = new Vector2(); C1.X = Vector2.Dot(perp, d); C1.Y = aB - aA - referenceAngle; float linearError = Math.Abs(C1.X); float angularError = Math.Abs(C1.Y); bool active = false; float C2 = 0.0f; if (_enableLimit) { float translation = Vector2.Dot(axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.linearSlop) { // Prevent large angular corrections C2 = MathUtils.clamp(translation, -Settings.maxLinearCorrection, Settings.maxLinearCorrection); linearError = Math.Max(linearError, Math.Abs(translation)); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.clamp(translation - _lowerTranslation + Settings.linearSlop, -Settings.maxLinearCorrection, 0.0f); linearError = Math.Max(linearError, _lowerTranslation - translation); active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.clamp(translation - _upperTranslation - Settings.linearSlop, 0.0f, Settings.maxLinearCorrection); linearError = Math.Max(linearError, translation - _upperTranslation); active = true; } } if (active) { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k13 = iA * s1 * a1 + iB * s2 * a2; float k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } float k23 = iA * a1 + iB * a2; float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = new Mat33(); K.ex = new Vector3(k11, k12, k13); K.ey = new Vector3(k12, k22, k23); K.ez = new Vector3(k13, k23, k33); Vector3 C = new Vector3(); C.X = C1.X; C.Y = C1.Y; C.Z = C2; impulse = K.Solve33(-C); } else { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } Mat22 K = new Mat22(); K.ex = new Vector2(k11, k12); K.ey = new Vector2(k12, k22); Vector2 impulse1 = K.Solve(-C1); impulse = new Vector3(); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } Vector2 P = impulse.X * perp + impulse.Z * axis; float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2; cA -= mA * P; aA -= iA * LA; cB += mB * P; aB += iB * LB; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(linearError <= Settings.linearSlop && angularError <= Settings.angularSlop); }
public override bool solvePositionConstraints(SolverData data) { Rot qA = pool.popRot(); Rot qB = pool.popRot(); Vec2 rA = pool.popVec2(); Vec2 rB = pool.popVec2(); Vec2 d = pool.popVec2(); Vec2 axis = pool.popVec2(); Vec2 perp = pool.popVec2(); Vec2 temp = pool.popVec2(); Vec2 C1 = pool.popVec2(); Vec3 impulse = pool.popVec3(); Vec2 cA = data.positions[m_indexA].c; double aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; double aB = data.positions[m_indexB].a; qA.set(aA); qB.set(aB); double mA = m_invMassA, mB = m_invMassB; double iA = m_invIA, iB = m_invIB; // Compute fresh Jacobians Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); Rot.mulToOutUnsafe(qA, m_localXAxisA, axis); double a1 = Vec2.cross(temp.set(d).addLocal(rA), axis); double a2 = Vec2.cross(rB, axis); Rot.mulToOutUnsafe(qA, m_localYAxisA, perp); double s1 = Vec2.cross(temp.set(d).addLocal(rA), perp); double s2 = Vec2.cross(rB, perp); C1.x = Vec2.dot(perp, d); C1.y = aB - aA - m_referenceAngle; double linearError = MathUtils.abs(C1.x); double angularError = MathUtils.abs(C1.y); bool active = false; double C2 = 0.0d; if (m_enableLimit) { double translation = Vec2.dot(axis, d); if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0d * Settings.linearSlop) { // Prevent large angular corrections C2 = MathUtils.clamp(translation, -Settings.maxLinearCorrection, Settings.maxLinearCorrection); linearError = MathUtils.max(linearError, MathUtils.abs(translation)); active = true; } else if (translation <= m_lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.clamp(translation - m_lowerTranslation + Settings.linearSlop, -Settings.maxLinearCorrection, 0.0d); linearError = MathUtils.max(linearError, m_lowerTranslation - translation); active = true; } else if (translation >= m_upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.clamp(translation - m_upperTranslation - Settings.linearSlop, 0.0d, Settings.maxLinearCorrection); linearError = MathUtils.max(linearError, translation - m_upperTranslation); active = true; } } if (active) { double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; double k12 = iA * s1 + iB * s2; double k13 = iA * s1 * a1 + iB * s2 * a2; double k22 = iA + iB; if (k22 == 0.0d) { // For fixed rotation k22 = 1.0d; } double k23 = iA * a1 + iB * a2; double k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = pool.popMat33(); K.ex.set(k11, k12, k13); K.ey.set(k12, k22, k23); K.ez.set(k13, k23, k33); Vec3 C = pool.popVec3(); C.x = C1.x; C.y = C1.y; C.z = C2; K.solve33ToOut(C.negateLocal(), impulse); pool.pushVec3(1); pool.pushMat33(1); } else { double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; double k12 = iA * s1 + iB * s2; double k22 = iA + iB; if (k22 == 0.0d) { k22 = 1.0d; } Mat22 K = pool.popMat22(); K.ex.set(k11, k12); K.ey.set(k12, k22); // temp is impulse1 K.solveToOut(C1.negateLocal(), temp); C1.negateLocal(); impulse.x = temp.x; impulse.y = temp.y; impulse.z = 0.0d; pool.pushMat22(1); } double Px = impulse.x * perp.x + impulse.z * axis.x; double Py = impulse.x * perp.y + impulse.z * axis.y; double LA = impulse.x * s1 + impulse.y + impulse.z * a1; double LB = impulse.x * s2 + impulse.y + impulse.z * a2; cA.x -= mA * Px; cA.y -= mA * Py; aA -= iA * LA; cB.x += mB * Px; cB.y += mB * Py; aB += iB * LB; // data.positions[m_indexA].c.set(cA); data.positions[m_indexA].a = aA; // data.positions[m_indexB].c.set(cB); data.positions[m_indexB].a = aB; pool.pushVec2(7); pool.pushVec3(1); pool.pushRot(2); return(linearError <= Settings.linearSlop && angularError <= Settings.angularSlop); }
public static Vector2 Mul(ref Mat22 A, ref Vector2 v) { return(new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y)); }
internal override void initVelocityConstraints(ref SolverData data) { _indexA = bodyA.islandIndex; _localCenterA = bodyA._sweep.localCenter; _invMassA = bodyA._invMass; _invIA = bodyA._invI; var cA = data.positions[_indexA].c; var aA = data.positions[_indexA].a; var vA = data.velocities[_indexA].v; var wA = data.velocities[_indexA].w; var qA = new Rot(aA); float mass = bodyA.mass; // Frequency float omega = 2.0f * Settings.pi * frequency; // 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. float h = data.step.dt; Debug.Assert(d + h * k > Settings.epsilon, "damping is less than Epsilon. Does the body have mass?"); _gamma = h * (d + h * k); if (_gamma != 0.0f) { _gamma = 1.0f / _gamma; } _beta = h * k * _gamma; // Compute the effective mass matrix. _rA = MathUtils.mul(qA, localAnchorA - _localCenterA); // 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] var K = new Mat22(); K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma; K.ex.Y = -_invIA * _rA.X * _rA.Y; K.ey.X = K.ex.Y; K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma; _mass = K.Inverse; _C = cA + _rA - _worldAnchor; _C *= _beta; // Cheat with some damping wA *= 0.98f; if (Settings.enableWarmstarting) { _impulse *= data.step.dtRatio; vA += _invMassA * _impulse; wA += _invIA * MathUtils.cross(_rA, _impulse); } else { _impulse = Vector2.Zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; }
public static Vector2 MulT(ref Mat22 A, ref Vector2 v) { return(new Vector2(v.X * A.ex.X + v.Y * A.ex.Y, v.X * A.ey.X + v.Y * A.ey.Y)); }
/// <summary> /// Muls the t using the specified a /// </summary> /// <param name="a">The </param> /// <param name="v">The </param> /// <returns>The vector</returns> public static Vector2 MulT(ref Mat22 a, ref Vector2 v) => new Vector2(v.X * a.Ex.X + v.Y * a.Ex.Y, v.X * a.Ey.X + v.Y * a.Ey.Y);
internal override void InitVelocityConstraints(ref TimeStep step) { Body b = BodyA; float mass = b.Mass; // Frequency float omega = 2.0f * Settings.Pi * Frequency; // 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. Debug.Assert(d + step.dt * k > Settings.Epsilon); _gamma = step.dt * (d + step.dt * k); if (_gamma != 0.0f) { _gamma = 1.0f / _gamma; } _beta = step.dt * k * _gamma; // Compute the effective mass matrix. Transform xf1; b.GetTransform(out xf1); Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter); // 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(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass)); Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y), new Vector2(-invI * r.X * r.Y, invI * r.X * r.X)); Mat22 K; Mat22.Add(ref K1, ref K2, out K); K.Col1.X += _gamma; K.Col2.Y += _gamma; _mass = K.Inverse; _C = b.Sweep.C + r - _worldAnchor; // Cheat with some damping b.AngularVelocityInternal *= 0.98f; // Warm starting. _impulse *= step.dtRatio; b.LinearVelocityInternal += invMass * _impulse; b.AngularVelocityInternal += invI * MathUtils.Cross(r, _impulse); }
/// <summary> /// Muls the a /// </summary> /// <param name="a">The </param> /// <param name="v">The </param> /// <returns>The vector</returns> public static Vector2 Mul(ref Mat22 a, Vector2 v) => Mul(ref a, ref v);
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Compute fresh Jacobians Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 d = cB + rB - cA - rA; Vector2 axis = MathUtils.Mul(qA, LocalXAxis); float a1 = MathUtils.Cross(d + rA, axis); float a2 = MathUtils.Cross(rB, axis); Vector2 perp = MathUtils.Mul(qA, _localYAxisA); float s1 = MathUtils.Cross(d + rA, perp); float s2 = MathUtils.Cross(rB, perp); Vector3 impulse; Vector2 C1 = new Vector2(); C1.X = Vector2.Dot(perp, d); C1.Y = aB - aA - ReferenceAngle; float linearError = Math.Abs(C1.X); float angularError = Math.Abs(C1.Y); bool active = false; float C2 = 0.0f; if (_enableLimit) { float translation = Vector2.Dot(axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Math.Max(linearError, Math.Abs(translation)); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = Math.Max(linearError, _lowerTranslation - translation); active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = Math.Max(linearError, translation - _upperTranslation); active = true; } } if (active) { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k13 = iA * s1 * a1 + iB * s2 * a2; float k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } float k23 = iA * a1 + iB * a2; float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = new Mat33(); K.ex = new Vector3(k11, k12, k13); K.ey = new Vector3(k12, k22, k23); K.ez = new Vector3(k13, k23, k33); Vector3 C = new Vector3(); C.X = C1.X; C.Y = C1.Y; C.Z = C2; impulse = K.Solve33(-C); } else { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } Mat22 K = new Mat22(); K.ex = new Vector2(k11, k12); K.ey = new Vector2(k12, k22); Vector2 impulse1 = K.Solve(-C1); impulse = new Vector3(); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } Vector2 P = impulse.X * perp + impulse.Z * axis; float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2; cA -= mA * P; aA -= iA * LA; cB += mB * P; aB += iB * LB; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; }
/// <summary> /// Muls the a /// </summary> /// <param name="a">The </param> /// <param name="v">The </param> /// <returns>The vector</returns> public static Vector2 Mul(ref Mat22 a, ref Vector2 v) => new Vector2(a.Ex.X * v.X + a.Ey.X * v.Y, a.Ex.Y * v.X + a.Ey.Y * v.Y);
internal override bool SolvePositionConstraints(float baumgarte) { Body b1 = _bodyA; Body b2 = _bodyB; Vector2 c1 = b1._sweep.c; float a1 = b1._sweep.a; Vector2 c2 = b2._sweep.c; float a2 = b2._sweep.a; // Solve linear limit raint. float linearError = 0.0f, angularError = 0.0f; bool active = false; float C2 = 0.0f; Mat22 R1 = new Mat22(a1); Mat22 R2 = new Mat22(a2); Vector2 r1 = MathUtils.Multiply(ref R1, _localAnchor1 - _localCenter1); Vector2 r2 = MathUtils.Multiply(ref R2, _localAnchor2 - _localCenter2); Vector2 d = c2 + r2 - c1 - r1; if (_enableLimit) { _axis = MathUtils.Multiply(ref R1, _localXAxis1); _a1 = MathUtils.Cross(d + r1, _axis); _a2 = MathUtils.Cross(r2, _axis); float translation = Vector2.Dot(_axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection); linearError = Math.Abs(translation); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f); linearError = _lowerTranslation - translation; active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.b2_linearSlop, 0.0f, Settings.b2_maxLinearCorrection); linearError = translation - _upperTranslation; active = true; } } _perp = MathUtils.Multiply(ref R1, _localYAxis1); _s1 = MathUtils.Cross(d + r1, _perp); _s2 = MathUtils.Cross(r2, _perp); Vector2 impulse; float C1; C1 = Vector2.Dot(_perp, d); linearError = Math.Max(linearError, Math.Abs(C1)); angularError = 0.0f; if (active) { float m1 = _invMass1, m2 = _invMass2; float i1 = _invI1, i2 = _invI2; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2; float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; _K.col1 = new Vector2(k11, k12); _K.col2 = new Vector2(k12, k22); Vector2 C = new Vector2(-C1, -C2); impulse = _K.Solve(C); //note i inverted above } else { float m1 = _invMass1, m2 = _invMass2; float i1 = _invI1, i2 = _invI2; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float impulse1 = (-C1) / k11; impulse.X = impulse1; impulse.Y = 0.0f; } Vector2 P = impulse.X * _perp + impulse.Y * _axis; float L1 = impulse.X * _s1 + impulse.Y * _a1; float L2 = impulse.X * _s2 + impulse.Y * _a2; c1 -= _invMass1 * P; a1 -= _invI1 * L1; c2 += _invMass2 * P; a2 += _invI2 * L2; // TODO_ERIN remove need for this. b1._sweep.c = c1; b1._sweep.a = a1; b2._sweep.c = c2; b2._sweep.a = a2; b1.SynchronizeTransform(); b2.SynchronizeTransform(); return linearError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop; }
internal override bool SolvePositionConstraints(ref SolverData data) { FPVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; FPVector2 cB = data.positions[_indexB].c; FP aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); FP angularError = 0.0f; FP positionError; bool fixedRotation = (_invIA + _invIB == 0.0f); // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { FP angle = aB - aA - ReferenceAngle; FP limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections FP C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = FP.Abs(C); } else if (_limitState == LimitState.AtLower) { FP C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpper) { FP C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } aA -= _invIA * limitImpulse; aB += _invIB * limitImpulse; } // Solve point-to-point constraint. { qA.Set(aA); qB.Set(aB); FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); FPVector2 C = cB + rB - cA - rA; positionError = C.magnitude; FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; Mat22 K = new Mat22(); K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y; K.ey.x = K.ex.y; K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; FPVector2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * MathUtils.Cross(rA, impulse); cB += mB * impulse; aB += iB * MathUtils.Cross(rB, impulse); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override void InitVelocityConstraints(ref TimeStep step) { Body bA = BodyA; Body bB = BodyB; Transform xfA, xfB; bA.GetTransform(out xfA); bB.GetTransform(out xfB); // Compute the effective mass matrix. Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] float mA = bA.InvMass, mB = bB.InvMass; float iA = bA.InvI, iB = bB.InvI; Mat22 K1 = new Mat22(); K1.Col1.X = mA + mB; K1.Col2.X = 0.0f; K1.Col1.Y = 0.0f; K1.Col2.Y = mA + mB; Mat22 K2 = new Mat22(); K2.Col1.X = iA*rA.Y*rA.Y; K2.Col2.X = -iA*rA.X*rA.Y; K2.Col1.Y = -iA*rA.X*rA.Y; K2.Col2.Y = iA*rA.X*rA.X; Mat22 K3 = new Mat22(); K3.Col1.X = iB*rB.Y*rB.Y; K3.Col2.X = -iB*rB.X*rB.Y; K3.Col1.Y = -iB*rB.X*rB.Y; K3.Col2.Y = iB*rB.X*rB.X; Mat22 K12; Mat22.Add(ref K1, ref K2, out K12); Mat22 K; Mat22.Add(ref K12, ref K3, out K); _linearMass = K.Inverse; _angularMass = iA + iB; if (_angularMass > 0.0f) { _angularMass = 1.0f/_angularMass; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _linearImpulse *= step.dtRatio; _angularImpulse *= step.dtRatio; Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); bA.LinearVelocityInternal -= mA*P; bA.AngularVelocityInternal -= iA*(MathUtils.Cross(rA, P) + _angularImpulse); bB.LinearVelocityInternal += mB*P; bB.AngularVelocityInternal += iB*(MathUtils.Cross(rB, P) + _angularImpulse); } else { _linearImpulse = Vector2.Zero; _angularImpulse = 0.0f; } }
public void SolveVelocityConstraints() { for (int i = 0; i < Count; ++i) { ContactVelocityConstraint vc = VelocityConstraints[i]; int indexA = vc.IndexA; int indexB = vc.IndexB; float mA = vc.InvMassA; float mB = vc.InvMassB; float iA = vc.InvIA; float iB = vc.InvIB; int pointCount = vc.PointCount; Vec2 vA = Velocities[indexA].V; float wA = Velocities[indexA].W; Vec2 vB = Velocities[indexB].V; float wB = Velocities[indexB].W; //Debug.Assert(wA == 0); //Debug.Assert(wB == 0); Vec2 normal = vc.Normal; //Vec2.crossToOutUnsafe(normal, 1f, tangent); tangent.X = 1.0f * vc.Normal.Y; tangent.Y = (-1.0f) * vc.Normal.X; float friction = vc.Friction; Debug.Assert(pointCount == 1 || pointCount == 2); // Solve tangent constraints for (int j = 0; j < pointCount; ++j) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j]; //Vec2.crossToOutUnsafe(wA, vcp.rA, temp); //Vec2.crossToOutUnsafe(wB, vcp.rB, dv); //dv.addLocal(vB).subLocal(vA).subLocal(temp); Vec2 a = vcp.RA; dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a.Y; dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a.X; // Compute tangent force float vt = dv.X * tangent.X + dv.Y * tangent.Y - vc.TangentSpeed; float lambda = vcp.TangentMass * (-vt); // Clamp the accumulated force float maxFriction = friction * vcp.NormalImpulse; float newImpulse = MathUtils.Clamp(vcp.TangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - vcp.TangentImpulse; vcp.TangentImpulse = newImpulse; // Apply contact impulse // Vec2 P = lambda * tangent; float Px = tangent.X * lambda; float Py = tangent.Y * lambda; // vA -= invMassA * P; vA.X -= Px * mA; vA.Y -= Py * mA; wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px); // vB += invMassB * P; vB.X += Px * mB; vB.Y += Py * mB; wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px); //Console.WriteLine("tangent solve velocity (point "+j+") for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA); //Console.WriteLine("tangent solve velocity (point "+j+") for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB); } // Solve normal constraints if (vc.PointCount == 1) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[0]; Vec2 a1 = vcp.RA; // Relative velocity at contact //Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA); //Vec2.crossToOut(wA, vcp.rA, temp1); //Vec2.crossToOut(wB, vcp.rB, dv); //dv.addLocal(vB).subLocal(vA).subLocal(temp1); dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a1.Y; dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a1.X; // Compute normal impulse float vn = dv.X * normal.X + dv.Y * normal.Y; float lambda = (-vcp.NormalMass) * (vn - vcp.VelocityBias); // Clamp the accumulated impulse float a = vcp.NormalImpulse + lambda; float newImpulse = (a > 0.0f ? a : 0.0f); lambda = newImpulse - vcp.NormalImpulse; //Debug.Assert(newImpulse == 0); vcp.NormalImpulse = newImpulse; // Apply contact impulse float Px = normal.X * lambda; float Py = normal.Y * lambda; // vA -= invMassA * P; vA.X -= Px * mA; vA.Y -= Py * mA; wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px); //Debug.Assert(vA.x == 0); // vB += invMassB * P; vB.X += Px * mB; vB.Y += Py * mB; wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px); //Debug.Assert(vB.x == 0); } else { // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on // Box2D_Lite). // Build the mini LCP for this contact patch // // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 // // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) // b = vn_0 - velocityBias // // The system is solved using the "Total enumeration method" (s. Murty). The complementary // constraint vn_i * x_i // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D // contact problem the cases // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be // tested. The first valid // solution that satisfies the problem is chosen. // // In order to account of the accumulated impulse 'a' (because of the iterative nature of // the solver which only requires // that the accumulated impulse is clamped and not the incremental impulse) we change the // impulse variable (x_i). // // Substitute: // // x = a + d // // a := old total impulse // x := new total impulse // d := incremental impulse // // For the current iteration we extend the formula for the incremental impulse // to compute the new total impulse: // // vn = A * d + b // = A * (x - a) + b // = A * x + b - A * a // = A * x + b' // b' = b - A * a; ContactVelocityConstraint.VelocityConstraintPoint cp1 = vc.Points[0]; ContactVelocityConstraint.VelocityConstraintPoint cp2 = vc.Points[1]; a.X = cp1.NormalImpulse; a.Y = cp2.NormalImpulse; Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f); // Relative velocity at contact // Vec2 dv1 = vB + Cross(wB, cp1.rB) - vA - Cross(wA, cp1.rA); dv1.X = (-wB) * cp1.RB.Y + vB.X - vA.X + wA * cp1.RA.Y; dv1.Y = wB * cp1.RB.X + vB.Y - vA.Y - wA * cp1.RA.X; // Vec2 dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA); dv2.X = (-wB) * cp2.RB.Y + vB.X - vA.X + wA * cp2.RA.Y; dv2.Y = wB * cp2.RB.X + vB.Y - vA.Y - wA * cp2.RA.X; // Compute normal velocity float vn1 = dv1.X * normal.X + dv1.Y * normal.Y; float vn2 = dv2.X * normal.X + dv2.Y * normal.Y; b.X = vn1 - cp1.VelocityBias; b.Y = vn2 - cp2.VelocityBias; //Console.WriteLine("b is " + b.x + "," + b.y); // Compute b' Mat22 R = vc.K; b.X -= (R.Ex.X * a.X + R.Ey.X * a.Y); b.Y -= (R.Ex.Y * a.X + R.Ey.Y * a.Y); //Console.WriteLine("b' is " + b.x + "," + b.y); // final float k_errorTol = 1e-3f; // B2_NOT_USED(k_errorTol); for (; ;) { // // Case 1: vn = 0 // // 0 = A * x' + b' // // Solve for x': // // x' = - inv(A) * b' // // Vec2 x = - Mul(c.normalMass, b); Mat22.MulToOutUnsafe(vc.NormalMass, b, x); x.MulLocal(-1); if (x.X >= 0.0f && x.Y >= 0.0f) { //Console.WriteLine("case 1"); // Get the incremental impulse // Vec2 d = x - a; d.Set(x).SubLocal(a); // Apply incremental impulse // Vec2 P1 = d.x * normal; // Vec2 P2 = d.y * normal; P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); /* * vA -= invMassA * (P1 + P2); wA -= invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; /* * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA - * Cross(wA, cp1.rA); dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA); * * // Compute normal velocity vn1 = Dot(dv1, normal); vn2 = Dot(dv2, normal); * * Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); Debug.Assert(Abs(vn2 - cp2.velocityBias) * < k_errorTol); #endif */ if (DEBUG_SOLVER) { // Postconditions Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA))); Vec2 _dv2 = vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA))); // Compute normal velocity vn1 = Vec2.Dot(_dv1, normal); vn2 = Vec2.Dot(_dv2, normal); Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I); Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I); } break; } // // Case 2: vn1 = 0 and x2 = 0 // // 0 = a11 * x1' + a12 * 0 + b1' // vn2 = a21 * x1' + a22 * 0 + ' // x.X = (-cp1.NormalMass) * b.X; x.Y = 0.0f; vn1 = 0.0f; vn2 = vc.K.Ex.Y * x.X + b.Y; if (x.X >= 0.0f && vn2 >= 0.0f) { //Console.WriteLine("case 2"); // Get the incremental impulse d.Set(x).SubLocal(a); // Apply incremental impulse // Vec2 P1 = d.x * normal; // Vec2 P2 = d.y * normal; P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); /* * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate //Debug.Assert(x.x == 0 && x.y == 0); cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; /* * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA - * Cross(wA, cp1.rA); * * // Compute normal velocity vn1 = Dot(dv1, normal); * * Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif */ if (DEBUG_SOLVER) { // Postconditions Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA))); // Compute normal velocity vn1 = Vec2.Dot(_dv1, normal); Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I); } break; } // // Case 3: wB = 0 and x1 = 0 // // vn1 = a11 * 0 + a12 * x2' + b1' // 0 = a21 * 0 + a22 * x2' + ' // x.X = 0.0f; x.Y = (-cp2.NormalMass) * b.Y; vn1 = vc.K.Ey.X * x.Y + b.X; vn2 = 0.0f; if (x.Y >= 0.0f && vn1 >= 0.0f) { //Console.WriteLine("case 3"); // Resubstitute for the incremental impulse d.Set(x).SubLocal(a); // Apply incremental impulse /* * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate //Debug.Assert(x.x == 0 && x.y == 0); cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; /* * #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + Cross(wB, cp2.rB) - vA - * Cross(wA, cp2.rA); * * // Compute normal velocity vn2 = Dot(dv2, normal); * * Debug.Assert(Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif */ if (DEBUG_SOLVER) { // Postconditions Vec2 _dv2 = vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA))); // Compute normal velocity vn2 = Vec2.Dot(_dv2, normal); Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I); } break; } // // Case 4: x1 = 0 and x2 = 0 // // vn1 = b1 // vn2 = ; x.X = 0.0f; x.Y = 0.0f; vn1 = b.X; vn2 = b.Y; if (vn1 >= 0.0f && vn2 >= 0.0f) { //Console.WriteLine("case 4"); // Resubstitute for the incremental impulse d.Set(x).SubLocal(a); // Apply incremental impulse /* * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate //Debug.Assert(x.x == 0 && x.y == 0); cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; break; } // No solution, give up. This is hit sometimes, but it doesn't seem to matter. break; } } Velocities[indexA].V.Set(vA); Velocities[indexA].W = wA; Velocities[indexB].V.Set(vB); Velocities[indexB].W = wB; //Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA); //Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB); } }
internal override bool SolvePositionConstraints() { Body b1 = BodyA; Body b2 = BodyB; Vector2 c1 = b1.Sweep.C; float a1 = b1.Sweep.A; Vector2 c2 = b2.Sweep.C; float a2 = b2.Sweep.A; // Solve linear limit constraint. float linearError = 0.0f; bool active = false; float C2 = 0.0f; Mat22 R1 = new Mat22(a1); Mat22 R2 = new Mat22(a2); Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA); Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB); Vector2 d = c2 + r2 - c1 - r1; if (_enableLimit) { _axis = MathUtils.Multiply(ref R1, _localXAxis1); _a1 = MathUtils.Cross(d + r1, _axis); _a2 = MathUtils.Cross(r2, _axis); float translation = Vector2.Dot(_axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Math.Abs(translation); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = _lowerTranslation - translation; active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = translation - _upperTranslation; active = true; } } _perp = MathUtils.Multiply(ref R1, _localYAxis1); _s1 = MathUtils.Cross(d + r1, _perp); _s2 = MathUtils.Cross(r2, _perp); Vector3 impulse; Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - ReferenceAngle); linearError = Math.Max(linearError, Math.Abs(C1.X)); float angularError = Math.Abs(C1.Y); if (active) { float m1 = InvMassA, m2 = InvMassB; float i1 = InvIA, i2 = InvIB; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float k12 = i1 * _s1 + i2 * _s2; float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2; float k22 = i1 + i2; float k23 = i1 * _a1 + i2 * _a2; float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; _K.Col1 = new Vector3(k11, k12, k13); _K.Col2 = new Vector3(k12, k22, k23); _K.Col3 = new Vector3(k13, k23, k33); Vector3 C = new Vector3(-C1.X, -C1.Y, -C2); impulse = _K.Solve33(C); // negated above } else { float m1 = InvMassA, m2 = InvMassB; float i1 = InvIA, i2 = InvIB; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float k12 = i1 * _s1 + i2 * _s2; float k22 = i1 + i2; _K.Col1 = new Vector3(k11, k12, 0.0f); _K.Col2 = new Vector3(k12, k22, 0.0f); Vector2 impulse1 = _K.Solve22(-C1); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } Vector2 P = impulse.X * _perp + impulse.Z * _axis; float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1; float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2; c1 -= InvMassA * P; a1 -= InvIA * L1; c2 += InvMassB * P; a2 += InvIB * L2; // TODO_ERIN remove need for this. b1.Sweep.C = c1; b1.Sweep.A = a1; b2.Sweep.C = c2; b2.Sweep.A = a2; b1.SynchronizeTransform(); b2.SynchronizeTransform(); return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; }
/// <summary> /// Set the position of the body's origin and rotation (radians). /// This breaks any contacts and wakes the other bodies. /// </summary> /// <param name="position">The new world position of the body's origin (not necessarily /// the center of mass).</param> /// <param name="angle">The new world rotation angle of the body in radians.</param> /// <returns>Return false if the movement put a shape outside the world. In this case the /// body is automatically frozen.</returns> public bool SetTransform(Vector2 position, Mat22 rotation)
internal override void InitVelocityConstraints(ref TimeStep step) { Body b = BodyA; float mass = b.Mass; // Frequency float omega = 2.0f*Settings.Pi*Frequency; // 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. Debug.Assert(d + step.dt*k > Settings.Epsilon); _gamma = step.dt*(d + step.dt*k); if (_gamma != 0.0f) { _gamma = 1.0f/_gamma; } _beta = step.dt*k*_gamma; // Compute the effective mass matrix. Transform xf1; b.GetTransform(out xf1); Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter); // 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(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass)); Mat22 K2 = new Mat22(new Vector2(invI*r.Y*r.Y, -invI*r.X*r.Y), new Vector2(-invI*r.X*r.Y, invI*r.X*r.X)); Mat22 K; Mat22.Add(ref K1, ref K2, out K); K.Col1.X += _gamma; K.Col2.Y += _gamma; _mass = K.Inverse; _C = b.Sweep.C + r - _worldAnchor; // Cheat with some damping b.AngularVelocityInternal *= 0.98f; // Warm starting. _impulse *= step.dtRatio; b.LinearVelocityInternal += invMass*_impulse; b.AngularVelocityInternal += invI*MathUtils.Cross(r, _impulse); }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _invMassA = BodyA._invMass; _invIA = BodyA._invI; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Complex qA = Complex.FromAngle(aA); float mass = BodyA.Mass; // Frequency float omega = 2.0f * MathHelper.Pi * Frequency; // 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. float h = data.step.dt; Debug.Assert(d + h * k > Settings.Epsilon); _gamma = h * (d + h * k); if (_gamma != 0.0f) { _gamma = 1.0f / _gamma; } _beta = h * k * _gamma; // Compute the effective mass matrix. _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); // 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] Mat22 K = new Mat22(); K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma; K.ex.Y = -_invIA * _rA.X * _rA.Y; K.ey.X = K.ex.Y; K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma; _mass = K.Inverse; _C = cA + _rA - _worldAnchor; _C *= _beta; // Cheat with some damping wA *= 0.98f; if (data.step.warmStarting) { _impulse *= data.step.dtRatio; vA += _invMassA * _impulse; wA += _invIA * MathUtils.Cross(ref _rA, ref _impulse); } else { _impulse = Vector2.Zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; }