public static Clamp ( double value, double min, double max ) : double | ||
value | double | |
min | double | |
max | double | |
Результат | double |
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 void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; bool fixedRotation = (iA + iB == 0.0f); // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false) { float Cdot = wB - wA - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = data.step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); float Cdot2 = wB - wA; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 impulse = -_mass.Solve33(Cdot); if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse < 0.0f) { Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y); Vector2 reduced = _mass.Solve22(rhs); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } else { _impulse += impulse; } } else if (_limitState == LimitState.AtUpper) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y); Vector2 reduced = _mass.Solve22(rhs); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } else { _impulse += impulse; } } Vector2 P = new Vector2(impulse.X, impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z); } else { // Solve point-to-point constraint Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); Vector2 impulse = _mass.Solve22(-Cdot); _impulse.X += impulse.X; _impulse.Y += impulse.Y; vA -= mA * impulse; wA -= iA * MathUtils.Cross(_rA, impulse); vB += mB * impulse; wB += iB * MathUtils.Cross(_rB, impulse); } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override void SolveVelocityConstraints(ref TimeStep step) { Body b2 = BodyB; Vector2 v1 = Vector2.Zero; float w1 = 0.0f; Vector2 v2 = b2.LinearVelocityInternal; float w2 = b2.AngularVelocityInternal; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; float impulse = _motorMass * (_motorSpeed - Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.dt * _maxMotorForce; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; Vector2 P = impulse * _axis; float L1 = impulse * _a1; float L2 = impulse * _a2; v1 -= InvMassA * P; w1 -= InvIA * L1; v2 += InvMassB * P; w2 += InvIB * L2; } float Cdot1 = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1; if (_enableLimit && _limitState != LimitState.Inactive) { // Solve prismatic and limit constraint in block form. float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; Vector2 Cdot = new Vector2(Cdot1, Cdot2); Vector2 f1 = _impulse; Vector2 df = _K.Solve(-Cdot); _impulse += df; if (_limitState == LimitState.AtLower) { _impulse.Y = Math.Max(_impulse.Y, 0.0f); } else if (_limitState == LimitState.AtUpper) { _impulse.Y = Math.Min(_impulse.Y, 0.0f); } // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1) float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.col2.X; float f2r; if (_K.col1.X != 0.0f) { f2r = b / _K.col1.X + f1.X; } else { f2r = f1.X; } _impulse.X = f2r; df = _impulse - f1; Vector2 P = df.X * _perp + df.Y * _axis; float L2 = df.X * _s2 + df.Y * _a2; v2 += InvMassB * P; w2 += InvIB * L2; } else { // Limit is inactive, just solve the prismatic constraint in block form. float df; if (_K.col1.X != 0.0f) { df = -Cdot1 / _K.col1.X; } else { df = 0.0f; } _impulse.X += df; Vector2 P = df * _perp; float L2 = df * _s2; v2 += InvMassB * P; w2 += InvIB * L2; } b2.LinearVelocityInternal = v2; b2.AngularVelocityInternal = w2; }
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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Compute fresh Jacobians Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 d = cB + rB - cA - rA; Vector2 axis = Complex.Multiply(ref _localXAxis, ref qA); float a1 = MathUtils.Cross(d + rA, axis); float a2 = MathUtils.Cross(ref rB, ref axis); Vector2 perp = Complex.Multiply(ref _localYAxisA, ref qA); float s1 = MathUtils.Cross(d + rA, perp); float s2 = MathUtils.Cross(ref rB, ref 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); }
internal override void SolveVelocityConstraints(ref TimeStep step) { Body bA = BodyA; Body bB = BodyB; Vector2 vA = bA.LinearVelocity; float wA = bA.AngularVelocityInternal; Vector2 vB = bB.LinearVelocityInternal; float wB = bB.AngularVelocityInternal; // Solve spring constraint { float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA; float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse); _springImpulse += impulse; Vector2 P = impulse * _ax; float LA = impulse * _sAx; float LB = impulse * _sBx; vA -= InvMassA * P; wA -= InvIA * LA; vB += InvMassB * P; wB += InvIB * LB; } // Solve rotational motor constraint { float Cdot = wB - wA - _motorSpeed; float impulse = -_motorMass * Cdot; float oldImpulse = _motorImpulse; float maxImpulse = step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; wA -= InvIA * impulse; wB += InvIB * impulse; } // Solve point to line constraint { float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA; float impulse = _mass * (-Cdot); _impulse += impulse; Vector2 P = impulse * _ay; float LA = impulse * _sAy; float LB = impulse * _sBy; vA -= InvMassA * P; wA -= InvIA * LA; vB += InvMassB * P; wB += InvIB * LB; } bA.LinearVelocityInternal = vA; bA.AngularVelocityInternal = wA; bB.LinearVelocityInternal = vB; bB.AngularVelocityInternal = wB; }
private void SolveVelocityConstraints(int start, int end) { for (int i = start; i < end; ++i) { ContactVelocityConstraint vc = _velocityConstraints[i]; #if NET40 || NET45 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1 // find lower order item int orderedIndexA = vc.indexA; int orderedIndexB = vc.indexB; if (orderedIndexB < orderedIndexA) { orderedIndexA = vc.indexB; orderedIndexB = vc.indexA; } for (; ;) { if (Interlocked.CompareExchange(ref _locks[orderedIndexA], 1, 0) == 0) { if (Interlocked.CompareExchange(ref _locks[orderedIndexB], 1, 0) == 0) { break; } System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0); } #if NET40 || NET45 || NETSTANDARD2_0 Thread.Sleep(0); #endif } #endif int indexA = vc.indexA; int indexB = vc.indexB; float mA = vc.invMassA; float iA = vc.invIA; float mB = vc.invMassB; float iB = vc.invIB; int pointCount = vc.pointCount; Vector2 vA = _velocities[indexA].v; float wA = _velocities[indexA].w; Vector2 vB = _velocities[indexB].v; float wB = _velocities[indexB].w; Vector2 normal = vc.normal; Vector2 tangent = MathUtils.Rot270(ref normal); float friction = vc.friction; Debug.Assert(pointCount == 1 || pointCount == 2); // Solve tangent constraints first because non-penetration is more important // than friction. for (int j = 0; j < pointCount; ++j) { VelocityConstraintPoint vcp = vc.points[j]; // Relative velocity at contact Vector2 dv = vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA); // Compute tangent force float vt = Vector2.Dot(dv, tangent) - vc.tangentSpeed; float lambda = vcp.tangentMass * (-vt); // b2Clamp 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 Vector2 P = lambda * tangent; vA -= mA * P; wA -= iA * MathUtils.Cross(ref vcp.rA, ref P); vB += mB * P; wB += iB * MathUtils.Cross(ref vcp.rB, ref P); } // Solve normal constraints if (vc.pointCount == 1) { VelocityConstraintPoint vcp = vc.points[0]; // Relative velocity at contact Vector2 dv = vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA); // Compute normal impulse float vn = Vector2.Dot(dv, normal); float lambda = -vcp.normalMass * (vn - vcp.velocityBias); // b2Clamp the accumulated impulse float newImpulse = Math.Max(vcp.normalImpulse + lambda, 0.0f); lambda = newImpulse - vcp.normalImpulse; vcp.normalImpulse = newImpulse; // Apply contact impulse Vector2 P = lambda * normal; vA -= mA * P; wA -= iA * MathUtils.Cross(ref vcp.rA, ref P); vB += mB * P; wB += iB * MathUtils.Cross(ref vcp.rB, ref P); } 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 = vn0 - 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; VelocityConstraintPoint cp1 = vc.points[0]; VelocityConstraintPoint cp2 = vc.points[1]; Vector2 a = new Vector2(cp1.normalImpulse, cp2.normalImpulse); Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f); // Relative velocity at contact Vector2 dv1 = vB + MathUtils.Cross(wB, ref cp1.rB) - vA - MathUtils.Cross(wA, ref cp1.rA); Vector2 dv2 = vB + MathUtils.Cross(wB, ref cp2.rB) - vA - MathUtils.Cross(wA, ref cp2.rA); // Compute normal velocity float vn1 = Vector2.Dot(dv1, normal); float vn2 = Vector2.Dot(dv2, normal); Vector2 b = new Vector2(); b.X = vn1 - cp1.velocityBias; b.Y = vn2 - cp2.velocityBias; // Compute b' b -= MathUtils.Mul(ref vc.K, ref a); const 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' // Vector2 x = -MathUtils.Mul(ref vc.normalMass, ref b); if (x.X >= 0.0f && x.Y >= 0.0f) { // Get the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.X * normal; Vector2 P2 = d.Y * normal; vA -= mA * (P1 + P2); wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2)); vB += mB * (P1 + P2); wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2)); // Accumulate cp1.normalImpulse = x.X; cp2.normalImpulse = x.Y; #if B2_DEBUG_SOLVER // Postconditions dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity vn1 = Vector2.Dot(dv1, normal); vn2 = Vector2.Dot(dv2, normal); b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol); b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif break; } // // Case 2: vn1 = 0 and x2 = 0 // // 0 = a11 * x1 + a12 * 0 + b1' // vn2 = a21 * x1 + a22 * 0 + b2' // 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) { // Get the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.X * normal; Vector2 P2 = d.Y * normal; vA -= mA * (P1 + P2); wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2)); vB += mB * (P1 + P2); wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2)); // Accumulate cp1.normalImpulse = x.X; cp2.normalImpulse = x.Y; #if B2_DEBUG_SOLVER // Postconditions dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); // Compute normal velocity vn1 = Vector2.Dot(dv1, normal); b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif break; } // // Case 3: vn2 = 0 and x1 = 0 // // vn1 = a11 * 0 + a12 * x2 + b1' // 0 = a21 * 0 + a22 * x2 + b2' // 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) { // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.X * normal; Vector2 P2 = d.Y * normal; vA -= mA * (P1 + P2); wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2)); vB += mB * (P1 + P2); wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2)); // Accumulate cp1.normalImpulse = x.X; cp2.normalImpulse = x.Y; #if B2_DEBUG_SOLVER // Postconditions dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity vn2 = Vector2.Dot(dv2, normal); b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif break; } // // Case 4: x1 = 0 and x2 = 0 // // vn1 = b1 // vn2 = b2; x.X = 0.0f; x.Y = 0.0f; vn1 = b.X; vn2 = b.Y; if (vn1 >= 0.0f && vn2 >= 0.0f) { // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.X * normal; Vector2 P2 = d.Y * normal; vA -= mA * (P1 + P2); wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2)); vB += mB * (P1 + P2); wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2)); // Accumulate 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 = vA; _velocities[indexA].w = wA; _velocities[indexB].v = vB; _velocities[indexB].w = wB; #if NET40 || NET45 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1 System.Threading.Interlocked.Exchange(ref _locks[orderedIndexB], 0); System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0); #endif } }
// Sequential position solver for position constraints. public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB) { float minSeparation = 0.0f; for (int i = 0; i < _count; ++i) { ContactPositionConstraint pc = _positionConstraints[i]; int indexA = pc.indexA; int indexB = pc.indexB; Vector2 localCenterA = pc.localCenterA; Vector2 localCenterB = pc.localCenterB; int pointCount = pc.pointCount; float mA = 0.0f; float iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc.invMassA; iA = pc.invIA; } float mB = 0.0f; float iB = 0.0f; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc.invMassB; iB = pc.invIB; } Vector2 cA = _positions[indexA].c; float aA = _positions[indexA].a; Vector2 cB = _positions[indexB].c; float aB = _positions[indexB].a; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { Transform xfA = new Transform(Vector2.Zero, aA); Transform xfB = new Transform(Vector2.Zero, aB); xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q); xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q); Vector2 normal; Vector2 point; float separation; PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation); Vector2 rA = point - cA; Vector2 rB = point - cB; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(ref rA, ref normal); float rnB = MathUtils.Cross(ref rB, ref normal); float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; Vector2 P = impulse * normal; cA -= mA * P; aA -= iA * MathUtils.Cross(ref rA, ref P); cB += mB * P; aB += iB * MathUtils.Cross(ref rB, ref P); } _positions[indexA].c = cA; _positions[indexA].a = aA; _positions[indexB].c = cB; _positions[indexB].a = aB; } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -1.5f * Settings.LinearSlop); }
protected virtual float PlaceRoom(List <RoomDef> Collision, RoomDef Prev, RoomDef Next, float Angle) { LastSpace = null; if (Next == null) { Log.Error("Null next room"); return(-1); } Angle %= 360f; if (Angle < 0) { Angle += 360f; } var PrevCenter = new Vector2((Prev.Left + Prev.Right) / 2f, (Prev.Top + Prev.Bottom) / 2f); var M = Math.Tan(Angle / A + Math.PI / 2.0); var B = PrevCenter.Y - M * PrevCenter.X; Vector2 Start; RoomDef.Connection Direction; if (Math.Abs(M) >= 1) { if (Angle < 90 || Angle > 270) { Direction = RoomDef.Connection.Top; Start = new Vector2((int)Math.Round((Prev.Top - B) / M), Prev.Top); } else { Direction = RoomDef.Connection.Bottom; Start = new Vector2((int)Math.Round((Prev.Bottom - B) / M), Prev.Bottom); } } else { if (Angle < 180) { Direction = RoomDef.Connection.Right; Start = new Vector2(Prev.Right, (int)Math.Round(M * Prev.Right + B)); } else { Direction = RoomDef.Connection.Left; Start = new Vector2(Prev.Left, (int)Math.Round(M * Prev.Left + B)); } } if (Direction == RoomDef.Connection.Top || Direction == RoomDef.Connection.Bottom) { Start.X = (int)MathUtils.Clamp(Prev.Left + 1, Prev.Right - 1, Start.X); } else { Start.Y = (int)MathUtils.Clamp(Prev.Top + 1, Prev.Bottom - 1, Start.Y); } var Space = LastSpace = FindFreeSpace(Start, Collision, Math.Max(Next.GetMaxWidth(), Next.GetMaxHeight())); if (!Next.SetSizeWithLimit(Space.GetWidth() + 1, Space.GetHeight() + 1)) { return(-1); } var TargetCenter = new Dot(); if (Direction == RoomDef.Connection.Top) { TargetCenter.Y = (int)(Prev.Top - (Next.GetHeight() - 1) / 2f); TargetCenter.X = (int)((TargetCenter.Y - B) / M); Next.SetPos((int)Math.Round(TargetCenter.X - (Next.GetWidth() - 1) / 2f), Prev.Top - (Next.GetHeight() - 1)); } else if (Direction == RoomDef.Connection.Bottom) { TargetCenter.Y = (int)(Prev.Bottom + (Next.GetHeight() - 1) / 2f); TargetCenter.X = (int)((TargetCenter.Y - B) / M); Next.SetPos((int)Math.Round(TargetCenter.X - (Next.GetWidth() - 1) / 2f), Prev.Bottom); } else if (Direction == RoomDef.Connection.Right) { TargetCenter.X = (int)(Prev.Right + (Next.GetWidth() - 1) / 2f); TargetCenter.Y = (int)(M * TargetCenter.X + B); Next.SetPos(Prev.Right, (int)Math.Round(TargetCenter.Y - (Next.GetHeight() - 1) / 2f)); } else if (Direction == RoomDef.Connection.Left) { TargetCenter.X = (int)(Prev.Left - (Next.GetWidth() - 1) / 2f); TargetCenter.Y = (int)(M * TargetCenter.X + B); Next.SetPos(Prev.Left - (Next.GetWidth() - 1), (int)Math.Round(TargetCenter.Y - (Next.GetHeight() - 1) / 2f)); } if (Direction == RoomDef.Connection.Top || Direction == RoomDef.Connection.Bottom) { if (Next.Right < Prev.Left + 2) { Next.Shift(Prev.Left + 2 - Next.Right, 0); } else if (Next.Left > Prev.Right - 2) { Next.Shift(Prev.Right - 2 - Next.Left, 0); } if (Next.Right > Space.Right) { Next.Shift(Space.Right - Next.Right, 0); } else if (Next.Left < Space.Left) { Next.Shift(Space.Left - Next.Left, 0); } } else { if (Next.Bottom < Prev.Top + 2) { Next.Shift(0, Prev.Top + 2 - Next.Bottom); } else if (Next.Top > Prev.Bottom - 2) { Next.Shift(0, Prev.Bottom - 2 - Next.Top); } if (Next.Bottom > Space.Bottom) { Next.Shift(0, Space.Bottom - Next.Bottom); } else if (Next.Top < Space.Top) { Next.Shift(0, Space.Top - Next.Top); } } if (Next.ConnectWithRoom(Prev)) { if (Next is ConnectionRoom || Next is BossRoom || Next is ShopRoom) { Next.Id = Prev.Id; } else { Next.Id = Prev.Id + 1; } return(AngleBetweenRooms(Prev, Next)); } return(-1); }
internal override void SolveVelocityConstraints(ref TimeStep step) { Body b1 = BodyA; Vector2 v1 = b1.LinearVelocityInternal; float w1 = b1.AngularVelocityInternal; Vector2 v2 = Vector2.zero; const float w2 = 0; float m1 = b1.InvMass; float i1 = b1.InvI; // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = w2 - w1 - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; w1 -= i1 * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { Transform xf1; b1.GetTransform(out xf1); Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = _worldAnchor; // Solve point-to-point constraint Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1); float Cdot2 = w2 - w1; Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2); Vector3 impulse = _mass.Solve33(-Cdot); if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse < 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.x; impulse.Y = reduced.y; impulse.Z = -_impulse.Z; _impulse.X += reduced.x; _impulse.Y += reduced.y; _impulse.Z = 0.0f; } } else if (_limitState == LimitState.AtUpper) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.x; impulse.Y = reduced.y; impulse.Z = -_impulse.Z; _impulse.X += reduced.x; _impulse.Y += reduced.y; _impulse.Z = 0.0f; } } Vector2 P = new Vector2(impulse.X, impulse.Y); v1 -= m1 * P; w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z); } else { Transform xf1; b1.GetTransform(out xf1); Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = _worldAnchor; // Solve point-to-point constraint Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1); Vector2 impulse = _mass.Solve22(-Cdot); _impulse.X += impulse.x; _impulse.Y += impulse.y; v1 -= m1 * impulse; w1 -= i1 * MathUtils.Cross(r1, impulse); } b1.LinearVelocityInternal = v1; b1.AngularVelocityInternal = w1; }
public void Solve(ref TimeStep step, ref Vector2 gravity) { GGame.Math.Fix64 h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; Vector2 c = b._sweep.C; GGame.Math.Fix64 a = b._sweep.A; Vector2 v = b._linearVelocity; GGame.Math.Fix64 w = b._angularVelocity; // Store positions for continuous collision. b._sweep.C0 = b._sweep.C; b._sweep.A0 = b._sweep.A; if (b.BodyType == BodyType.Dynamic) { // Integrate velocities. //Velcro: Only apply gravity if the body wants it. if (b.IgnoreGravity) { v += h * (b._invMass * b._force); } else { v += h * (b.GravityScale * gravity + b._invMass * b._force); } w += h * b._invI * b._torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } Positions[i].C = c; Positions[i].A = a; Velocities[i].V = v; Velocities[i].W = w; } // Solver data SolverData solverData = new SolverData(); solverData.Step = step; solverData.Positions = Positions; solverData.Velocities = Velocities; _contactSolver.Reset(step, ContactCount, _contacts, Positions, Velocities); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } if (Settings.EnableDiagnostics) { _watch.Start(); } for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref solverData); } } if (Settings.EnableDiagnostics) { _watch.Stop(); } // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } joint.SolveVelocityConstraints(ref solverData); joint.Validate(step.inv_dt); if (Settings.EnableDiagnostics) { _watch.Stop(); } } _contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { Vector2 c = Positions[i].C; GGame.Math.Fix64 a = Positions[i].A; Vector2 v = Velocities[i].V; GGame.Math.Fix64 w = Velocities[i].W; // Check for large velocities Vector2 translation = h * v; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { GGame.Math.Fix64 ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } GGame.Math.Fix64 rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { GGame.Math.Fix64 ratio = Settings.MaxRotation / Fix64.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; Positions[i].C = c; Positions[i].A = a; Velocities[i].V = v; Velocities[i].W = w; } // Solve position constraints bool positionSolved = false; for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } bool jointOkay = joint.SolvePositionConstraints(ref solverData); if (Settings.EnableDiagnostics) { _watch.Stop(); } jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } if (Settings.EnableDiagnostics) { JointUpdateTime = _watch.ElapsedTicks; _watch.Reset(); } // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body body = Bodies[i]; body._sweep.C = Positions[i].C; body._sweep.A = Positions[i].A; body._linearVelocity = Velocities[i].V; body._angularVelocity = Velocities[i].W; body.SynchronizeTransform(); } Report(_contactSolver.VelocityConstraints); if (Settings.AllowSleep) { GGame.Math.Fix64 minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += h; minSleepTime = Math.Min((float)minSleepTime, (float)b.SleepTime); } } if (minSleepTime >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
public bool SolvePositionConstraints() { float minSeparation = 0.0f; for (int i = 0; i < _count; ++i) { ContactPositionConstraint pc = _positionConstraints[i]; int indexA = pc.IndexA; int indexB = pc.IndexB; Vector2 localCenterA = pc.LocalCenterA; float mA = pc.InvMassA; float iA = pc.InvIA; Vector2 localCenterB = pc.LocalCenterB; float mB = pc.InvMassB; float iB = pc.InvIB; int pointCount = pc.PointCount; Vector2 cA = _positions[indexA].C; float aA = _positions[indexA].A; Vector2 cB = _positions[indexB].C; float aB = _positions[indexB].A; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { Transform xfA = new Transform(); Transform xfB = new Transform(); xfA.q.Set(aA); xfB.q.Set(aB); xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA); xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB); PositionSolverManifold.Initialize(pc, xfA, xfB, j, out Vector2 normal, out Vector2 point, out float separation); Vector2 rA = point - cA; Vector2 rB = point - cB; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(rA, normal); float rnB = MathUtils.Cross(rB, normal); float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; Vector2 P = impulse * normal; cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } _positions[indexA].C = cA; _positions[indexA].A = aA; _positions[indexB].C = cB; _positions[indexB].A = aB; } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -3.0f * Settings.LinearSlop); }
public void Update(float dt) { var dialog = m_game.Screen.ModalDialog; var allowButtonInput = (dialog == null) || (dialog is DialogBox && !((DialogBox)dialog).BlockInput); if (AllowUserRotate) { // Rotate camera if (m_game.Mouse.Buttons[MouseButton.Right].Held) { m_game.Screen.InputMethod = InputMethod.Mouse; Yaw += 12.0f * ((float)m_game.Mouse.DX / (float)m_game.Window.Width) * m_game.Camera.FOV; Pitch += 6.0f * ((float)m_game.Mouse.DY / (float)m_game.Window.Height) * m_game.Camera.FOV; } if (m_game.ActiveSteamController != null) { var joystick = m_game.ActiveSteamController.Joysticks[SteamControllerJoystick.InGameCamera.GetID()]; var dx = joystick.X; var dy = joystick.Y; if (dx != 0.0f || dy != 0.0f) { m_game.Screen.InputMethod = InputMethod.SteamController; Yaw = Yaw - 2.0f * dx * dt; Pitch = Pitch + 2.0f * dy * dt; } } if (m_game.ActiveGamepad != null) { var joystick = m_game.ActiveGamepad.Joysticks[GamepadJoystick.Right]; var dx = joystick.X; var dy = joystick.Y; if (dx != 0.0f || dy != 0.0f) { m_game.Screen.InputMethod = InputMethod.Gamepad; Yaw = Yaw - 2.0f * dx * dt; Pitch = Pitch - 2.0f * dy * dt; } } // Clamp to allowed limits if (AllowUserInvert) { Pitch = Math.Min(Math.Max( Pitch, MathHelper.DegreesToRadians(-85.0f)), MathHelper.DegreesToRadians(85.0f) ); } else { Pitch = Math.Min(Math.Max( Pitch, MathHelper.DegreesToRadians(30.0f)), MathHelper.DegreesToRadians(85.0f) ); } } if (AllowUserZoom) { // Zoom camera if (m_game.Mouse.Wheel != 0) { m_game.Screen.InputMethod = InputMethod.Mouse; TargetDistance -= m_game.Mouse.Wheel * 2.0f; } if (allowButtonInput && m_game.ActiveGamepad != null) { if (m_game.ActiveGamepad.Buttons[GamepadButton.Down].Held) { m_game.Screen.InputMethod = InputMethod.Gamepad; TargetDistance += 16.0f * dt; } if (m_game.ActiveGamepad.Buttons[GamepadButton.Up].Held) { m_game.Screen.InputMethod = InputMethod.Gamepad; TargetDistance -= 16.0f * dt; } } // Clamp to allowed limits TargetDistance = MathUtils.Clamp(TargetDistance, 12.0f, 30.0f); } if (Distance < TargetDistance) { Distance = Math.Min(Distance + 48.0f * dt, TargetDistance); } else if (Distance > TargetDistance) { Distance = Math.Max(Distance - 48.0f * dt, TargetDistance); } if (AllowUserPan) { // Scrolling var margin = 10.0f; var autoScrollSpeed = 10.0f; var forward = new Vector3( -(float)Math.Cos(Yaw), 0.0f, -(float)Math.Sin(Yaw) ); var right = new Vector3( (float)Math.Sin(Yaw), 0.0f, -(float)Math.Cos(Yaw) ); var scroll = Vector3.Zero; var distance = margin; if (m_game.Cursor.Position.X < margin) { scroll -= right; distance = Math.Min(m_game.Cursor.Position.X, margin); } else if (m_game.Cursor.Position.X >= m_game.Screen.Width - margin) { scroll += right; distance = Math.Min(m_game.Screen.Width - m_game.Cursor.Position.X, margin); } if (m_game.Cursor.Position.Y < margin) { scroll += forward; distance = Math.Min(m_game.Cursor.Position.Y, margin); } else if (m_game.Cursor.Position.Y >= m_game.Screen.Height - margin) { scroll -= forward; distance = Math.Min(m_game.Screen.Height - m_game.Cursor.Position.Y, margin); } if (scroll.LengthSquared > 0.0f) { scroll.Normalize(); Focus += scroll * dt * (1.0f - (distance / margin)) * autoScrollSpeed; ApplyBounds(); } } }
public static float Angle(Vec3 from, Vec3 to) { return(MathUtils.Acos(MathUtils.Clamp(Dot(Normalize(@from), Normalize(to)), -1, 1)) * MathUtils.RAD_TO_DEG); }
// Sequential position solver for position constraints. public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB) { float minSeparation = 0.0f; for (int i = 0; i < Count; ++i) { ContactPositionConstraint pc = PositionConstraints[i]; int indexA = pc.IndexA; int indexB = pc.IndexB; Vec2 localCenterA = pc.LocalCenterA; Vec2 localCenterB = pc.LocalCenterB; int pointCount = pc.PointCount; float mA = 0.0f; float iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc.InvMassA; iA = pc.InvIA; } float mB = pc.InvMassB; float iB = pc.InvIB; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc.InvMassB; iB = pc.InvIB; } Vec2 cA = Positions[indexA].C; float aA = Positions[indexA].A; Vec2 cB = Positions[indexB].C; float aB = Positions[indexB].A; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { xfA.Q.Set(aA); xfB.Q.Set(aB); Rot.MulToOutUnsafe(xfA.Q, localCenterA, xfA.P); xfA.P.NegateLocal().AddLocal(cA); Rot.MulToOutUnsafe(xfB.Q, localCenterB, xfB.P); xfB.P.NegateLocal().AddLocal(cB); PositionSolverManifold psm = psolver; psm.Initialize(pc, xfA, xfB, j); Vec2 normal = psm.Normal; Vec2 point = psm.Point; float separation = psm.Separation; rA.Set(point).SubLocal(cA); rB.Set(point).SubLocal(cB); // Track max constraint error. minSeparation = MathUtils.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(Settings.TOI_BAUGARTE * (separation + Settings.LINEAR_SLOP), -Settings.MAX_LINEAR_CORRECTION, 0.0f); // Compute the effective mass. float rnA = Vec2.Cross(rA, normal); float rnB = Vec2.Cross(rB, normal); float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? (-C) / K : 0.0f; P.Set(normal).MulLocal(impulse); cA.SubLocal(temp.Set(P).MulLocal(mA)); aA -= iA * Vec2.Cross(rA, P); cB.AddLocal(temp.Set(P).MulLocal(mB)); aB += iB * Vec2.Cross(rB, P); } Positions[indexA].C.Set(cA); Positions[indexA].A = aA; Positions[indexB].C.Set(cB); Positions[indexB].A = aB; } // We can't expect minSpeparation >= -_linearSlop because we don't // push the separation above -_linearSlop. return(minSeparation >= (-1.5f) * Settings.LINEAR_SLOP); }
internal override void SolveVelocityConstraints(ref TimeStep step) { Body bB = BodyB; Vector2 v1 = Vector2.Zero; float w1 = 0.0f; Vector2 v2 = bB.LinearVelocityInternal; float w2 = bB.AngularVelocityInternal; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; float impulse = _motorMass * (_motorSpeed - Cdot); float oldImpulse = MotorForce; float maxImpulse = step.dt * _maxMotorForce; MotorForce = MathUtils.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse); impulse = MotorForce - oldImpulse; Vector2 P = impulse * _axis; float L1 = impulse * _a1; float L2 = impulse * _a2; v1 -= InvMassA * P; w1 -= InvIA * L1; v2 += InvMassB * P; w2 += InvIB * L2; } Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1); if (_enableLimit && _limitState != LimitState.Inactive) { // Solve prismatic and limit constraint in block form. float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 f1 = _impulse; Vector3 df = _K.Solve33(-Cdot); _impulse += df; if (_limitState == LimitState.AtLower) { _impulse.Z = Math.Max(_impulse.Z, 0.0f); } else if (_limitState == LimitState.AtUpper) { _impulse.Z = Math.Min(_impulse.Z, 0.0f); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y); Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y); _impulse.X = f2r.X; _impulse.Y = f2r.Y; df = _impulse - f1; Vector2 P = df.X * _perp + df.Z * _axis; float L2 = df.X * _s2 + df.Y + df.Z * _a2; v2 += InvMassB * P; w2 += InvIB * L2; } else { // Limit is inactive, just solve the prismatic constraint in block form. Vector2 df = _K.Solve22(-Cdot1); _impulse.X += df.X; _impulse.Y += df.Y; Vector2 P = df.X * _perp; float L2 = df.X * _s2 + df.Y; v2 += InvMassB * P; w2 += InvIB * L2; } bB.LinearVelocityInternal = v2; bB.AngularVelocityInternal = w2; }
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() { //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); }
public void Solve(ref TimeStep step, ref Vector2 gravity) { // Integrate velocities and apply damping. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType != BodyType.Dynamic) { continue; } // Integrate velocities. // FPE 3 only - Only apply gravity if the body wants it. if (b.IgnoreGravity) { b.LinearVelocityInternal.X += step.dt * (b.InvMass * b.Force.X); b.LinearVelocityInternal.Y += step.dt * (b.InvMass * b.Force.Y); b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; } else { b.LinearVelocityInternal.X += step.dt * (gravity.X + b.InvMass * b.Force.X); b.LinearVelocityInternal.Y += step.dt * (gravity.Y + b.InvMass * b.Force.Y); b.AngularVelocityInternal += step.dt * b.InvI * b.Torque; } // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f); b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f); } // Partition contacts so that contacts with static bodies are solved last. int i1 = -1; for (int i2 = 0; i2 < ContactCount; ++i2) { Fixture fixtureA = _contacts[i2].FixtureA; Fixture fixtureB = _contacts[i2].FixtureB; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static; if (nonStatic) { ++i1; //TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162 Contact tmp = _contacts[i1]; _contacts[i1] = _contacts[i2]; _contacts[i2] = tmp; } } // Initialize velocity constraints. _contactSolver.Reset(_contacts, ContactCount, step.dtRatio, Settings.EnableWarmstarting); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Start(); _tmpTime = 0; } #endif for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref step); } } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _tmpTime += _watch.ElapsedTicks; } #endif // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } joint.SolveVelocityConstraints(ref step); joint.Validate(step.inv_dt); } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif _contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). _contactSolver.StoreImpulses(); // Integrate positions. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } // Check for large velocities. float translationX = step.dt * b.LinearVelocityInternal.X; float translationY = step.dt * b.LinearVelocityInternal.Y; float result = translationX * translationX + translationY * translationY; if (result > Settings.MaxTranslationSquared) { float sq = (float)Math.Sqrt(result); float ratio = Settings.MaxTranslation / sq; b.LinearVelocityInternal.X *= ratio; b.LinearVelocityInternal.Y *= ratio; } float rotation = step.dt * b.AngularVelocityInternal; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); b.AngularVelocityInternal *= ratio; } // Store positions for continuous collision. b.Sweep.C0.X = b.Sweep.C.X; b.Sweep.C0.Y = b.Sweep.C.Y; b.Sweep.A0 = b.Sweep.A; // Integrate b.Sweep.C.X += step.dt * b.LinearVelocityInternal.X; b.Sweep.C.Y += step.dt * b.LinearVelocityInternal.Y; b.Sweep.A += step.dt * b.AngularVelocityInternal; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); bool jointsOkay = true; #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } bool jointOkay = joint.SolvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { JointUpdateTime = _tmpTime; } #endif Report(_contactSolver.Constraints); if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if ((b.Flags & BodyFlags.AutoSleep) == 0) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } if ((b.Flags & BodyFlags.AutoSleep) == 0 || b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr || Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += step.dt; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= Settings.TimeToSleep) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
private bool SolvePositionConstraints(int start, int end) { float minSeparation = 0.0f; for (int i = start; i < end; ++i) { ContactPositionConstraint pc = _positionConstraints[i]; #if NET40 || NET45 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1 // Find lower order item. int orderedIndexA = pc.indexA; int orderedIndexB = pc.indexB; if (orderedIndexB < orderedIndexA) { orderedIndexA = pc.indexB; orderedIndexB = pc.indexA; } // Lock bodies. for (; ;) { if (Interlocked.CompareExchange(ref _locks[orderedIndexA], 1, 0) == 0) { if (Interlocked.CompareExchange(ref _locks[orderedIndexB], 1, 0) == 0) { break; } System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0); } #if NET40 || NET45 || NETSTANDARD2_0 Thread.Sleep(0); #endif } #endif int indexA = pc.indexA; int indexB = pc.indexB; Vector2 localCenterA = pc.localCenterA; float mA = pc.invMassA; float iA = pc.invIA; Vector2 localCenterB = pc.localCenterB; float mB = pc.invMassB; float iB = pc.invIB; int pointCount = pc.pointCount; Vector2 cA = _positions[indexA].c; float aA = _positions[indexA].a; Vector2 cB = _positions[indexB].c; float aB = _positions[indexB].a; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { Transform xfA = new Transform(Vector2.Zero, aA); Transform xfB = new Transform(Vector2.Zero, aB); xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q); xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q); Vector2 normal; Vector2 point; float separation; PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation); Vector2 rA = point - cA; Vector2 rB = point - cB; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(ref rA, ref normal); float rnB = MathUtils.Cross(ref rB, ref normal); float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; Vector2 P = impulse * normal; cA -= mA * P; aA -= iA * MathUtils.Cross(ref rA, ref P); cB += mB * P; aB += iB * MathUtils.Cross(ref rB, ref P); } _positions[indexA].c = cA; _positions[indexA].a = aA; _positions[indexB].c = cB; _positions[indexB].a = aB; #if NET40 || NET45 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1 // Unlock bodies. System.Threading.Interlocked.Exchange(ref _locks[orderedIndexB], 0); System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0); #endif } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -3.0f * Settings.LinearSlop); }
public override bool ShadowHit(Ray ray, ref double tmin) { if (!shadows || !bbox.Hit(ray)) { return(false); } double ox = ray.O.X; double oy = ray.O.Y; double oz = ray.O.Z; double dx = ray.D.X; double dy = ray.D.Y; double dz = ray.D.Z; double x0 = bbox.x0; double y0 = bbox.y0; double z0 = bbox.z0; double x1 = bbox.x0; double y1 = bbox.y0; double z1 = bbox.z0; double txMin = 0, tyMin = 0, tzMin = 0; double txMax = 0, tyMax = 0, tzMax = 0; double a = 1.0 / dx; if (a >= 0) { txMin = (x0 - ox) * a; txMax = (x1 - ox) * a; } else { txMin = (x1 - ox) * a; txMax = (x0 - ox) * a; } double b = 1.0 / dy; if (b >= 0) { tyMin = (y0 - oy) * b; tyMax = (y1 - oy) * b; } else { txMin = (y1 - oy) * b; txMax = (y0 - oy) * b; } double c = 1.0 / dz; if (c >= 0) { tzMin = (z0 - oz) * c; tzMax = (z1 - oz) * c; } else { tzMin = (z1 - oz) * c; tzMax = (z0 - oz) * c; } double t0, t1; if (txMin > tyMin) { t0 = txMax; } else { t0 = tyMin; } if (tzMin > t0) { t0 = tzMin; } if (txMax < tyMax) { t1 = txMax; } else { t1 = tyMax; } if (tzMax < t1) { t1 = tzMax; } if (t0 > t1) { return(false); } int ix, iy, iz; if (bbox.Inside(ray.O)) { ix = (int)MathUtils.Clamp((ox - x0) * nx / (x1 - x0), 0, nx - 1); iy = (int)MathUtils.Clamp((oy - y0) * ny / (y1 - y0), 0, ny - 1); iz = (int)MathUtils.Clamp((oz - z0) * nz / (z1 - z0), 0, nz - 1); } else { Vec3 p = ray.HitPoint(t0); ix = (int)MathUtils.Clamp((p.X - x0) * nx / (x1 - x0), 0, nx - 1); iy = (int)MathUtils.Clamp((p.Y - y0) * ny / (y1 - y0), 0, ny - 1); iz = (int)MathUtils.Clamp((p.Z - z0) * nz / (z1 - z0), 0, nz - 1); } double dtx = (txMax - txMin) / nx; double dty = (tyMax - tyMin) / ny; double dtz = (tzMax - tzMin) / nz; double txNext, tyNext, tzNext; int ixStep, iyStep, izStep; int ixStop, iyStop, izStop; if (dx > 0) { txNext = txMin + (ix + 1) * dtx; ixStep = +1; ixStop = nx; } else { txNext = txMin + (nx - ix) * dtx; ixStep = -1; ixStop = -1; } if (dx == 0.0) { txNext = MathUtils.HugeValue; ixStep = -1; ixStop = -1; } if (dy > 0) { tyNext = tyMin + (iy + 1) * dty; iyStep = +1; iyStop = ny; } else { tyNext = tyMin + (ny - iy) * dty; iyStep = -1; iyStop = -1; } if (dy == 0.0) { tyNext = MathUtils.HugeValue; iyStep = -1; iyStop = -1; } if (dz > 0) { tzNext = tzMin + (iz + 1) * dtz; izStep = +1; izStop = nz; } else { tzNext = tzMin + (nz - iz) * dtz; izStep = -1; izStop = -1; } if (dz == 0.0) { tzNext = MathUtils.HugeValue; izStep = -1; izStop = -1; } while (true) { GeometricObject obj = cells[ix + nx * iy + nx * ny * iz]; if (txNext < tyNext && txNext < tzNext) { if (obj != null && obj.ShadowHit(ray, ref tmin) && tmin < txNext) { material = obj.Material; return(true); } txNext += dtx; ix += ixStep; if (ix == ixStop) { return(false); } } else { if (tyNext < tzNext) { if (obj != null && obj.ShadowHit(ray, ref tmin) && tmin < tyNext) { material = obj.Material; return(true); } tyNext += dty; iy += iyStep; if (iy == iyStop) { return(false); } } else { if (obj != null && obj.ShadowHit(ray, ref tmin) && tmin < tzNext) { material = obj.Material; return(true); } tzNext += dtz; iz += izStep; if (iz == izStop) { return(false); } } } } }
internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; float impulse = _motorMass * (_motorSpeed - Cdot); float oldImpulse = MotorImpulse; float maxImpulse = data.step.dt * _maxMotorForce; MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse); impulse = MotorImpulse - oldImpulse; Vector2 P = impulse * _axis; float LA = impulse * _a1; float LB = impulse * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } Vector2 Cdot1 = new Vector2(); Cdot1.X = Vector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA; Cdot1.Y = wB - wA; if (_enableLimit && _limitState != LimitState.Inactive) { // Solve prismatic and limit constraint in block form. float Cdot2; Cdot2 = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 f1 = _impulse; Vector3 df = _K.Solve33(-Cdot); _impulse += df; if (_limitState == LimitState.AtLower) { _impulse.Z = Math.Max(_impulse.Z, 0.0f); } else if (_limitState == LimitState.AtUpper) { _impulse.Z = Math.Min(_impulse.Z, 0.0f); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.ez.X, _K.ez.Y); Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y); _impulse.X = f2r.X; _impulse.Y = f2r.Y; df = _impulse - f1; Vector2 P = df.X * _perp + df.Z * _axis; float LA = df.X * _s1 + df.Y + df.Z * _a1; float LB = df.X * _s2 + df.Y + df.Z * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } else { // Limit is inactive, just solve the prismatic constraint in block form. Vector2 df = _K.Solve22(-Cdot1); _impulse.X += df.X; _impulse.Y += df.Y; Vector2 P = df.X * _perp; float LA = df.X * _s1 + df.Y; float LB = df.X * _s2 + df.Y; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
public void SetupCells() { Vec3 p0 = MinCoordinates(); Vec3 p1 = MaxCoordinates(); bbox = new BBox(p0, p1); int numObjects = objects.Count; double wx = p1.X - p0.X; double wy = p1.Y - p0.Y; double wz = p1.Z - p0.Z; double multiplier = 2.0; double s = Math.Pow(wx * wy * wz / numObjects, 0.3333333); nx = (int)(multiplier * wx / s + 1); ny = (int)(multiplier * wy / s + 1); nz = (int)(multiplier * wz / s + 1); int numCells = nx * ny * nz; cells.Capacity = numObjects; for (int j = 0; j < numCells; j++) { cells.Add(null); } List <int> counts = new List <int>(numCells); for (int j = 0; j < numCells; j++) { counts.Add(0); } BBox objBbox; int index; for (int j = 0; j < numObjects; j++) { objBbox = objects[j].BoundingBox; int ixmin = (int)MathUtils.Clamp((objBbox.x0 - p0.X) * nx / (p1.X - p0.X), 0, nx - 1); int iymin = (int)MathUtils.Clamp((objBbox.y0 - p0.Y) * ny / (p1.Y - p0.Y), 0, ny - 1); int izmin = (int)MathUtils.Clamp((objBbox.z0 - p0.Z) * nz / (p1.Z - p0.Z), 0, nz - 1); int ixmax = (int)MathUtils.Clamp((objBbox.x1 - p0.X) * nx / (p1.X - p0.X), 0, nx - 1); int iymax = (int)MathUtils.Clamp((objBbox.y1 - p0.Y) * ny / (p1.Y - p0.Y), 0, ny - 1); int izmax = (int)MathUtils.Clamp((objBbox.z1 - p0.Z) * nz / (p1.Z - p0.Z), 0, nz - 1); for (int iz = izmin; iz <= izmax; iz++) { for (int iy = iymin; iy <= iymax; iy++) { for (int ix = ixmin; ix <= ixmax; ix++) { index = ix + nx * iy + nx * ny * iz; if (counts[index] == 0) { cells[index] = objects[j]; counts[index] += 1; } else { if (counts[index] == 1) { Compound c = new Compound(); c.AddObject(cells[index]); c.AddObject(objects[j]); cells[index] = c; counts[index] += 1; } else { if (cells[index] is Compound) { ((Compound)cells[index]).AddObject(objects[j]); } counts[index] += 1; } } } } } } objects.Clear(); counts.Clear(); }
internal override bool SolvePositionConstraints() { Body b1 = BodyA; Body b2 = BodyB; Vector2 s1 = GroundAnchorA; Vector2 s2 = GroundAnchorB; float linearError = 0.0f; if (_state == LimitState.AtUpper) { Transform xf1, xf2; b1.GetTransform(out xf1); b2.GetTransform(out xf2); Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); Vector2 p1 = b1.Sweep.C + r1; Vector2 p2 = b2.Sweep.C + r2; // Get the pulley axes. _u1 = p1 - s1; _u2 = p2 - s2; float length1 = _u1.magnitude; float length2 = _u2.magnitude; if (length1 > Settings.LinearSlop) { _u1 *= 1.0f / length1; } else { _u1 = Vector2.zero; } if (length2 > Settings.LinearSlop) { _u2 *= 1.0f / length2; } else { _u2 = Vector2.zero; } float C = _ant - length1 - Ratio * length2; linearError = Math.Max(linearError, -C); C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_pulleyMass * C; Vector2 P1 = -impulse * _u1; Vector2 P2 = -Ratio * impulse * _u2; b1.Sweep.C += b1.InvMass * P1; b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1); b2.Sweep.C += b2.InvMass * P2; b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2); b1.SynchronizeTransform(); b2.SynchronizeTransform(); } if (_limitState1 == LimitState.AtUpper) { Transform xf1; b1.GetTransform(out xf1); Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); Vector2 p1 = b1.Sweep.C + r1; _u1 = p1 - s1; float length1 = _u1.magnitude; if (length1 > Settings.LinearSlop) { _u1 *= 1.0f / length1; } else { _u1 = Vector2.zero; } float C = MaxLengthA - length1; linearError = Math.Max(linearError, -C); C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_limitMass1 * C; Vector2 P1 = -impulse * _u1; b1.Sweep.C += b1.InvMass * P1; b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1); b1.SynchronizeTransform(); } if (_limitState2 == LimitState.AtUpper) { Transform xf2; b2.GetTransform(out xf2); Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter); Vector2 p2 = b2.Sweep.C + r2; _u2 = p2 - s2; float length2 = _u2.magnitude; if (length2 > Settings.LinearSlop) { _u2 *= 1.0f / length2; } else { _u2 = Vector2.zero; } float C = MaxLengthB - length2; linearError = Math.Max(linearError, -C); C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_limitMass2 * C; Vector2 P2 = -impulse * _u2; b2.Sweep.C += b2.InvMass * P2; b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2); b2.SynchronizeTransform(); } return(linearError < Settings.LinearSlop); }
public void TestClamp() { Assert.AreEqual(0, MathUtils.Clamp(-100, 0, 1000), Delta); Assert.AreEqual(1, MathUtils.Clamp(1, 0, 2), Delta); Assert.AreEqual(2, MathUtils.Clamp(100, 0, 2), Delta); }
public void Solve(ref TimeStep step, ref FVector2 gravity) { float h = step.dt; //GABS:PROFILER UnityEngine.Profiling.Profiler.BeginSample("Int. Velocities"); // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; FVector2 c = b.Sweep.C; float a = b.Sweep.A; FVector2 v = b.LinearVelocity; float w = b.AngularVelocity; // Store positions for continuous collision. b.Sweep.C0 = b.Sweep.C; b.Sweep.A0 = b.Sweep.A; if (b.BodyType == BodyType.Dynamic) { // Integrate velocities. v += h * (b.GravityScale * gravity + b.InvMass * b.Force); w += h * b.InvI * b.Torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } //GABS:PROFILER UnityEngine.Profiling.Profiler.EndSample(); //GABS:PROFILER UnityEngine.Profiling.Profiler.BeginSample("Init. VConstraints"); // Solver data SolverData solverData = new SolverData(); solverData.step = step; solverData.positions = _positions; solverData.velocities = _velocities; // Initialize velocity constraints. //b2ContactSolverDef contactSolverDef; //contactSolverDef.step = step; //contactSolverDef.contacts = m_contacts; //contactSolverDef.count = m_contactCount; //contactSolverDef.positions = m_positions; //contactSolverDef.velocities = m_velocities; //contactSolverDef.allocator = m_allocator; _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); _contactSolver.InitializeVelocityConstraints(); if (Settings.EnableWarmstarting) { _contactSolver.WarmStart(); } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Start(); _tmpTime = 0; } #endif for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref solverData); } } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _tmpTime += _watch.ElapsedTicks; } #endif //GABS:PROFILER UnityEngine.Profiling.Profiler.EndSample(); //GABS:PROFILER UnityEngine.Profiling.Profiler.BeginSample("Solve VConstraints"); // Solve velocity constraints. for (int i = 0; i < Settings.VelocityIterations; ++i) { #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { FarseerJoint joint = _joints[j]; if (!joint.Enabled) { continue; } joint.SolveVelocityConstraints(ref solverData); //TODO: Move up before solve? joint.Validate(step.inv_dt); } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif //GABS:PROFILER UnityEngine.Profiling.Profiler.BeginSample("_contactSolver"); _contactSolver.SolveVelocityConstraints(); // HIGH //GABS:PROFILER UnityEngine.Profiling.Profiler.EndSample(); } //GABS:PROFILER UnityEngine.Profiling.Profiler.EndSample(); //GABS:PROFILER UnityEngine.Profiling.Profiler.BeginSample("Store Impulses"); // Store impulses for warm starting. _contactSolver.StoreImpulses(); //GABS:PROFILER UnityEngine.Profiling.Profiler.EndSample(); //GABS:PROFILER UnityEngine.Profiling.Profiler.BeginSample("Integrate Positions"); // Integrate positions for (int i = 0; i < BodyCount; ++i) { FVector2 c = _positions[i].c; float a = _positions[i].a; FVector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities FVector2 translation = h * v; if (FVector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } //GABS:PROFILER UnityEngine.Profiling.Profiler.EndSample(); //GABS:PROFILER UnityEngine.Profiling.Profiler.BeginSample("Solve PConstraints"); // Solve position constraints bool positionSolved = false; for (int i = 0; i < Settings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { FarseerJoint joint = _joints[j]; if (!joint.Enabled) { continue; } bool jointOkay = joint.SolvePositionConstraints(ref solverData); jointsOkay = jointsOkay && jointOkay; } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } #if (!SILVERLIGHT) if (Settings.EnableDiagnostics) { JointUpdateTime = _tmpTime; } #endif //GABS:PROFILER UnityEngine.Profiling.Profiler.EndSample(); // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body body = Bodies[i]; body.Sweep.C = _positions[i].c; body.Sweep.A = _positions[i].a; body.LinearVelocity = _velocities[i].v; body.AngularVelocity = _velocities[i].w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); if (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if ((b.Flags & BodyFlags.AutoSleep) == 0 || b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr || FVector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += h; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
public static Vector2 CalculateWind() { float t = Engine.Time * 0.1f; double a = Math.Cos(t) * Math.Sin(t * 1.3f) + Math.Cos(t * 1.5f); return(new Vector2(MathUtils.Clamp(-MaxSpeed, MaxSpeed, (float)Math.Cos(a)), MathUtils.Clamp(-MaxSpeed, MaxSpeed, (float)Math.Sin(a)))); }
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 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. { qA.Set(aA); qB.Set(aB); Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); 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(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 SolveVelocityConstraints(ref SolverData data) { float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; // Solve spring constraint { float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA; float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse); _springImpulse += impulse; Vector2 P = impulse * _ax; float LA = impulse * _sAx; float LB = impulse * _sBx; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } // Solve rotational motor constraint { float Cdot = wB - wA - _motorSpeed; float impulse = -_motorMass * Cdot; float oldImpulse = _motorImpulse; float maxImpulse = data.step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve point to line constraint { float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA; float impulse = -_mass * Cdot; _impulse += impulse; Vector2 P = impulse * _ay; float LA = impulse * _sAy; float LB = impulse * _sBy; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
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 SolveVelocityConstraints(ref TimeStep step) { Body b1 = BodyA; Body b2 = BodyB; Vector2 v1 = b1.LinearVelocityInternal; float w1 = b1.AngularVelocityInternal; Vector2 v2 = b2.LinearVelocityInternal; float w2 = b2.AngularVelocityInternal; float m1 = b1.InvMass, m2 = b2.InvMass; float i1 = b1.InvI, i2 = b2.InvI; // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = w2 - w1 - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; w1 -= i1 * impulse; w2 += i2 * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { /*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); // Solve point-to-point constraint MathUtils.Cross(w2, ref r2, out _tmpVector2); MathUtils.Cross(w1, ref r1, out _tmpVector1); Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1; float Cdot2 = w2 - w1; Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2); Vector3 impulse = _mass.Solve33(-Cdot); if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { float newImpulse = _impulse.z + impulse.z; if (newImpulse < 0.0f) { Vector2 rhs = -Cdot1 + _impulse.z * new Vector2(_mass.Col3.x, _mass.Col3.y); Vector2 reduced = _mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -_impulse.z; _impulse.x += reduced.x; _impulse.y += reduced.y; _impulse.z = 0.0f; } else { _impulse += impulse; } } else if (_limitState == LimitState.AtUpper) { float newImpulse = _impulse.z + impulse.z; if (newImpulse > 0.0f) { Vector2 rhs = -Cdot1 + _impulse.z * new Vector2(_mass.Col3.x, _mass.Col3.y); Vector2 reduced = _mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -_impulse.z; _impulse.x += reduced.x; _impulse.y += reduced.y; _impulse.z = 0.0f; } else { _impulse += impulse; } } Vector2 P = new Vector2(impulse.x, impulse.y); v1 -= m1 * P; MathUtils.Cross(ref r1, ref P, out _tmpFloat1); w1 -= i1 * (/* r1 x P */ _tmpFloat1 + impulse.z); v2 += m2 * P; MathUtils.Cross(ref r2, ref P, out _tmpFloat1); w2 += i2 * (/* r2 x P */ _tmpFloat1 + impulse.z); } else { /*Transform xf1, xf2; * b1.GetTransform(out xf1); * b2.GetTransform(out xf2);*/ _tmpVector1 = LocalAnchorA - b1.LocalCenter; _tmpVector2 = LocalAnchorB - b2.LocalCenter; Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1); Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2); // Solve point-to-point constraint MathUtils.Cross(w2, ref r2, out _tmpVector2); MathUtils.Cross(w1, ref r1, out _tmpVector1); Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1; Vector2 impulse = _mass.Solve22(-Cdot); _impulse.x += impulse.x; _impulse.y += impulse.y; v1 -= m1 * impulse; MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1); w1 -= i1 * /* r1 x impulse */ _tmpFloat1; v2 += m2 * impulse; MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1); w2 += i2 * /* r2 x impulse */ _tmpFloat1; } b1.LinearVelocityInternal = v1; b1.AngularVelocityInternal = w1; b2.LinearVelocityInternal = v2; b2.AngularVelocityInternal = w2; }