internal unsafe void Initialize(SimplexCache *cache,
Shape shapeA, XForm transformA,
Shape shapeB, XForm transformB)
{
ShapeA = shapeA;
ShapeB = shapeB;
int count = cache->Count;
Box2DXDebug.Assert(0 < count && count < 3);
if (count == 1)
{
FaceType = Type.Points;
Vec2 localPointA = ShapeA.GetVertex(cache->IndexA[0]);
Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]);
Vec2 pointA = Common.Math.Mul(transformA, localPointA);
Vec2 pointB = Common.Math.Mul(transformB, localPointB);
Axis = pointB - pointA;
Axis.Normalize();
}
else if (cache->IndexB[0] == cache->IndexB[1])
{
// Two points on A and one on B
FaceType = Type.FaceA;
Vec2 localPointA1 = ShapeA.GetVertex(cache->IndexA[0]);
Vec2 localPointA2 = ShapeA.GetVertex(cache->IndexA[1]);
Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]);
LocalPoint = 0.5f * (localPointA1 + localPointA2);
Axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
Axis.Normalize();
Vec2 normal = Common.Math.Mul(transformA.R, Axis);
Vec2 pointA = Common.Math.Mul(transformA, LocalPoint);
Vec2 pointB = Common.Math.Mul(transformB, localPointB);
float s = Vec2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
Axis = -Axis;
}
}
else
{
// Two points on B and one or two points on A.
// We ignore the second point on A.
FaceType = Type.FaceB;
Vec2 localPointA = shapeA.GetVertex(cache->IndexA[0]);
Vec2 localPointB1 = shapeB.GetVertex(cache->IndexB[0]);
Vec2 localPointB2 = shapeB.GetVertex(cache->IndexB[1]);
LocalPoint = 0.5f * (localPointB1 + localPointB2);
Axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
Axis.Normalize();
Vec2 normal = Common.Math.Mul(transformB.R, Axis);
Vec2 pointB = Common.Math.Mul(transformB, LocalPoint);
Vec2 pointA = Common.Math.Mul(transformA, localPointA);
float s = Vec2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
Axis = -Axis;
}
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.C + r1;
Vector2 p2 = b2._sweep.C + r2;
Vector2 s1 = _ground.GetTransform().position + _groundAnchor1;
Vector2 s2 = _ground.GetTransform().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 = r1.Cross(_u1);
float cr2u2 = r2.Cross(_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 * r1.Cross(P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * r2.Cross(P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
// Create and destroy proxies. These call Flush first.
public ushort CreateProxy(AABB aabb, object userData)
{
Box2DXDebug.Assert(_proxyCount < Settings.MaxProxies);
Box2DXDebug.Assert(_freeProxy != PairManager.NullProxy);
ushort proxyId = _freeProxy;
Proxy proxy = _proxyPool[proxyId];
_freeProxy = proxy.Next;
proxy.OverlapCount = 0;
proxy.UserData = userData;
int boundCount = 2 * _proxyCount;
ushort[] lowerValues = new ushort[2], upperValues = new ushort[2];
ComputeBounds(out lowerValues, out upperValues, aabb);
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
int lowerIndex, upperIndex;
Query(out lowerIndex, out upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);
#warning "Check this"
//memmove(bounds + upperIndex + 2, bounds + upperIndex, (boundCount - upperIndex) * sizeof(b2Bound));
Bound[] tmp = new Bound[boundCount - upperIndex];
for (int i = 0; i < (boundCount - upperIndex); i++)
{
tmp[i] = bounds[upperIndex + i].Clone();
}
for (int i = 0; i < (boundCount - upperIndex); i++)
{
bounds[upperIndex + 2 + i] = tmp[i];
}
//memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(b2Bound));
tmp = new Bound[upperIndex - lowerIndex];
for (int i = 0; i < (upperIndex - lowerIndex); i++)
{
tmp[i] = bounds[lowerIndex + i].Clone();
}
for (int i = 0; i < (upperIndex - lowerIndex); i++)
{
bounds[lowerIndex + 1 + i] = tmp[i];
}
// The upper index has increased because of the lower bound insertion.
++upperIndex;
// Copy in the new bounds.
bounds[lowerIndex].Value = lowerValues[axis];
bounds[lowerIndex].ProxyId = proxyId;
bounds[upperIndex].Value = upperValues[axis];
bounds[upperIndex].ProxyId = proxyId;
bounds[lowerIndex].StabbingCount = lowerIndex == 0 ? (ushort)0 : bounds[lowerIndex - 1].StabbingCount;
bounds[upperIndex].StabbingCount = bounds[upperIndex - 1].StabbingCount;
// Adjust the stabbing count between the new bounds.
for (int index = lowerIndex; index < upperIndex; ++index)
{
++bounds[index].StabbingCount;
}
// Adjust the all the affected bound indices.
for (int index = lowerIndex; index < boundCount + 2; ++index)
{
Proxy proxy_ = _proxyPool[bounds[index].ProxyId];
if (bounds[index].IsLower)
{
proxy_.LowerBounds[axis] = (ushort)index;
}
else
{
proxy_.UpperBounds[axis] = (ushort)index;
}
}
}
++_proxyCount;
Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies);
// Create pairs if the AABB is in range.
for (int i = 0; i < _queryResultCount; ++i)
{
Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies);
Box2DXDebug.Assert(_proxyPool[_queryResults[i]].IsValid);
_pairManager.AddBufferedPair(proxyId, _queryResults[i]);
}
_pairManager.Commit();
if (IsValidate)
{
Validate();
}
// Prepare for next query.
_queryResultCount = 0;
IncrementTimeStamp();
return(proxyId);
}
internal void ContactSolverSetup(Manifold manifold, WorldManifold worldManifold, ContactConstraint cc)
{
// this is kind of yucky but we do know these were setup before entry to this method
var bodyA = cc.BodyA;
var bodyB = cc.BodyB;
Vector2 vA = bodyA._linearVelocity;
Vector2 vB = bodyB._linearVelocity;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
ContactConstraintPoint[] ccPointsPtr = cc.Points;
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint ccp = ccPointsPtr[j];
ccp.NormalImpulse = cp.NormalImpulse;
ccp.TangentImpulse = cp.TangentImpulse;
ccp.LocalPoint = cp.LocalPoint;
ccp.RA = worldManifold.Points[j] - bodyA._sweep.C;
ccp.RB = worldManifold.Points[j] - bodyB._sweep.C;
float rnA = ccp.RA.Cross(cc.Normal);
float rnB = ccp.RB.Cross(cc.Normal);
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
ccp.NormalMass = 1.0f / kNormal;
float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;
Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
ccp.EqualizedMass = 1.0f / kEqualized;
Vector2 tangent = cc.Normal.CrossScalarPostMultiply(1.0f);
float rtA = ccp.RA.Cross(tangent);
float rtB = ccp.RB.Cross(tangent);
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
float vRel = Vector2.Dot(cc.Normal, vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA));
if (vRel < -Common.Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = ccPointsPtr[0];
ContactConstraintPoint ccp2 = ccPointsPtr[1];
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
float rn1A = ccp1.RA.Cross(cc.Normal);
float rn1B = ccp1.RB.Cross(cc.Normal);
float rn2A = ccp2.RA.Cross(cc.Normal);
float rn2B = ccp2.RB.Cross(cc.Normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K.Col1 = new Vector2(k11, k12);
cc.K.Col2 = new Vector2(k12, k22);
cc.NormalMass = cc.K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
Vec2 vA = bodyA._linearVelocity;
Vec2 vB = bodyB._linearVelocity;
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
Vec2 normal = c.Normal;
Vec2 tangent = Vec2.Cross(normal, 1.0f);
float friction = c.Friction;
Box2DNetDebug.Assert(c.PointCount == 1 || c.PointCount == 2);
unsafe
{
fixed(ContactConstraintPoint *pointsPtr = c.Points)
{
// Solve tangent constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint *ccp = &pointsPtr[j];
// Relative velocity at contact
Vec2 dv = vB + Vec2.Cross(wB, ccp->RB) - vA - Vec2.Cross(wA, ccp->RA);
// Compute tangent force
float vt = Vec2.Dot(dv, tangent);
float lambda = ccp->TangentMass * (-vt);
// b2Clamp the accumulated force
float maxFriction = friction * ccp->NormalImpulse;
float newImpulse = Common.Math.Clamp(ccp->TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp->TangentImpulse;
// Apply contact impulse
Vec2 P = lambda * tangent;
vA -= invMassA * P;
wA -= invIA * Vec2.Cross(ccp->RA, P);
vB += invMassB * P;
wB += invIB * Vec2.Cross(ccp->RB, P);
ccp->TangentImpulse = newImpulse;
}
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
Vec2 dv = vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA);
// Compute normal impulse
float vn = Vec2.Dot(dv, normal);
float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);
// Clamp the accumulated impulse
float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp.NormalImpulse;
// Apply contact impulse
Vec2 P = lambda * normal;
vA -= invMassA * P;
wA -= invIA * Vec2.Cross(ccp.RA, P);
vB += invMassB * P;
wB += invIB * Vec2.Cross(ccp.RB, P);
ccp.NormalImpulse = newImpulse;
}
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 = x' - a
//
// Plug into above equation:
//
// vn = A * x + b
// = A * (x' - a) + b
// = A * x' + b - A * a
// = A * x' + b'
// b' = b - A * a;
ContactConstraintPoint *cp1 = &pointsPtr[0];
ContactConstraintPoint *cp2 = &pointsPtr[1];
Vec2 a = new Vec2(cp1->NormalImpulse, cp2->NormalImpulse);
Box2DNetDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
// Relative velocity at contact
Vec2 dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);
Vec2 dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);
// Compute normal velocity
float vn1 = Vec2.Dot(dv1, normal);
float vn2 = Vec2.Dot(dv2, normal);
Vec2 b;
b.X = vn1 - cp1->VelocityBias;
b.Y = vn2 - cp2->VelocityBias;
b -= Common.Math.Mul(c.K, 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'
//
Vec2 x = -Common.Math.Mul(c.NormalMass, b);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
// Resubstitute for the incremental impulse
Vec2 d = x - a;
// Apply incremental impulse
Vec2 P1 = d.X * normal;
Vec2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2));
// Accumulate
cp1->NormalImpulse = x.X;
cp2->NormalImpulse = x.Y;
#if DEBUG_SOLVER
// Postconditions
dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);
dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);
// Compute normal velocity
vn1 = Vec2.Dot(dv1, normal);
vn2 = Vec2.Dot(dv2, normal);
Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
Box2DXDebug.Assert(Common.Math.Abs(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 = c.K.Col1.Y * x.X + b.Y;
if (x.X >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
Vec2 d = x - a;
// Apply incremental impulse
Vec2 P1 = d.X * normal;
Vec2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2));
// Accumulate
cp1->NormalImpulse = x.X;
cp2->NormalImpulse = x.Y;
#if DEBUG_SOLVER
// Postconditions
dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);
// Compute normal velocity
vn1 = Vec2.Dot(dv1, normal);
Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: w2 = 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 = c.K.Col2.X * x.Y + b.X;
vn2 = 0.0f;
if (x.Y >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
Vec2 d = x - a;
// Apply incremental impulse
Vec2 P1 = d.X * normal;
Vec2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, P2));
// Accumulate
cp1->NormalImpulse = x.X;
cp2->NormalImpulse = x.Y;
#if DEBUG_SOLVER
// Postconditions
dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);
// Compute normal velocity
vn2 = Vec2.Dot(dv2, normal);
Box2DXDebug.Assert(Common.Math.Abs(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
Vec2 d = x - a;
// Apply incremental impulse
Vec2 P1 = d.X * normal;
Vec2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (Vec2.Cross(cp1->RA, P1) + Vec2.Cross(cp2->RA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (Vec2.Cross(cp1->RB, P1) + Vec2.Cross(cp2->RB, 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;
}
}
bodyA._linearVelocity = vA;
bodyA._angularVelocity = wA;
bodyB._linearVelocity = vB;
bodyB._angularVelocity = wB;
}
}
}
}
internal PolygonShape(ShapeDef def) : base(def)
{
Box2DXDebug.Assert(def.Type == ShapeType.PolygonShape);
this._type = ShapeType.PolygonShape;
PolygonDef polygonDef = (PolygonDef)def;
this._vertexCount = polygonDef.VertexCount;
Box2DXDebug.Assert(3 <= this._vertexCount && this._vertexCount <= Settings.MaxPolygonVertices);
for (int i = 0; i < this._vertexCount; i++)
{
this._vertices[i] = polygonDef.Vertices[i];
}
for (int i = 0; i < this._vertexCount; i++)
{
int num = i;
int num2 = (i + 1 < this._vertexCount) ? (i + 1) : 0;
Vec2 a = this._vertices[num2] - this._vertices[num];
Box2DXDebug.Assert(a.LengthSquared() > Settings.FLT_EPSILON * Settings.FLT_EPSILON);
this._normals[i] = Vec2.Cross(a, 1f);
this._normals[i].Normalize();
}
for (int i = 0; i < this._vertexCount; i++)
{
for (int j = 0; j < this._vertexCount; j++)
{
if (j != i && j != (i + 1) % this._vertexCount)
{
float num3 = Vec2.Dot(this._normals[i], this._vertices[j] - this._vertices[i]);
Box2DXDebug.Assert(num3 < -Settings.LinearSlop);
}
}
}
for (int i = 1; i < this._vertexCount; i++)
{
float num4 = Vec2.Cross(this._normals[i - 1], this._normals[i]);
num4 = Box2DX.Common.Math.Clamp(num4, -1f, 1f);
float num5 = (float)System.Math.Asin((double)num4);
Box2DXDebug.Assert(num5 > Settings.AngularSlop);
}
this._centroid = PolygonShape.ComputeCentroid(polygonDef.Vertices, polygonDef.VertexCount);
PolygonShape.ComputeOBB(out this._obb, this._vertices, this._vertexCount);
for (int i = 0; i < this._vertexCount; i++)
{
int num = (i - 1 >= 0) ? (i - 1) : (this._vertexCount - 1);
int num2 = i;
Vec2 a2 = this._normals[num];
Vec2 a3 = this._normals[num2];
Vec2 b = this._vertices[i] - this._centroid;
Vec2 b2 = default(Vec2);
b2.X = Vec2.Dot(a2, b) - Settings.ToiSlop;
b2.Y = Vec2.Dot(a3, b) - Settings.ToiSlop;
Box2DXDebug.Assert(b2.X >= 0f);
Box2DXDebug.Assert(b2.Y >= 0f);
Mat22 mat = default(Mat22);
mat.Col1.X = a2.X;
mat.Col2.X = a2.Y;
mat.Col1.Y = a3.X;
mat.Col2.Y = a3.Y;
this._coreVertices[i] = mat.Solve(b2) + this._centroid;
}
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
this._step = step;
this._constraintCount = 0;
for (int i = 0; i < contactCount; i++)
{
Box2DXDebug.Assert(contacts[i].IsSolid());
this._constraintCount += contacts[i].GetManifoldCount();
}
this._constraints = new ContactConstraint[this._constraintCount];
for (int i = 0; i < this._constraintCount; i++)
{
this._constraints[i] = new ContactConstraint();
}
int num = 0;
for (int i = 0; i < contactCount; i++)
{
Contact contact = contacts[i];
Shape shape = contact._shape1;
Shape shape2 = contact._shape2;
Body body = shape.GetBody();
Body body2 = shape2.GetBody();
int manifoldCount = contact.GetManifoldCount();
Manifold[] manifolds = contact.GetManifolds();
float friction = Settings.MixFriction(shape.Friction, shape2.Friction);
float restitution = Settings.MixRestitution(shape.Restitution, shape2.Restitution);
Vec2 linearVelocity = body._linearVelocity;
Vec2 linearVelocity2 = body2._linearVelocity;
float angularVelocity = body._angularVelocity;
float angularVelocity2 = body2._angularVelocity;
for (int j = 0; j < manifoldCount; j++)
{
Manifold manifold = manifolds[j];
Box2DXDebug.Assert(manifold.PointCount > 0);
Vec2 normal = manifold.Normal;
Box2DXDebug.Assert(num < this._constraintCount);
ContactConstraint contactConstraint = this._constraints[num];
contactConstraint.Body1 = body;
contactConstraint.Body2 = body2;
contactConstraint.Manifold = manifold;
contactConstraint.Normal = normal;
contactConstraint.PointCount = manifold.PointCount;
contactConstraint.Friction = friction;
contactConstraint.Restitution = restitution;
for (int k = 0; k < contactConstraint.PointCount; k++)
{
ManifoldPoint manifoldPoint = manifold.Points[k];
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[k];
contactConstraintPoint.NormalImpulse = manifoldPoint.NormalImpulse;
contactConstraintPoint.TangentImpulse = manifoldPoint.TangentImpulse;
contactConstraintPoint.Separation = manifoldPoint.Separation;
contactConstraintPoint.LocalAnchor1 = manifoldPoint.LocalPoint1;
contactConstraintPoint.LocalAnchor2 = manifoldPoint.LocalPoint2;
contactConstraintPoint.R1 = Box2DX.Common.Math.Mul(body.GetXForm().R, manifoldPoint.LocalPoint1 - body.GetLocalCenter());
contactConstraintPoint.R2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, manifoldPoint.LocalPoint2 - body2.GetLocalCenter());
float num2 = Vec2.Cross(contactConstraintPoint.R1, normal);
float num3 = Vec2.Cross(contactConstraintPoint.R2, normal);
num2 *= num2;
num3 *= num3;
float num4 = body._invMass + body2._invMass + body._invI * num2 + body2._invI * num3;
Box2DXDebug.Assert(num4 > Settings.FLT_EPSILON);
contactConstraintPoint.NormalMass = 1f / num4;
float num5 = body._mass * body._invMass + body2._mass * body2._invMass;
num5 += body._mass * body._invI * num2 + body2._mass * body2._invI * num3;
Box2DXDebug.Assert(num5 > Settings.FLT_EPSILON);
contactConstraintPoint.EqualizedMass = 1f / num5;
Vec2 b = Vec2.Cross(normal, 1f);
float num6 = Vec2.Cross(contactConstraintPoint.R1, b);
float num7 = Vec2.Cross(contactConstraintPoint.R2, b);
num6 *= num6;
num7 *= num7;
float num8 = body._invMass + body2._invMass + body._invI * num6 + body2._invI * num7;
Box2DXDebug.Assert(num8 > Settings.FLT_EPSILON);
contactConstraintPoint.TangentMass = 1f / num8;
contactConstraintPoint.VelocityBias = 0f;
if (contactConstraintPoint.Separation > 0f)
{
contactConstraintPoint.VelocityBias = -step.Inv_Dt * contactConstraintPoint.Separation;
}
else
{
float num9 = Vec2.Dot(contactConstraint.Normal, linearVelocity2 + Vec2.Cross(angularVelocity2, contactConstraintPoint.R2) - linearVelocity - Vec2.Cross(angularVelocity, contactConstraintPoint.R1));
if (num9 < -Settings.VelocityThreshold)
{
contactConstraintPoint.VelocityBias = -contactConstraint.Restitution * num9;
}
}
}
if (contactConstraint.PointCount == 2)
{
ContactConstraintPoint contactConstraintPoint2 = contactConstraint.Points[0];
ContactConstraintPoint contactConstraintPoint3 = contactConstraint.Points[1];
float invMass = body._invMass;
float invI = body._invI;
float invMass2 = body2._invMass;
float invI2 = body2._invI;
float num10 = Vec2.Cross(contactConstraintPoint2.R1, normal);
float num11 = Vec2.Cross(contactConstraintPoint2.R2, normal);
float num12 = Vec2.Cross(contactConstraintPoint3.R1, normal);
float num13 = Vec2.Cross(contactConstraintPoint3.R2, normal);
float num14 = invMass + invMass2 + invI * num10 * num10 + invI2 * num11 * num11;
float num15 = invMass + invMass2 + invI * num12 * num12 + invI2 * num13 * num13;
float num16 = invMass + invMass2 + invI * num10 * num12 + invI2 * num11 * num13;
if (num14 * num14 < 100f * (num14 * num15 - num16 * num16))
{
contactConstraint.K.Col1.Set(num14, num16);
contactConstraint.K.Col2.Set(num16, num15);
contactConstraint.NormalMass = contactConstraint.K.Invert();
}
else
{
contactConstraint.PointCount = 1;
}
}
num++;
}
}
Box2DXDebug.Assert(num == this._constraintCount);
}
internal Body(BodyDef bd, World world)
{
Box2DXDebug.Assert(world._lock == false);
_flags = 0;
if (bd.IsBullet)
{
_flags |= BodyFlags.Bullet;
}
if (bd.FixedRotation)
{
_flags |= BodyFlags.FixedRotation;
}
if (bd.AllowSleep)
{
_flags |= BodyFlags.AllowSleep;
}
if (bd.IsSleeping)
{
_flags |= BodyFlags.Sleep;
}
_world = world;
_xf.Position = bd.Position;
_xf.R.Set(bd.Angle);
_sweep.LocalCenter = bd.MassData.Center;
_sweep.T0 = 1.0f;
_sweep.A0 = _sweep.A = bd.Angle;
_sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter);
//_jointList = null;
//_contactList = null;
//_controllerList = null;
//_prev = null;
//_next = null;
_linearVelocity = bd.LinearVelocity;
_angularVelocity = bd.AngularVelocity;
_linearDamping = bd.LinearDamping;
_angularDamping = bd.AngularDamping;
//_force.Set(0.0f, 0.0f);
//_torque = 0.0f;
//_linearVelocity.SetZero();
//_angularVelocity = 0.0f;
//_sleepTime = 0.0f;
//_invMass = 0.0f;
//_I = 0.0f;
//_invI = 0.0f;
_mass = bd.MassData.Mass;
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
}
_I = bd.MassData.I;
if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
{
_invI = 1.0f / _I;
}
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
}
else
{
_type = BodyType.Dynamic;
}
_userData = bd.UserData;
//_fixtureList = null;
//_fixtureCount = 0;
}
public void Create(BroadPhase broadPhase, Body body, Transform xf, FixtureDef def)
{
UserData = def.UserData;
Friction = def.Friction;
Restitution = def.Restitution;
Density = def.Density;
_body = body;
_next = null;
Filter = def.Filter;
_isSensor = def.IsSensor;
_type = def.Type;
// Allocate and initialize the child shape.
switch (_type)
{
case ShapeType.CircleShape:
{
CircleShape circle = new CircleShape();
CircleDef circleDef = (CircleDef)def;
circle._position = circleDef.LocalPosition;
circle._radius = circleDef.Radius;
_shape = circle;
}
break;
case ShapeType.PolygonShape:
{
PolygonShape polygon = new PolygonShape();
PolygonDef polygonDef = (PolygonDef)def;
polygon.Set(polygonDef.Vertices, polygonDef.VertexCount);
_shape = polygon;
}
break;
case ShapeType.EdgeShape:
{
EdgeShape edge = new EdgeShape();
EdgeDef edgeDef = (EdgeDef)def;
edge.Set(edgeDef.Vertex1, edgeDef.Vertex2);
_shape = edge;
}
break;
default:
Box2DXDebug.Assert(false);
break;
}
// Create proxy in the broad-phase.
AABB aabb;
_shape.ComputeAABB(out aabb, xf);
bool inRange = broadPhase.InRange(aabb);
// You are creating a shape outside the world box.
Box2DXDebug.Assert(inRange);
if (inRange)
{
_proxyId = broadPhase.CreateProxy(aabb, this);
}
else
{
_proxyId = PairManager.NullProxy;
}
}
int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey)
{
float maxLambda = 1;
float dx = (segment.P2.X - segment.P1.X) * _quantizationFactor.X;
float dy = (segment.P2.Y - segment.P1.Y) * _quantizationFactor.Y;
int sx = dx < -Settings.FLT_EPSILON ? -1 : (dx > Settings.FLT_EPSILON ? 1 : 0);
int sy = dy < -Settings.FLT_EPSILON ? -1 : (dy > Settings.FLT_EPSILON ? 1 : 0);
Box2DXDebug.Assert(sx != 0 || sy != 0);
float p1x = (segment.P1.X - _worldAABB.LowerBound.X) * _quantizationFactor.X;
float p1y = (segment.P1.Y - _worldAABB.LowerBound.Y) * _quantizationFactor.Y;
#if ALLOWUNSAFE
ushort *startValues = stackalloc ushort[2];
ushort *startValues2 = stackalloc ushort[2];
#else
ushort[] startValues = new ushort[2];
ushort[] startValues2 = new ushort[2];
#endif
int xIndex;
int yIndex;
ushort proxyId;
Proxy proxy;
// TODO_ERIN implement fast float to ushort conversion.
startValues[0] = (ushort)((ushort)(p1x) & (BROADPHASE_MAX - 1));
startValues2[0] = (ushort)((ushort)(p1x) | 1);
startValues[1] = (ushort)((ushort)(p1y) & (BROADPHASE_MAX - 1));
startValues2[1] = (ushort)((ushort)(p1y) | 1);
//First deal with all the proxies that contain segment.p1
int lowerIndex;
int upperIndex;
Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], _bounds[0], 2 * _proxyCount, 0);
if (sx >= 0)
{
xIndex = upperIndex - 1;
}
else
{
xIndex = lowerIndex;
}
Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], _bounds[1], 2 * _proxyCount, 1);
if (sy >= 0)
{
yIndex = upperIndex - 1;
}
else
{
yIndex = lowerIndex;
}
//If we are using sortKey, then sort what we have so far, filtering negative keys
if (sortKey != null)
{
//Fill keys
for (int j = 0; j < _queryResultCount; j++)
{
_querySortKeys[j] = sortKey(_proxyPool[_queryResults[j]].UserData);
}
//Bubble sort keys
//Sorting negative values to the top, so we can easily remove them
int i = 0;
while (i < _queryResultCount - 1)
{
float a = _querySortKeys[i];
float b = _querySortKeys[i + 1];
if ((a < 0) ? (b >= 0) : (a > b && b >= 0))
{
_querySortKeys[i + 1] = a;
_querySortKeys[i] = b;
ushort tempValue = _queryResults[i + 1];
_queryResults[i + 1] = _queryResults[i];
_queryResults[i] = tempValue;
i--;
if (i == -1)
{
i = 1;
}
}
else
{
i++;
}
}
//Skim off negative values
while (_queryResultCount > 0 && _querySortKeys[_queryResultCount - 1] < 0)
{
_queryResultCount--;
}
}
//Now work through the rest of the segment
for (; ;)
{
float xProgress = 0;
float yProgress = 0;
if (xIndex < 0 || xIndex >= _proxyCount * 2)
{
break;
}
if (yIndex < 0 || yIndex >= _proxyCount * 2)
{
break;
}
if (sx != 0)
{
//Move on to the next bound
if (sx > 0)
{
xIndex++;
if (xIndex == _proxyCount * 2)
{
break;
}
}
else
{
xIndex--;
if (xIndex < 0)
{
break;
}
}
xProgress = (_bounds[0][xIndex].Value - p1x) / dx;
}
if (sy != 0)
{
//Move on to the next bound
if (sy > 0)
{
yIndex++;
if (yIndex == _proxyCount * 2)
{
break;
}
}
else
{
yIndex--;
if (yIndex < 0)
{
break;
}
}
yProgress = (_bounds[1][yIndex].Value - p1y) / dy;
}
for (; ;)
{
if (sy == 0 || (sx != 0 && xProgress < yProgress))
{
if (xProgress > maxLambda)
{
break;
}
//Check that we are entering a proxy, not leaving
if (sx > 0 ? _bounds[0][xIndex].IsLower : _bounds[0][xIndex].IsUpper)
{
//Check the other axis of the proxy
proxyId = _bounds[0][xIndex].ProxyId;
proxy = _proxyPool[proxyId];
if (sy >= 0)
{
if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
else
{
if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
}
//Early out
if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && xProgress > _querySortKeys[_queryResultCount - 1])
{
break;
}
//Move on to the next bound
if (sx > 0)
{
xIndex++;
if (xIndex == _proxyCount * 2)
{
break;
}
}
else
{
xIndex--;
if (xIndex < 0)
{
break;
}
}
xProgress = (_bounds[0][xIndex].Value - p1x) / dx;
}
else
{
if (yProgress > maxLambda)
{
break;
}
//Check that we are entering a proxy, not leaving
if (sy > 0 ? _bounds[1][yIndex].IsLower : _bounds[1][yIndex].IsUpper)
{
//Check the other axis of the proxy
proxyId = _bounds[1][yIndex].ProxyId;
proxy = _proxyPool[proxyId];
if (sx >= 0)
{
if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
else
{
if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
}
//Early out
if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && yProgress > _querySortKeys[_queryResultCount - 1])
{
break;
}
//Move on to the next bound
if (sy > 0)
{
yIndex++;
if (yIndex == _proxyCount * 2)
{
break;
}
}
else
{
yIndex--;
if (yIndex < 0)
{
break;
}
}
yProgress = (_bounds[1][yIndex].Value - p1y) / dy;
}
}
break;
}
int count = 0;
for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count)
{
Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies);
Proxy proxy_ = _proxyPool[_queryResults[i]];
Box2DXDebug.Assert(proxy_.IsValid);
userData[i] = proxy_.UserData;
}
// Prepare for next query.
_queryResultCount = 0;
IncrementTimeStamp();
return(count);
}
public void Dispose()
{
Box2DXDebug.Assert(_world._lock == false);
// shapes and joints are destroyed in World.Destroy
}
// Call MoveProxy as many times as you like, then when you are done
// call Commit to finalized the proxy pairs (for your time step).
public void MoveProxy(int proxyId, AABB aabb)
{
if (proxyId == PairManager.NullProxy || Settings.MaxProxies <= proxyId)
{
Box2DXDebug.Assert(false);
return;
}
if (aabb.IsValid == false)
{
Box2DXDebug.Assert(false);
return;
}
int boundCount = 2 * _proxyCount;
Proxy proxy = _proxyPool[proxyId];
// Get new bound values
BoundValues newValues = new BoundValues();;
ComputeBounds(out newValues.LowerValues, out newValues.UpperValues, aabb);
// Get old bound values
BoundValues oldValues = new BoundValues();
for (int axis = 0; axis < 2; ++axis)
{
oldValues.LowerValues[axis] = _bounds[axis][proxy.LowerBounds[axis]].Value;
oldValues.UpperValues[axis] = _bounds[axis][proxy.UpperBounds[axis]].Value;
}
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
int lowerIndex = proxy.LowerBounds[axis];
int upperIndex = proxy.UpperBounds[axis];
ushort lowerValue = newValues.LowerValues[axis];
ushort upperValue = newValues.UpperValues[axis];
int deltaLower = lowerValue - bounds[lowerIndex].Value;
int deltaUpper = upperValue - bounds[upperIndex].Value;
bounds[lowerIndex].Value = lowerValue;
bounds[upperIndex].Value = upperValue;
//
// Expanding adds overlaps
//
// Should we move the lower bound down?
if (deltaLower < 0)
{
int index = lowerIndex;
while (index > 0 && lowerValue < bounds[index - 1].Value)
{
Bound bound = bounds[index];
Bound prevBound = bounds[index - 1];
int prevProxyId = prevBound.ProxyId;
Proxy prevProxy = _proxyPool[prevBound.ProxyId];
++prevBound.StabbingCount;
if (prevBound.IsUpper == true)
{
if (TestOverlap(newValues, prevProxy))
{
_pairManager.AddBufferedPair(proxyId, prevProxyId);
}
++prevProxy.UpperBounds[axis];
++bound.StabbingCount;
}
else
{
++prevProxy.LowerBounds[axis];
--bound.StabbingCount;
}
--proxy.LowerBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index - 1]);
--index;
}
}
// Should we move the upper bound up?
if (deltaUpper > 0)
{
int index = upperIndex;
while (index < boundCount - 1 && bounds[index + 1].Value <= upperValue)
{
Bound bound = bounds[index];
Bound nextBound = bounds[index + 1];
int nextProxyId = nextBound.ProxyId;
Proxy nextProxy = _proxyPool[nextProxyId];
++nextBound.StabbingCount;
if (nextBound.IsLower == true)
{
if (TestOverlap(newValues, nextProxy))
{
_pairManager.AddBufferedPair(proxyId, nextProxyId);
}
--nextProxy.LowerBounds[axis];
++bound.StabbingCount;
}
else
{
--nextProxy.UpperBounds[axis];
--bound.StabbingCount;
}
++proxy.UpperBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index + 1]);
++index;
}
}
//
// Shrinking removes overlaps
//
// Should we move the lower bound up?
if (deltaLower > 0)
{
int index = lowerIndex;
while (index < boundCount - 1 && bounds[index + 1].Value <= lowerValue)
{
Bound bound = bounds[index];
Bound nextBound = bounds[index + 1];
int nextProxyId = nextBound.ProxyId;
Proxy nextProxy = _proxyPool[nextProxyId];
--nextBound.StabbingCount;
if (nextBound.IsUpper)
{
if (TestOverlap(oldValues, nextProxy))
{
_pairManager.RemoveBufferedPair(proxyId, nextProxyId);
}
--nextProxy.UpperBounds[axis];
--bound.StabbingCount;
}
else
{
--nextProxy.LowerBounds[axis];
++bound.StabbingCount;
}
++proxy.LowerBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index + 1]);
++index;
}
}
// Should we move the upper bound down?
if (deltaUpper < 0)
{
int index = upperIndex;
while (index > 0 && upperValue < bounds[index - 1].Value)
{
Bound bound = bounds[index];
Bound prevBound = bounds[index - 1];
int prevProxyId = prevBound.ProxyId;
Proxy prevProxy = _proxyPool[prevProxyId];
--prevBound.StabbingCount;
if (prevBound.IsLower == true)
{
if (TestOverlap(oldValues, prevProxy))
{
_pairManager.RemoveBufferedPair(proxyId, prevProxyId);
}
++prevProxy.LowerBounds[axis];
--bound.StabbingCount;
}
else
{
++prevProxy.UpperBounds[axis];
++bound.StabbingCount;
}
--proxy.UpperBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index - 1]);
--index;
}
}
}
if (IsValidate)
{
Validate();
}
}
public override void RayCast(out RayCastOutput output, ref RayCastInput input, Transform xf)
{
output = new RayCastOutput();
float lower = 0.0f, upper = input.MaxFraction;
// Put the ray into the polygon's frame of reference.
Vec2 p1 = Math.MulT(xf.R, input.P1 - xf.Position);
Vec2 p2 = Math.MulT(xf.R, input.P2 - xf.Position);
Vec2 d = p2 - p1;
int index = -1;
output.Hit = false;
for (int i = 0; i < VertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vec2.Dot(Normals[i], Vertices[i] - p1);
float denominator = Vec2.Dot(Normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return;
}
}
Box2DXDebug.Assert(0.0f <= lower && lower <= input.MaxFraction);
if (index >= 0)
{
output.Hit = true;
output.Fraction = lower;
output.Normal = Math.Mul(xf.R, Normals[index]);
return;
}
}
public static void CollidePolygonAndCircle(ref Manifold manifold,
PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vec2 c = Common.Math.Mul(xf2, circle.GetLocalPosition());
Vec2 cLocal = Common.Math.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = circle.GetRadius();
int vertexCount = polygon.VertexCount;
Vec2[] vertices = polygon.GetVertices();
Vec2[] normals = polygon.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vec2.Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// If the center is inside the polygon ...
if (separation < Common.Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Normal = Common.Math.Mul(xf1.R, normals[normalIndex]);
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
Vec2 position = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position);
manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position);
manifold.Points[0].Separation = separation - radius;
return;
}
// Project the circle center onto the edge segment.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vec2 e = vertices[vertIndex2] - vertices[vertIndex1];
float length = e.Normalize();
Box2DXDebug.Assert(length > Settings.FLT_EPSILON);
// Project the center onto the edge.
float u = Vec2.Dot(cLocal - vertices[vertIndex1], e);
Vec2 p;
if (u <= 0.0f)
{
p = vertices[vertIndex1];
manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1;
}
else if (u >= length)
{
p = vertices[vertIndex2];
manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex2;
}
else
{
p = vertices[vertIndex1] + u * e;
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature;
}
Vec2 d = cLocal - p;
float dist = d.Normalize();
if (dist > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Normal = Common.Math.Mul(xf1.R, d);
Vec2 position_ = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position_);
manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position_);
manifold.Points[0].Separation = dist - radius;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
}
public override Vector2 GetVertex(int index)
{
Box2DXDebug.Assert(0 <= index && index < _vertexCount);
return(_vertices[index]);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1;
Vec2 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.SetZero();
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2.SetZero();
}
float C = _constant - length1 - _ratio * length2;
if (C > 0.0f)
{
_state = LimitState.InactiveLimit;
_force = 0.0f;
}
else
{
_state = LimitState.AtUpperLimit;
_positionImpulse = 0.0f;
}
if (length1 < _maxLength1)
{
_limitState1 = LimitState.InactiveLimit;
_limitForce1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpperLimit;
_limitPositionImpulse1 = 0.0f;
}
if (length2 < _maxLength2)
{
_limitState2 = LimitState.InactiveLimit;
_limitForce2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpperLimit;
_limitPositionImpulse2 = 0.0f;
}
// Compute effective mass.
float cr1u1 = Vec2.Cross(r1, _u1);
float cr2u2 = Vec2.Cross(r2, _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)
{
// Warm starting.
Vec2 P1 = Settings.FORCE_SCALE(step.Dt) * (-_force - _limitForce1) * _u1;
Vec2 P2 = Settings.FORCE_SCALE(step.Dt) * (-_ratio * _force - _limitForce2) * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
}
else
{
_force = 0.0f;
_limitForce1 = 0.0f;
_limitForce2 = 0.0f;
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body = this._body1;
Body body2 = this._body2;
float num = 0f;
if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
{
float num2 = body2._sweep.A - body._sweep.A - this._referenceAngle;
float num3 = 0f;
if (this._limitState == LimitState.EqualLimits)
{
float num4 = Box2DX.Common.Math.Clamp(num2, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
num3 = -this._motorMass * num4;
num = Box2DX.Common.Math.Abs(num4);
}
else
{
if (this._limitState == LimitState.AtLowerLimit)
{
float num4 = num2 - this._lowerAngle;
num = -num4;
num4 = Box2DX.Common.Math.Clamp(num4 + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0f);
num3 = -this._motorMass * num4;
}
else
{
if (this._limitState == LimitState.AtUpperLimit)
{
float num4 = num2 - this._upperAngle;
num = num4;
num4 = Box2DX.Common.Math.Clamp(num4 - Settings.AngularSlop, 0f, Settings.MaxAngularCorrection);
num3 = -this._motorMass * num4;
}
}
}
Body expr_139_cp_0 = body;
expr_139_cp_0._sweep.A = expr_139_cp_0._sweep.A - body._invI * num3;
Body expr_154_cp_0 = body2;
expr_154_cp_0._sweep.A = expr_154_cp_0._sweep.A + body2._invI * num3;
body.SynchronizeTransform();
body2.SynchronizeTransform();
}
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 vec3 = body2._sweep.C + vec2 - body._sweep.C - vec;
float num5 = vec3.Length();
float invMass = body._invMass;
float invMass2 = body2._invMass;
float invI = body._invI;
float invI2 = body2._invI;
float num6 = 10f * Settings.LinearSlop;
if (vec3.LengthSquared() > num6 * num6)
{
Vec2 vec4 = vec3;
vec4.Normalize();
float num7 = invMass + invMass2;
Box2DXDebug.Assert(num7 > Settings.FLT_EPSILON);
float a = 1f / num7;
Vec2 v = a * -vec3;
float num8 = 0.5f;
Body expr_283_cp_0 = body;
expr_283_cp_0._sweep.C = expr_283_cp_0._sweep.C - num8 * invMass * v;
Body expr_2A5_cp_0 = body2;
expr_2A5_cp_0._sweep.C = expr_2A5_cp_0._sweep.C + num8 * invMass2 * v;
vec3 = body2._sweep.C + vec2 - body._sweep.C - vec;
}
Mat22 a2 = default(Mat22);
a2.Col1.X = invMass + invMass2;
a2.Col2.X = 0f;
a2.Col1.Y = 0f;
a2.Col2.Y = invMass + invMass2;
Mat22 b = default(Mat22);
b.Col1.X = invI * vec.Y * vec.Y;
b.Col2.X = -invI * vec.X * vec.Y;
b.Col1.Y = -invI * vec.X * vec.Y;
b.Col2.Y = invI * vec.X * vec.X;
Mat22 b2 = default(Mat22);
b2.Col1.X = invI2 * vec2.Y * vec2.Y;
b2.Col2.X = -invI2 * vec2.X * vec2.Y;
b2.Col1.Y = -invI2 * vec2.X * vec2.Y;
b2.Col2.Y = invI2 * vec2.X * vec2.X;
Vec2 vec5 = (a2 + b + b2).Solve(-vec3);
Body expr_465_cp_0 = body;
expr_465_cp_0._sweep.C = expr_465_cp_0._sweep.C - body._invMass * vec5;
Body expr_488_cp_0 = body;
expr_488_cp_0._sweep.A = expr_488_cp_0._sweep.A - body._invI * Vec2.Cross(vec, vec5);
Body expr_4AA_cp_0 = body2;
expr_4AA_cp_0._sweep.C = expr_4AA_cp_0._sweep.C + body2._invMass * vec5;
Body expr_4CD_cp_0 = body2;
expr_4CD_cp_0._sweep.A = expr_4CD_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec5);
body.SynchronizeTransform();
body2.SynchronizeTransform();
return(num5 <= Settings.LinearSlop && num <= Settings.AngularSlop);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
XForm xf1 = b1.GetXForm();
XForm xf2 = b2.GetXForm();
// Compute the effective masses.
Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2);
Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
_invMass1 = b1._invMass;
_invI1 = b1._invI;
_invMass2 = b2._invMass;
_invI2 = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1);
_a1 = Vec2.Cross(d + r1, _axis);
_a2 = Vec2.Cross(r2, _axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic constraint.
{
_perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1);
_s1 = Vec2.Cross(d + r1, _perp);
_s2 = Vec2.Cross(r2, _perp);
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1.Set(k11, k12);
_K.Col2.Set(k12, k22);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vec2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitState = LimitState.AtLowerLimit;
_impulse.Y = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitState = LimitState.AtUpperLimit;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.WarmStarting)
{
// Account for variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;
b1._linearVelocity -= _invMass1 * P;
b1._angularVelocity -= _invI1 * L1;
b2._linearVelocity += _invMass2 * P;
b2._angularVelocity += _invI2 * L2;
}
else
{
_impulse.SetZero();
_motorImpulse = 0.0f;
}
}
public virtual void Dispose()
{
Box2DXDebug.Assert(_proxyId == PairManager.NullProxy);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
}
// Compute the effective mass matrix.
Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
_mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
_mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
_mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
_mass.Col3.Y = r1.X * i1 + r2.X * i2;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = i1 + i2;
_motorMass = 1.0f / (i1 + i2);
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLowerLimit)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpperLimit)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (step.WarmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vec2 P = new Vec2(_impulse.X, _impulse.Y);
b1._linearVelocity -= m1 * P;
b1._angularVelocity -= i1 * (Vec2.Cross(r1, P) + _motorImpulse + _impulse.Z);
b2._linearVelocity += m2 * P;
b2._angularVelocity += i2 * (Vec2.Cross(r2, P) + _motorImpulse + _impulse.Z);
}
else
{
_impulse.SetZero();
_motorImpulse = 0.0f;
}
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < this._constraintCount; i++)
{
ContactConstraint contactConstraint = this._constraints[i];
Body body = contactConstraint.Body1;
Body body2 = contactConstraint.Body2;
float num = body._angularVelocity;
float num2 = body2._angularVelocity;
Vec2 vec = body._linearVelocity;
Vec2 vec2 = body2._linearVelocity;
float invMass = body._invMass;
float invI = body._invI;
float invMass2 = body2._invMass;
float invI2 = body2._invI;
Vec2 normal = contactConstraint.Normal;
Vec2 vec3 = Vec2.Cross(normal, 1f);
float friction = contactConstraint.Friction;
Box2DXDebug.Assert(contactConstraint.PointCount == 1 || contactConstraint.PointCount == 2);
if (contactConstraint.PointCount == 1)
{
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[0];
Vec2 a = vec2 + Vec2.Cross(num2, contactConstraintPoint.R2) - vec - Vec2.Cross(num, contactConstraintPoint.R1);
float num3 = Vec2.Dot(a, normal);
float num4 = -contactConstraintPoint.NormalMass * (num3 - contactConstraintPoint.VelocityBias);
float num5 = Box2DX.Common.Math.Max(contactConstraintPoint.NormalImpulse + num4, 0f);
num4 = num5 - contactConstraintPoint.NormalImpulse;
Vec2 vec4 = num4 * normal;
vec -= invMass * vec4;
num -= invI * Vec2.Cross(contactConstraintPoint.R1, vec4);
vec2 += invMass2 * vec4;
num2 += invI2 * Vec2.Cross(contactConstraintPoint.R2, vec4);
contactConstraintPoint.NormalImpulse = num5;
}
else
{
ContactConstraintPoint contactConstraintPoint2 = contactConstraint.Points[0];
ContactConstraintPoint contactConstraintPoint3 = contactConstraint.Points[1];
Vec2 vec5 = new Vec2(contactConstraintPoint2.NormalImpulse, contactConstraintPoint3.NormalImpulse);
Box2DXDebug.Assert(vec5.X >= 0f && vec5.Y >= 0f);
Vec2 a2 = vec2 + Vec2.Cross(num2, contactConstraintPoint2.R2) - vec - Vec2.Cross(num, contactConstraintPoint2.R1);
Vec2 a3 = vec2 + Vec2.Cross(num2, contactConstraintPoint3.R2) - vec - Vec2.Cross(num, contactConstraintPoint3.R1);
float num6 = Vec2.Dot(a2, normal);
float num7 = Vec2.Dot(a3, normal);
Vec2 vec6;
vec6.X = num6 - contactConstraintPoint2.VelocityBias;
vec6.Y = num7 - contactConstraintPoint3.VelocityBias;
vec6 -= Box2DX.Common.Math.Mul(contactConstraint.K, vec5);
Vec2 v = -Box2DX.Common.Math.Mul(contactConstraint.NormalMass, vec6);
if (v.X >= 0f && v.Y >= 0f)
{
Vec2 vec7 = v - vec5;
Vec2 vec8 = vec7.X * normal;
Vec2 vec9 = vec7.Y * normal;
vec -= invMass * (vec8 + vec9);
num -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
vec2 += invMass2 * (vec8 + vec9);
num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
contactConstraintPoint2.NormalImpulse = v.X;
contactConstraintPoint3.NormalImpulse = v.Y;
}
else
{
v.X = -contactConstraintPoint2.NormalMass * vec6.X;
v.Y = 0f;
num7 = contactConstraint.K.Col1.Y * v.X + vec6.Y;
if (v.X >= 0f && num7 >= 0f)
{
Vec2 vec7 = v - vec5;
Vec2 vec8 = vec7.X * normal;
Vec2 vec9 = vec7.Y * normal;
vec -= invMass * (vec8 + vec9);
num -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
vec2 += invMass2 * (vec8 + vec9);
num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
contactConstraintPoint2.NormalImpulse = v.X;
contactConstraintPoint3.NormalImpulse = v.Y;
}
else
{
v.X = 0f;
v.Y = -contactConstraintPoint3.NormalMass * vec6.Y;
num6 = contactConstraint.K.Col2.X * v.Y + vec6.X;
if (v.Y >= 0f && num6 >= 0f)
{
Vec2 vec7 = v - vec5;
Vec2 vec8 = vec7.X * normal;
Vec2 vec9 = vec7.Y * normal;
vec -= invMass * (vec8 + vec9);
num -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
vec2 += invMass2 * (vec8 + vec9);
num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
contactConstraintPoint2.NormalImpulse = v.X;
contactConstraintPoint3.NormalImpulse = v.Y;
}
else
{
v.X = 0f;
v.Y = 0f;
num6 = vec6.X;
num7 = vec6.Y;
if (num6 >= 0f && num7 >= 0f)
{
Vec2 vec7 = v - vec5;
Vec2 vec8 = vec7.X * normal;
Vec2 vec9 = vec7.Y * normal;
vec -= invMass * (vec8 + vec9);
num -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
vec2 += invMass2 * (vec8 + vec9);
num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
contactConstraintPoint2.NormalImpulse = v.X;
contactConstraintPoint3.NormalImpulse = v.Y;
}
}
}
}
}
for (int j = 0; j < contactConstraint.PointCount; j++)
{
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
Vec2 a = vec2 + Vec2.Cross(num2, contactConstraintPoint.R2) - vec - Vec2.Cross(num, contactConstraintPoint.R1);
float num8 = Vec2.Dot(a, vec3);
float num4 = contactConstraintPoint.TangentMass * -num8;
float num9 = friction * contactConstraintPoint.NormalImpulse;
float num5 = Box2DX.Common.Math.Clamp(contactConstraintPoint.TangentImpulse + num4, -num9, num9);
num4 = num5 - contactConstraintPoint.TangentImpulse;
Vec2 vec4 = num4 * vec3;
vec -= invMass * vec4;
num -= invI * Vec2.Cross(contactConstraintPoint.R1, vec4);
vec2 += invMass2 * vec4;
num2 += invI2 * Vec2.Cross(contactConstraintPoint.R2, vec4);
contactConstraintPoint.TangentImpulse = num5;
}
body._linearVelocity = vec;
body._angularVelocity = num;
body2._linearVelocity = vec2;
body2._angularVelocity = num2;
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit.
Body b1 = _bodyA;
Body b2 = _bodyB;
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float C = Box2DXMath.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Box2DXMath.Abs(C);
}
else if (_limitState == LimitState.AtLowerLimit)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = Box2DXMath.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = Box2DXMath.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1._sweep.A -= b1._invI * limitImpulse;
b2._sweep.A += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 C = b2._sweep.C + r2 - b1._sweep.C - r1;
positionError = C.Length();
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Handle large detachment.
float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vec2 u = C; u.Normalize();
float k = invMass1 + invMass2;
Box2DXDebug.Assert(k > Settings.FLT_EPSILON);
float m = 1.0f / k;
Vec2 impulse = m * (-C);
float k_beta = 0.5f;
b1._sweep.C -= k_beta * invMass1 * impulse;
b2._sweep.C += k_beta * invMass2 * impulse;
C = b2._sweep.C + r2 - b1._sweep.C - r1;
}
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
Vec2 impulse_ = K.Solve(-C);
b1._sweep.C -= b1._invMass * impulse_;
b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse_);
b2._sweep.C += b2._invMass * impulse_;
b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse_);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/// <summary>
/// Get a vertex by index. Used by Distance.
/// </summary>
public override Vector2 GetVertex(int index)
{
Box2DXDebug.Assert(index == 0);
return(_position);
}
public PolyAndCircleContact(Fixture fixtureA, Fixture fixtureB)
: base(fixtureA, fixtureB)
{
Box2DXDebug.Assert(_fixtureA.GetType() == ShapeType.PolygonShape);
Box2DXDebug.Assert(_fixtureB.GetType() == ShapeType.CircleShape);
}
// TODO_ERIN adjust linear velocity and torque to account for movement of center.
/// <summary>
/// Compute the mass properties from the attached shapes. You typically call this
/// after adding all the shapes. If you add or remove shapes later, you may want
/// to call this again. Note that this changes the center of mass position.
/// </summary>
public void SetMassFromShapes()
{
lock (_world._lock) {
// Compute mass data from shapes. Each shape has its own density.
_mass = 0.0f;
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
Vec2 center = Vec2.Zero;
for (Shape s = _shapeList; s != null; s = s._next)
{
MassData massData;
s.ComputeMass(out massData);
_mass += massData.Mass;
center += massData.Mass * massData.Center;
_I += massData.I;
}
// Compute center of mass, and shift the origin to the COM.
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
center *= _invMass;
}
if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
{
// Center the inertia about the center of mass.
_I -= _mass * Vec2.Dot(center, center);
Box2DXDebug.Assert(_I > 0.0f);
_invI = 1.0f / _I;
}
else
{
_I = 0.0f;
_invI = 0.0f;
}
// Move center of mass.
_sweep.LocalCenter = center;
_sweep.C0 = _sweep.C = Common.Math.Mul(_xf, _sweep.LocalCenter);
// Update the sweep radii of all child shapes.
for (Shape s = _shapeList; s != null; s = s._next)
{
s.UpdateSweepRadius(_sweep.LocalCenter);
}
BodyType oldType = _type;
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
}
else
{
_type = BodyType.Dynamic;
}
// If the body type changed, we need to refilter the broad-phase proxies.
if (oldType != _type)
{
for (Shape s = _shapeList; s != null; s = s._next)
{
s.RefilterProxy(_world._broadPhase, _xf);
}
}
}
}
public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vector2.zero;
float lower = 0.0f, upper = maxLambda;
Vector2 p1 = xf.InverseTransformDirection(segment.P1 - xf.position);
Vector2 p2 = xf.InverseTransformDirection(segment.P2 - xf.position);
Vector2 d = p2 - p1;
int index = -1;
for (int i = 0; i < _vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vector2.Dot(_normals[i], _vertices[i] - p1);
float denominator = Vector2.Dot(_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return(SegmentCollide.MissCollide);
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return(SegmentCollide.MissCollide);
}
}
Box2DXDebug.Assert(0.0f <= lower && lower <= maxLambda);
if (index >= 0)
{
lambda = lower;
normal = xf.TransformDirection(_normals[index]);
return(SegmentCollide.HitCollide);
}
lambda = 0f;
return(SegmentCollide.StartInsideCollide);
}
static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
{
output = new DistanceOutput();
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
#if ALLOWUNSAFE
fixed(SimplexCache *sPtr = &cache)
{
simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
}
#else
simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif
// Get simplex vertices as an array.
#if ALLOWUNSAFE
SimplexVertex *vertices = &simplex._v1;
#else
SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
int *lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
int[] lastA = new int[4];
int[] lastB = new int[4];
#endif // ALLOWUNSAFE
int lastCount;
// Main iteration loop.
int iter = 0;
const int k_maxIterationCount = 20;
while (iter < k_maxIterationCount)
{
// Copy simplex so we can identify duplicates.
lastCount = simplex._count;
int i;
for (i = 0; i < lastCount; ++i)
{
lastA[i] = vertices[i].indexA;
lastB[i] = vertices[i].indexB;
}
switch (simplex._count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
#if DEBUG
Box2DXDebug.Assert(false);
#endif
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex._count == 3)
{
break;
}
// Compute closest point.
Vector2 p = simplex.GetClosestPoint();
float distanceSqr = p.LengthSquared;
// Ensure the search direction is numerically fit.
if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
#if ALLOWUNSAFE
SimplexVertex *vertex = vertices + simplex._count;
vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
//Vec2 wBLocal;
vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
#else
SimplexVertex vertex = vertices[simplex._count - 1];
vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
//Vec2 wBLocal;
vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
#endif // ALLOWUNSAFE
// Iteration count is equated to the number of support point calls.
++iter;
// Check for convergence.
#if ALLOWUNSAFE
float lowerBound = Vector2.Dot(p, vertex->w);
#else
float lowerBound = Vector2.Dot(p, vertex.w);
#endif
float upperBound = distanceSqr;
const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100
if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
{
// Converged!
break;
}
// Check for duplicate support points.
bool duplicate = false;
for (i = 0; i < lastCount; ++i)
{
#if ALLOWUNSAFE
if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex._count;
}
#if ALLOWUNSAFE
fixed(DistanceOutput *doPtr = &output)
{
// Prepare output.
simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB);
doPtr->Iterations = iter;
}
fixed(SimplexCache *sPtr = &cache)
{
// Cache the simplex.
simplex.WriteCache(sPtr);
}
#else
// Prepare output.
simplex.GetWitnessPoints(ref output.PointA, ref output.PointB);
output.Distance = Box2DX.Common.Math.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
#endif
// Apply radii if requested.
if (input.UseRadii)
{
float rA = shapeA._radius;
float rB = shapeB._radius;
if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 normal = output.PointB - output.PointA;
normal.Normalize();
output.PointA += rA * normal;
output.PointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
public override void ComputeMass(out MassData massData, float denstity)
{
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the Transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
Box2DXDebug.Assert(_vertexCount >= 3);
Vector2 center = Vector2.zero;
float area = 0.0f;
float I = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
Vector2 pRef = Vector2.zero;
#if O
// This code would put the reference point inside the polygon.
for (int i = 0; i < vCount; ++i)
{
pRef += _vertices[i];
}
pRef *= 1.0f / count;
#endif
const float k_inv3 = 1.0f / 3.0f;
for (int i = 0; i < _vertexCount; ++i)
{
// Triangle vertices.
Vector2 p1 = pRef;
Vector2 p2 = _vertices[i];
Vector2 p3 = i + 1 < _vertexCount ? _vertices[i + 1] : _vertices[0];
Vector2 e1 = p2 - p1;
Vector2 e2 = p3 - p1;
float D = e1.Cross(e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (p1 + p2 + p3);
float px = p1.x, py = p1.y;
float ex1 = e1.x, ey1 = e1.y;
float ex2 = e2.x, ey2 = e2.y;
float intx2 = k_inv3 * (0.25f * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) + (px * ex1 + px * ex2)) + 0.5f * px * px;
float inty2 = k_inv3 * (0.25f * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) + (py * ey1 + py * ey2)) + 0.5f * py * py;
I += D * (intx2 + inty2);
}
// Total mass
massData.Mass = denstity * area;
// Center of mass
Box2DXDebug.Assert(area > Common.Settings.FLT_EPSILON);
center *= 1.0f / area;
massData.Center = center;
// Inertia tensor relative to the local origin.
massData.I = denstity * I;
}
public void DestroyProxy(int proxyId)
{
Box2DXDebug.Assert(0 < _proxyCount && _proxyCount <= Settings.MaxProxies);
Proxy proxy = _proxyPool[proxyId];
Box2DXDebug.Assert(proxy.IsValid);
int boundCount = 2 * _proxyCount;
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
int lowerIndex = proxy.LowerBounds[axis];
int upperIndex = proxy.UpperBounds[axis];
ushort lowerValue = bounds[lowerIndex].Value;
ushort upperValue = bounds[upperIndex].Value;
#warning "Check this"
//memmove(bounds + lowerIndex, bounds + lowerIndex + 1, (upperIndex - lowerIndex - 1) * sizeof(b2Bound));
Bound[] tmp = new Bound[upperIndex - lowerIndex - 1];
for (int i = 0; i < (upperIndex - lowerIndex - 1); i++)
{
tmp[i] = bounds[lowerIndex + 1 + i].Clone();
}
for (int i = 0; i < (upperIndex - lowerIndex - 1); i++)
{
bounds[lowerIndex + i] = tmp[i];
}
//memmove(bounds + upperIndex - 1, bounds + upperIndex + 1, (boundCount - upperIndex - 1) * sizeof(b2Bound));
tmp = new Bound[boundCount - upperIndex - 1];
for (int i = 0; i < (boundCount - upperIndex - 1); i++)
{
tmp[i] = bounds[upperIndex + 1 + i].Clone();
}
for (int i = 0; i < (boundCount - upperIndex - 1); i++)
{
bounds[upperIndex - 1 + i] = tmp[i];
}
// Fix bound indices.
for (int index = lowerIndex; index < boundCount - 2; ++index)
{
Proxy proxy_ = _proxyPool[bounds[index].ProxyId];
if (bounds[index].IsLower)
{
proxy_.LowerBounds[axis] = (ushort)index;
}
else
{
proxy_.UpperBounds[axis] = (ushort)index;
}
}
// Fix stabbing count.
for (int index = lowerIndex; index < upperIndex - 1; ++index)
{
--bounds[index].StabbingCount;
}
// Query for pairs to be removed. lowerIndex and upperIndex are not needed.
Query(out lowerIndex, out upperIndex, lowerValue, upperValue, bounds, boundCount - 2, axis);
}
Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies);
for (int i = 0; i < _queryResultCount; ++i)
{
Box2DXDebug.Assert(_proxyPool[_queryResults[i]].IsValid);
_pairManager.RemoveBufferedPair(proxyId, _queryResults[i]);
}
_pairManager.Commit();
// Prepare for next query.
_queryResultCount = 0;
IncrementTimeStamp();
// Return the proxy to the pool.
proxy.UserData = null;
proxy.OverlapCount = BroadPhase.Invalid;
proxy.LowerBounds[0] = BroadPhase.Invalid;
proxy.LowerBounds[1] = BroadPhase.Invalid;
proxy.UpperBounds[0] = BroadPhase.Invalid;
proxy.UpperBounds[1] = BroadPhase.Invalid;
proxy.Next = _freeProxy;
_freeProxy = (ushort)proxyId;
--_proxyCount;
if (IsValidate)
{
Validate();
}
}
// CCD via the secant method.
/// <summary>
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
/// smaller than sum of the shape radii.
/// Warning the sweeps must have the same time interval.
/// </summary>
/// <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(TOIInput input, Shape shapeA, Shape shapeB)
{
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
Box2DXDebug.Assert(sweepA.T0 == sweepB.T0);
Box2DXDebug.Assert(1.0f - sweepA.T0 > Common.Settings.FLT_EPSILON);
float radius = shapeA._radius + shapeB._radius;
float tolerance = input.Tolerance;
float alpha = 0.0f;
const int k_maxIterations = 1000; // TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceInput distanceInput;
distanceInput.UseRadii = false;
for (; ;)
{
XForm xfA, xfB;
sweepA.GetTransform(out xfA, alpha);
sweepB.GetTransform(out xfB, alpha);
// Get the distance between shapes.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
Distance(out distanceOutput, ref cache, ref distanceInput, shapeA, shapeB);
if (distanceOutput.Distance <= 0.0f)
{
alpha = 1.0f;
break;
}
SeparationFunction fcn = new SeparationFunction();
unsafe
{
fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB);
}
float separation = fcn.Evaluate(xfA, xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
if (separation > radius)
{
target = Common.Math.Max(radius - tolerance, 0.75f * radius);
}
else
{
target = Common.Math.Max(separation - tolerance, 0.02f * radius);
}
}
if (separation - target < 0.5f * tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
#if _FALSE
// Dump the curve seen by the root finder
{
const int32 N = 100;
float32 dx = 1.0f / N;
float32 xs[N + 1];
