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);
}
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB)
{
manifold.PointCount = 0;
int edgeA = 0;
float separationA = FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
if (separationA > 0.0f)
{
return;
}
int edgeB = 0;
float separationB = FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
if (separationB > 0.0f)
{
return;
}
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
XForm xf1, xf2;
int edge1; // reference edge
byte flip;
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
flip = 0;
}
ClipVertex[] incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
Vector2[] vertices1 = poly1.GetVertices();
Vector2 v11 = vertices1[edge1];
Vector2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vector2 dv = v12 - v11;
Vector2 sideNormal = CommonMath.Mul(xf1.R, v12 - v11);
sideNormal.Normalize();
Vector2 frontNormal = CommonMath.Cross(sideNormal, 1.0f);
v11 = CommonMath.Mul(xf1, v11);
v12 = CommonMath.Mul(xf1, v12);
float frontOffset = Vector2.Dot(frontNormal, v11);
float sideOffset1 = -Vector2.Dot(sideNormal, v11);
float sideOffset2 = Vector2.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.Normal = flip != 0 ? -frontNormal : frontNormal;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;
if (separation <= 0.0f)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.Separation = separation;
cp.LocalPoint1 = CommonMath.MulT(xfA, clipPoints2[i].V);
cp.LocalPoint2 = CommonMath.MulT(xfB, clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
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);
}
internal override void InitVelocityConstraints(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());
Vector2 p1 = b1._sweep.C + r1;
Vector2 p2 = b2._sweep.C + r2;
Vector2 s1 = _ground.GetXForm().Position + _groundAnchor1;
Vector2 s2 = _ground.GetXForm().Position + _groundAnchor2;
// 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;
if (C > 0.0f)
{
_state = LimitState.InactiveLimit;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpperLimit;
}
if (length1 < _maxLength1)
{
_limitState1 = LimitState.InactiveLimit;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpperLimit;
}
if (length2 < _maxLength2)
{
_limitState2 = LimitState.InactiveLimit;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpperLimit;
}
// Compute effective mass.
float cr1u1 = CommonMath.Cross(ref r1, ref _u1);
float cr2u2 = CommonMath.Cross(ref r2, ref _u2);
_limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
_limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
//Box2DXDebug.Assert(_limitMass1 > Settings.FLT_EPSILON);
//Box2DXDebug.Assert(_limitMass2 > Settings.FLT_EPSILON);
//Box2DXDebug.Assert(_pulleyMass > Settings.FLT_EPSILON);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (step.WarmStarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.DtRatio;
_limitImpulse1 *= step.DtRatio;
_limitImpulse2 *= step.DtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _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);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
public static int ProcessThree(out Vector2 x1, out Vector2 x2, ref Vector2[] p1s, ref Vector2[] p2s, ref Vector2[] points)
{
Vector2 a = points[0];
Vector2 b = points[1];
Vector2 c = points[2];
Vector2 ab = b - a;
Vector2 ac = c - a;
Vector2 bc = c - b;
float sn = -Vector2.Dot(a, ab), sd = Vector2.Dot(b, ab);
float tn = -Vector2.Dot(a, ac), td = Vector2.Dot(c, ac);
float un = -Vector2.Dot(b, bc), ud = Vector2.Dot(c, bc);
// In vertex c region?
if (td <= 0.0f && ud <= 0.0f)
{
// Single point
x1 = p1s[2];
x2 = p2s[2];
p1s[0] = p1s[2];
p2s[0] = p2s[2];
points[0] = points[2];
return(1);
}
// Should not be in vertex a or b region.
//B2_NOT_USED(sd);
//B2_NOT_USED(sn);
//Box2DXDebug.Assert(sn > 0.0f || tn > 0.0f);
//Box2DXDebug.Assert(sd > 0.0f || un > 0.0f);
float n = CommonMath.Cross(ref ab, ref ac);
#if TARGET_FLOAT32_IS_FIXED
n = (n < 0.0f) ? -1.0f : ((n > 0.0f) ? 1.0f : 0.0f);
#endif
// Should not be in edge ab region.
float vc = n * CommonMath.Cross(ref a, ref b);
//Box2DXDebug.Assert(vc > 0.0f || sn > 0.0f || sd > 0.0f);
// In edge bc region?
float va = n * CommonMath.Cross(ref b, ref c);
if (va <= 0.0f && un >= 0.0f && ud >= 0.0f && (un + ud) > 0.0f)
{
//Box2DXDebug.Assert(un + ud > 0.0f);
float lambda = un / (un + ud);
x1 = p1s[1] + lambda * (p1s[2] - p1s[1]);
x2 = p2s[1] + lambda * (p2s[2] - p2s[1]);
p1s[0] = p1s[2];
p2s[0] = p2s[2];
points[0] = points[2];
return(2);
}
// In edge ac region?
float vb = n * CommonMath.Cross(ref c, ref a);
if (vb <= 0.0f && tn >= 0.0f && td >= 0.0f && (tn + td) > 0.0f)
{
//Box2DXDebug.Assert(tn + td > 0.0f);
float lambda = tn / (tn + td);
x1 = p1s[0] + lambda * (p1s[2] - p1s[0]);
x2 = p2s[0] + lambda * (p2s[2] - p2s[0]);
p1s[1] = p1s[2];
p2s[1] = p2s[2];
points[1] = points[2];
return(2);
}
// Inside the triangle, compute barycentric coordinates
float denom = va + vb + vc;
//Box2DXDebug.Assert(denom > 0.0f);
denom = 1.0f / denom;
#if TARGET_FLOAT32_IS_FIXED
x1 = denom * (va * p1s[0] + vb * p1s[1] + vc * p1s[2]);
x2 = denom * (va * p2s[0] + vb * p2s[1] + vc * p2s[2]);
#else
float u = va * denom;
float v = vb * denom;
float w = 1.0f - u - v;
x1 = u * p1s[0] + v * p1s[1] + w * p1s[2];
x2 = u * p2s[0] + v * p2s[1] + w * p2s[2];
#endif
return(3);
}
