internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); if (_state == LimitState.AtUpperLimit) { Vector2 v1 = b1._linearVelocity + CommonMath.Cross(b1._angularVelocity, r1); Vector2 v2 = b2._linearVelocity + CommonMath.Cross(b2._angularVelocity, r2); float Cdot = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2); float impulse = _pulleyMass * (-Cdot); float oldImpulse = _impulse; _impulse = CommonMath.Max(0.0f, _impulse + impulse); impulse = _impulse - oldImpulse; Vector2 P1 = -impulse * _u1; Vector2 P2 = -_ratio * impulse * _u2; b1._linearVelocity += b1._invMass * P1; b1._angularVelocity += b1._invI * CommonMath.Cross(ref r1, ref P1); b2._linearVelocity += b2._invMass * P2; b2._angularVelocity += b2._invI * CommonMath.Cross(ref r2, ref P2); } if (_limitState1 == LimitState.AtUpperLimit) { Vector2 v1 = b1._linearVelocity + CommonMath.Cross(b1._angularVelocity, r1); float Cdot = -Vector2.Dot(_u1, v1); float impulse = -_limitMass1 * Cdot; float oldImpulse = _limitImpulse1; _limitImpulse1 = CommonMath.Max(0.0f, _limitImpulse1 + impulse); impulse = _limitImpulse1 - oldImpulse; Vector2 P1 = -impulse * _u1; b1._linearVelocity += b1._invMass * P1; b1._angularVelocity += b1._invI * CommonMath.Cross(ref r1, ref P1); } if (_limitState2 == LimitState.AtUpperLimit) { Vector2 v2 = b2._linearVelocity + CommonMath.Cross(b2._angularVelocity, r2); float Cdot = -Vector2.Dot(_u2, v2); float impulse = -_limitMass2 * Cdot; float oldImpulse = _limitImpulse2; _limitImpulse2 = CommonMath.Max(0.0f, _limitImpulse2 + impulse); impulse = _limitImpulse2 - oldImpulse; Vector2 P2 = -impulse * _u2; b2._linearVelocity += b2._invMass * P2; b2._angularVelocity += b2._invI * CommonMath.Cross(ref r2, ref P2); } }
public static bool InPoints(Vector2 w, Vector2[] points, int pointCount) { float k_tolerance = 100.0f * Settings.FLT_EPSILON; for (int i = 0; i < pointCount; ++i) { Vector2 d = CommonMath.Abs(w - points[i]); Vector2 m = CommonMath.Max(CommonMath.Abs(w), CommonMath.Abs(points[i])); if (d.X < k_tolerance * (m.X + 1.0f) && d.Y < k_tolerance * (m.Y + 1.0f)) { return(true); } } return(false); }
// This algorithm uses conservative advancement to compute the time of // impact (TOI) of two shapes. // Refs: Bullet, Young Kim /// <summary> /// Compute the time when two shapes begin to touch or touch at a closer distance. /// warning the sweeps must have the same time interval. /// </summary> /// <param name="shape1"></param> /// <param name="sweep1"></param> /// <param name="shape2"></param> /// <param name="sweep2"></param> /// <returns> /// The fraction between [0,1] in which the shapes first touch. /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch. /// </returns> public static float TimeOfImpact(Shape shape1, Sweep sweep1, Shape shape2, Sweep sweep2) { float r1 = shape1.GetSweepRadius(); float r2 = shape2.GetSweepRadius(); //Box2DXDebug.Assert(sweep1.T0 == sweep2.T0); //Box2DXDebug.Assert(1.0f - sweep1.T0 > Common.Settings.FLT_EPSILON); float t0 = sweep1.T0; Vector2 v1 = sweep1.C - sweep1.C0; Vector2 v2 = sweep2.C - sweep2.C0; float omega1 = sweep1.A - sweep1.A0; float omega2 = sweep2.A - sweep2.A0; float alpha = 0.0f; Vector2 p1, p2; int k_maxIterations = 20; // TODO_ERIN b2Settings int iter = 0; Vector2 normal = Vector2.Zero; float distance = 0.0f; float targetDistance = 0.0f; for (;;) { float t = (1.0f - alpha) * t0 + alpha; XForm xf1, xf2; sweep1.GetXForm(out xf1, t); sweep2.GetXForm(out xf2, t); // Get the distance between shapes. distance = Distance(out p1, out p2, shape1, xf1, shape2, xf2); if (iter == 0) { // Compute a reasonable target distance to give some breathing room // for conservative advancement. if (distance > 2.0f * Settings.ToiSlop) { targetDistance = 1.5f * Settings.ToiSlop; } else { targetDistance = CommonMath.Max(0.05f * Settings.ToiSlop, distance - 0.5f * Settings.ToiSlop); } } if (distance - targetDistance < 0.05f * Settings.ToiSlop || iter == k_maxIterations) { break; } normal = p2 - p1; normal.Normalize(); // Compute upper bound on remaining movement. float approachVelocityBound = Vector2.Dot(normal, v1 - v2) + CommonMath.Abs(omega1) * r1 + CommonMath.Abs(omega2) * r2; if (CommonMath.Abs(approachVelocityBound) < Settings.FLT_EPSILON) { alpha = 1.0f; break; } // Get the conservative time increment. Don't advance all the way. float dAlpha = (distance - targetDistance) / approachVelocityBound; //float dt = (distance - 0.5f * Settings.LinearSlop) / approachVelocityBound; float newAlpha = alpha + dAlpha; // The shapes may be moving apart or a safe distance apart. if (newAlpha < 0.0f || 1.0f < newAlpha) { alpha = 1.0f; break; } // Ensure significant advancement. if (newAlpha < (1.0f + 100.0f * Settings.FLT_EPSILON) * alpha) { break; } alpha = newAlpha; ++iter; } return(alpha); }
internal override bool SolvePositionConstraints(float baumgarte) { Body b1 = _body1; Body b2 = _body2; Vector2 s1 = _ground.GetXForm().Position + _groundAnchor1; Vector2 s2 = _ground.GetXForm().Position + _groundAnchor2; float linearError = 0.0f; if (_state == LimitState.AtUpperLimit) { Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); 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.Length(); float length2 = _u2.Length(); 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 = _constant - length1 - _ratio * length2; linearError = CommonMath.Max(linearError, -C); C = CommonMath.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 * CommonMath.Cross(ref r1, ref P1); b2._sweep.C += b2._invMass * P2; b2._sweep.A += b2._invI * CommonMath.Cross(ref r2, ref P2); b1.SynchronizeTransform(); b2.SynchronizeTransform(); } if (_limitState1 == LimitState.AtUpperLimit) { Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vector2 p1 = b1._sweep.C + r1; _u1 = p1 - s1; float length1 = _u1.Length(); if (length1 > Settings.LinearSlop) { _u1 *= 1.0f / length1; } else { _u1 = Vector2.Zero; } float C = _maxLength1 - length1; linearError = CommonMath.Max(linearError, -C); C = CommonMath.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 * CommonMath.Cross(ref r1, ref P1); b1.SynchronizeTransform(); } if (_limitState2 == LimitState.AtUpperLimit) { Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vector2 p2 = b2._sweep.C + r2; _u2 = p2 - s2; float length2 = _u2.Length(); if (length2 > Settings.LinearSlop) { _u2 *= 1.0f / length2; } else { _u2 = Vector2.Zero; } float C = _maxLength2 - length2; linearError = CommonMath.Max(linearError, -C); C = CommonMath.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 * CommonMath.Cross(ref r2, ref P2); b2.SynchronizeTransform(); } return(linearError < Settings.LinearSlop); }
public static float DistanceGeneric <T1, T2>(out Vector2 x1, out Vector2 x2, T1 shape1_, XForm xf1, T2 shape2_, XForm xf2) { IGenericShape shape1 = shape1_ as IGenericShape; IGenericShape shape2 = shape2_ as IGenericShape; if (shape1 == null || shape2 == null) { //Box2DXDebug.Assert(false, "Can not cast some parameters to IGenericShape"); } Vector2[] p1s = new Vector2[3], p2s = new Vector2[3]; Vector2[] points = new Vector2[3]; int pointCount = 0; x1 = shape1.GetFirstVertex(xf1); x2 = shape2.GetFirstVertex(xf2); float vSqr = 0.0f; int maxIterations = 20; for (int iter = 0; iter < maxIterations; ++iter) { Vector2 v = x2 - x1; Vector2 w1 = shape1.Support(xf1, v); Vector2 w2 = shape2.Support(xf2, -v); vSqr = Vector2.Dot(v, v); Vector2 w = w2 - w1; float vw = Vector2.Dot(v, w); if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points { if (pointCount == 0) { x1 = w1; x2 = w2; } //GJKIterations = iter; return(CommonMath.Sqrt(vSqr)); } switch (pointCount) { case 0: p1s[0] = w1; p2s[0] = w2; points[0] = w; x1 = p1s[0]; x2 = p2s[0]; ++pointCount; break; case 1: p1s[1] = w1; p2s[1] = w2; points[1] = w; pointCount = ProcessTwo(out x1, out x2, ref p1s, ref p2s, ref points); break; case 2: p1s[2] = w1; p2s[2] = w2; points[2] = w; pointCount = ProcessThree(out x1, out x2, ref p1s, ref p2s, ref points); break; } // If we have three points, then the origin is in the corresponding triangle. if (pointCount == 3) { //GJKIterations = iter; return(0.0f); } float maxSqr = -Settings.FLT_MAX; for (int i = 0; i < pointCount; ++i) { maxSqr = CommonMath.Max(maxSqr, Vector2.Dot(points[i], points[i])); } #if TARGET_FLOAT32_IS_FIXED if (pointCount == 3 || vSqr <= 5.0 * Common.Settings.FLT_EPSILON * maxSqr) #else if (vSqr <= 100.0f * Settings.FLT_EPSILON * maxSqr) #endif { //GJKIterations = iter; v = x2 - x1; vSqr = Vector2.Dot(v, v); return(CommonMath.Sqrt(vSqr)); } } //GJKIterations = maxIterations; return(CommonMath.Sqrt(vSqr)); }