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));
}