// 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()
{
Box2DXDebug.Assert(_world._lock == false);
if (_world._lock == true)
{
return;
}
// 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 (Fixture f = _fixtureList; f != null; f = f.Next)
{
MassData massData;
f.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);
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 (Fixture f = _fixtureList; f != null; f = f.Next)
{
f.RefilterProxy(_world._broadPhase, _xf);
}
}
}
public override SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vec2.Zero;
float lower = 0.0f, upper = maxLambda;
Vec2 p1 = Common.Math.MulT(xf.R, segment.P1 - xf.Position);
Vec2 p2 = Common.Math.MulT(xf.R, segment.P2 - xf.Position);
Vec2 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 = Vec2.Dot(_normals[i], _vertices[i] - p1);
float denominator = Vec2.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 = Common.Math.Mul(xf.R, _normals[index]);
return(SegmentCollide.HitCollide);
}
lambda = 0f;
return(SegmentCollide.StartInsideCollide);
}
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.MathB2.Mul(xf2, circle._position);
Vec2 cLocal = Common.MathB2.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = polygon._radius + circle._radius;
int vertexCount = polygon._vertexCount;
Vec2[] vertices = polygon._vertices;
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;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vec2 v1 = vertices[vertIndex1];
Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Common.Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[normalIndex];
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
return;
}
// Compute barycentric coordinates
float u1 = Vec2.Dot(cLocal - v1, v2 - v1);
float u2 = Vec2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (Vec2.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v1;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v1;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
else if (u2 <= 0.0f)
{
if (Vec2.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v2;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v2;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
else
{
Vec2 faceCenter = 0.5f * (v1 + v2);
float separation_ = Vec2.Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation_ > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[vertIndex1];
manifold.LocalPoint = faceCenter;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vec2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorForce;
_motorImpulse = Box2DNet.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vec2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
float Cdot1 = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vec2 Cdot = new Vec2(Cdot1, Cdot2);
Vec2 f1 = _impulse;
Vec2 df = _K.Solve(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLowerLimit)
{
_impulse.Y = Box2DNet.Common.Math.Max(_impulse.Y, 0.0f);
}
else if (_limitState == LimitState.AtUpperLimit)
{
_impulse.Y = Box2DNet.Common.Math.Min(_impulse.Y, 0.0f);
}
// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.Col2.X;
float f2r = b / _K.Col1.X + f1.X;
_impulse.X = f2r;
df = _impulse - f1;
Vec2 P = df.X * _perp + df.Y * _axis;
float L1 = df.X * _s1 + df.Y * _a1;
float L2 = df.X * _s2 + df.Y * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
float df = (-Cdot1) / _K.Col1.X;
_impulse.X += df;
Vec2 P = df * _perp;
float L1 = df * _s1;
float L2 = df * _s2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
public override int GetSupport(Vec2 d)
{
return(Vec2.Dot(_v1, d) > Vec2.Dot(_v2, d) ? 0 : 1);
}
internal override bool SolvePositionConstraints()
{
Body b1 = _body1;
Body b2 = _body2;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
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 d = p2 - p1;
Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
// Solve linear (point-to-line) constraint.
float linearC = Vec2.Dot(ay1, d);
// Prevent overly large corrections.
linearC = Box2DXMath.Clamp(linearC, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float linearImpulse = -_linearMass * linearC;
b1._sweep.C += (invMass1 * linearImpulse) * _linearJacobian.Linear1;
b1._sweep.A += invI1 * linearImpulse * _linearJacobian.Angular1;
//b1->SynchronizeTransform(); // updated by angular constraint
b2._sweep.C += (invMass2 * linearImpulse) * _linearJacobian.Linear2;
b2._sweep.A += invI2 * linearImpulse * _linearJacobian.Angular2;
//b2->SynchronizeTransform(); // updated by angular constraint
float positionError = Box2DXMath.Abs(linearC);
// Solve angular constraint.
float angularC = b2._sweep.A - b1._sweep.A - _refAngle;
// Prevent overly large corrections.
angularC = Box2DXMath.Clamp(angularC, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
float angularImpulse = -_angularMass * angularC;
b1._sweep.A -= b1._invI * angularImpulse;
b2._sweep.A += b2._invI * angularImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
float angularError = Box2DXMath.Abs(angularC);
// Solve linear limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
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 d_ = p2_ - p1_;
Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
float translation = Vec2.Dot(ax1, d_);
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float limitC = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
limitImpulse = -_motorMass * limitC;
positionError = Box2DXMath.Max(positionError, Box2DXMath.Abs(angularC));
}
else if (_limitState == LimitState.AtLowerLimit)
{
float limitC = translation - _lowerTranslation;
positionError = Box2DXMath.Max(positionError, -limitC);
// Prevent large linear corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float limitC = translation - _upperTranslation;
positionError = Box2DXMath.Max(positionError, limitC);
// Prevent large linear corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
b1._sweep.C += (invMass1 * limitImpulse) * _motorJacobian.Linear1;
b1._sweep.A += invI1 * limitImpulse * _motorJacobian.Angular1;
b2._sweep.C += (invMass2 * limitImpulse) * _motorJacobian.Linear2;
b2._sweep.A += invI2 * limitImpulse * _motorJacobian.Angular2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
/// @param callback a callback class that is called for each proxy that is hit by the ray.
public void RayCast(IRayCastEnabled callback, RayCastInput input)
{
Vec2 p1 = input.P1;
Vec2 p2 = input.P2;
Vec2 r = p2 - p1;
Box2DXDebug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vec2 v = Vec2.Cross(1.0f, r);
Vec2 abs_v = Math.Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = Math.Min(p1, t);
segmentAABB.UpperBound = Math.Max(p1, t);
}
const int k_stackSize = 128;
int[] stack = new int[k_stackSize];
int count = 0;
stack[count++] = _root;
while (count > 0)
{
int nodeId = stack[--count];
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode node = _nodes[nodeId];
if (Collision.TestOverlap(node.Aabb, segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vec2 c = node.Aabb.GetCenter();
Vec2 h = node.Aabb.GetExtents();
float separation = Math.Abs(Vec2.Dot(v, p1 - c)) - Vec2.Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput = new RayCastInput();
subInput.P1 = input.P1;
subInput.P2 = input.P2;
subInput.MaxFraction = maxFraction;
maxFraction = callback.RayCastCallback(subInput, nodeId);
if (maxFraction == 0.0f)
{
return;
}
// Update segment bounding box.
{
Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = Math.Min(p1, t);
segmentAABB.UpperBound = Math.Max(p1, t);
}
}
else
{
Box2DXDebug.Assert(count + 1 < k_stackSize);
stack[count++] = node.Child1;
stack[count++] = node.Child2;
}
}
}
// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
internal void Solve3()
{
Vec2 w1 = _v1.w;
Vec2 w2 = _v2.w;
Vec2 w3 = _v3.w;
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
Vec2 e12 = w2 - w1;
float w1e12 = Vec2.Dot(w1, e12);
float w2e12 = Vec2.Dot(w2, e12);
float d12_1 = w2e12;
float d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
Vec2 e13 = w3 - w1;
float w1e13 = Vec2.Dot(w1, e13);
float w3e13 = Vec2.Dot(w3, e13);
float d13_1 = w3e13;
float d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
Vec2 e23 = w3 - w2;
float w2e23 = Vec2.Dot(w2, e23);
float w3e23 = Vec2.Dot(w3, e23);
float d23_1 = w3e23;
float d23_2 = -w2e23;
// Triangle123
float n123 = Vec2.Cross(e12, e13);
float d123_1 = n123 * Vec2.Cross(w2, w3);
float d123_2 = n123 * Vec2.Cross(w3, w1);
float d123_3 = n123 * Vec2.Cross(w1, w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
{
_v1.a = 1.0f;
_count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
float inv_d12 = 1.0f / (d12_1 + d12_2);
_v1.a = d12_1 * inv_d12;
_v2.a = d12_1 * inv_d12;
_count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
float inv_d13 = 1.0f / (d13_1 + d13_2);
_v1.a = d13_1 * inv_d13;
_v3.a = d13_2 * inv_d13;
_count = 2;
_v2 = _v3;
return;
}
// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
{
_v2.a = 1.0f;
_count = 1;
_v1 = _v2;
return;
}
// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
{
_v3.a = 1.0f;
_count = 1;
_v1 = _v3;
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
float inv_d23 = 1.0f / (d23_1 + d23_2);
_v2.a = d23_1 * inv_d23;
_v3.a = d23_2 * inv_d23;
_count = 2;
_v1 = _v3;
return;
}
// Must be in triangle123
float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
_v1.a = d123_1 * inv_d123;
_v2.a = d123_2 * inv_d123;
_v3.a = d123_3 * inv_d123;
_count = 3;
}
/// <summary>
/// Compute the closest points between two shapes. Supports any combination of:
/// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output.
/// On the first call set SimplexCache.Count to zero.
/// </summary>
public unsafe static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
{
output = new DistanceOutput();
XForm transformA = input.TransformA;
XForm transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
fixed(SimplexCache *sPtr = &cache)
{
simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
}
// Get simplex vertices as an array.
SimplexVertex *vertices = &simplex._v1;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int *lastA = stackalloc int[4], lastB = stackalloc int[4];
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.
Vec2 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.
SimplexVertex *vertex = vertices + simplex._count;
vertex->indexA = shapeA.GetSupport(Common.Math.MulT(transformA.R, p));
vertex->wA = Common.Math.Mul(transformA, shapeA.GetVertex(vertex->indexA));
//Vec2 wBLocal;
vertex->indexB = shapeB.GetSupport(Common.Math.MulT(transformB.R, -p));
vertex->wB = Common.Math.Mul(transformB, shapeB.GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
// Iteration count is equated to the number of support point calls.
++iter;
// Check for convergence.
float lowerBound = Vec2.Dot(p, vertex->w);
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 (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
{
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;
}
fixed(DistanceOutput *doPtr = &output)
{
// Prepare output.
simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
doPtr->Distance = Vec2.Distance(doPtr->PointA, doPtr->PointB);
doPtr->Iterations = iter;
}
fixed(SimplexCache *sPtr = &cache)
{
// Cache the simplex.
simplex.WriteCache(sPtr);
}
// 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;
Vec2 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.
Vec2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = body._sweep.C;
float num = body._sweep.A;
Vec2 vec2 = body2._sweep.C;
float num2 = body2._sweep.A;
float num3 = 0f;
bool flag = false;
float z = 0f;
Mat22 a = new Mat22(num);
Mat22 a2 = new Mat22(num2);
Vec2 v = Box2DX.Common.Math.Mul(a, this._localAnchor1 - this._localCenter1);
Vec2 vec3 = Box2DX.Common.Math.Mul(a2, this._localAnchor2 - this._localCenter2);
Vec2 vec4 = vec2 + vec3 - vec - v;
if (this._enableLimit)
{
this._axis = Box2DX.Common.Math.Mul(a, this._localXAxis1);
this._a1 = Vec2.Cross(vec4 + v, this._axis);
this._a2 = Vec2.Cross(vec3, this._axis);
float num4 = Vec2.Dot(this._axis, vec4);
if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
{
z = Box2DX.Common.Math.Clamp(num4, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
num3 = Box2DX.Common.Math.Abs(num4);
flag = true;
}
else
{
if (num4 <= this._lowerTranslation)
{
z = Box2DX.Common.Math.Clamp(num4 - this._lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
num3 = this._lowerTranslation - num4;
flag = true;
}
else
{
if (num4 >= this._upperTranslation)
{
z = Box2DX.Common.Math.Clamp(num4 - this._upperTranslation - Settings.LinearSlop, 0f, Settings.MaxLinearCorrection);
num3 = num4 - this._upperTranslation;
flag = true;
}
}
}
}
this._perp = Box2DX.Common.Math.Mul(a, this._localYAxis1);
this._s1 = Vec2.Cross(vec4 + v, this._perp);
this._s2 = Vec2.Cross(vec3, this._perp);
Vec2 v2 = default(Vec2);
v2.X = Vec2.Dot(this._perp, vec4);
v2.Y = num2 - num - this._refAngle;
num3 = Box2DX.Common.Math.Max(num3, Box2DX.Common.Math.Abs(v2.X));
float num5 = Box2DX.Common.Math.Abs(v2.Y);
Vec3 vec5;
if (flag)
{
float invMass = this._invMass1;
float invMass2 = this._invMass2;
float invI = this._invI1;
float invI2 = this._invI2;
float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
float num6 = invI * this._s1 + invI2 * this._s2;
float num7 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2;
float y = invI + invI2;
float num8 = invI * this._a1 + invI2 * this._a2;
float z2 = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;
this._K.Col1.Set(x, num6, num7);
this._K.Col2.Set(num6, y, num8);
this._K.Col3.Set(num7, num8, z2);
vec5 = this._K.Solve33(-new Vec3
{
X = v2.X,
Y = v2.Y,
Z = z
});
}
else
{
float invMass = this._invMass1;
float invMass2 = this._invMass2;
float invI = this._invI1;
float invI2 = this._invI2;
float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
float num6 = invI * this._s1 + invI2 * this._s2;
float y = invI + invI2;
this._K.Col1.Set(x, num6, 0f);
this._K.Col2.Set(num6, y, 0f);
Vec2 vec6 = this._K.Solve22(-v2);
vec5.X = vec6.X;
vec5.Y = vec6.Y;
vec5.Z = 0f;
}
Vec2 v3 = vec5.X * this._perp + vec5.Z * this._axis;
float num9 = vec5.X * this._s1 + vec5.Y + vec5.Z * this._a1;
float num10 = vec5.X * this._s2 + vec5.Y + vec5.Z * this._a2;
vec -= this._invMass1 * v3;
num -= this._invI1 * num9;
vec2 += this._invMass2 * v3;
num2 += this._invI2 * num10;
body._sweep.C = vec;
body._sweep.A = num;
body2._sweep.C = vec2;
body2._sweep.A = num2;
body.SynchronizeTransform();
body2.SynchronizeTransform();
return(num3 <= Settings.LinearSlop && num5 <= Settings.AngularSlop);
}
// This implements 2-sided edge vs circle collision.
public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, XForm transformA, CircleShape circle, XForm transformB)
{
manifold.PointCount = 0;
Vec2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
Vec2 normal = edge._normal;
Vec2 v1 = edge._v1;
Vec2 v2 = edge._v2;
float radius = edge._radius + circle._radius;
// Barycentric coordinates
float u1 = Vec2.Dot(cLocal - v1, v2 - v1);
float u2 = Vec2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
// Behind v1
if (Vec2.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v1;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v1;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
else if (u2 <= 0.0f)
{
// Ahead of v2
if (Vec2.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v2;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v2;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
else
{
float separation = Vec2.Dot(cLocal - v1, normal);
if (separation < -radius || radius < separation)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal;
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = body._linearVelocity;
float num = body._angularVelocity;
Vec2 vec2 = body2._linearVelocity;
float num2 = body2._angularVelocity;
if (this._enableMotor && this._limitState != LimitState.EqualLimits)
{
float num3 = Vec2.Dot(this._axis, vec2 - vec) + this._a2 * num2 - this._a1 * num;
float num4 = this._motorMass * (this._motorSpeed - num3);
float motorImpulse = this._motorImpulse;
float num5 = step.Dt * this._maxMotorForce;
this._motorImpulse = Box2DX.Common.Math.Clamp(this._motorImpulse + num4, -num5, num5);
num4 = this._motorImpulse - motorImpulse;
Vec2 v = num4 * this._axis;
float num6 = num4 * this._a1;
float num7 = num4 * this._a2;
vec -= this._invMass1 * v;
num -= this._invI1 * num6;
vec2 += this._invMass2 * v;
num2 += this._invI2 * num7;
}
Vec2 v2;
v2.X = Vec2.Dot(this._perp, vec2 - vec) + this._s2 * num2 - this._s1 * num;
v2.Y = num2 - num;
if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
{
float z = Vec2.Dot(this._axis, vec2 - vec) + this._a2 * num2 - this._a1 * num;
Vec3 v3 = new Vec3(v2.X, v2.Y, z);
Vec3 impulse = this._impulse;
Vec3 v4 = this._K.Solve33(-v3);
this._impulse += v4;
if (this._limitState == LimitState.AtLowerLimit)
{
this._impulse.Z = Box2DX.Common.Math.Max(this._impulse.Z, 0f);
}
else
{
if (this._limitState == LimitState.AtUpperLimit)
{
this._impulse.Z = Box2DX.Common.Math.Min(this._impulse.Z, 0f);
}
}
Vec2 b = -v2 - (this._impulse.Z - impulse.Z) * new Vec2(this._K.Col3.X, this._K.Col3.Y);
Vec2 vec3 = this._K.Solve22(b) + new Vec2(impulse.X, impulse.Y);
this._impulse.X = vec3.X;
this._impulse.Y = vec3.Y;
v4 = this._impulse - impulse;
Vec2 v = v4.X * this._perp + v4.Z * this._axis;
float num6 = v4.X * this._s1 + v4.Y + v4.Z * this._a1;
float num7 = v4.X * this._s2 + v4.Y + v4.Z * this._a2;
vec -= this._invMass1 * v;
num -= this._invI1 * num6;
vec2 += this._invMass2 * v;
num2 += this._invI2 * num7;
}
else
{
Vec2 vec4 = this._K.Solve22(-v2);
this._impulse.X = this._impulse.X + vec4.X;
this._impulse.Y = this._impulse.Y + vec4.Y;
Vec2 v = vec4.X * this._perp;
float num6 = vec4.X * this._s1 + vec4.Y;
float num7 = vec4.X * this._s2 + vec4.Y;
vec -= this._invMass1 * v;
num -= this._invI1 * num6;
vec2 += this._invMass2 * v;
num2 += this._invI2 * num7;
}
body._linearVelocity = vec;
body._angularVelocity = num;
body2._linearVelocity = vec2;
body2._angularVelocity = num2;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
this._localCenter1 = body.GetLocalCenter();
this._localCenter2 = body2.GetLocalCenter();
XForm xForm = body.GetXForm();
XForm xForm2 = body2.GetXForm();
Vec2 v = Box2DX.Common.Math.Mul(xForm.R, this._localAnchor1 - this._localCenter1);
Vec2 vec = Box2DX.Common.Math.Mul(xForm2.R, this._localAnchor2 - this._localCenter2);
Vec2 vec2 = body2._sweep.C + vec - body._sweep.C - v;
this._invMass1 = body._invMass;
this._invI1 = body._invI;
this._invMass2 = body2._invMass;
this._invI2 = body2._invI;
this._axis = Box2DX.Common.Math.Mul(xForm.R, this._localXAxis1);
this._a1 = Vec2.Cross(vec2 + v, this._axis);
this._a2 = Vec2.Cross(vec, this._axis);
this._motorMass = this._invMass1 + this._invMass2 + this._invI1 * this._a1 * this._a1 + this._invI2 * this._a2 * this._a2;
Box2DXDebug.Assert(this._motorMass > Settings.FLT_EPSILON);
this._motorMass = 1f / this._motorMass;
this._perp = Box2DX.Common.Math.Mul(xForm.R, this._localYAxis1);
this._s1 = Vec2.Cross(vec2 + v, this._perp);
this._s2 = Vec2.Cross(vec, this._perp);
float invMass = this._invMass1;
float invMass2 = this._invMass2;
float invI = this._invI1;
float invI2 = this._invI2;
float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
float num = invI * this._s1 + invI2 * this._s2;
float num2 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2;
float y = invI + invI2;
float num3 = invI * this._a1 + invI2 * this._a2;
float z = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;
this._K.Col1.Set(x, num, num2);
this._K.Col2.Set(num, y, num3);
this._K.Col3.Set(num2, num3, z);
if (this._enableLimit)
{
float num4 = Vec2.Dot(this._axis, vec2);
if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
{
this._limitState = LimitState.EqualLimits;
}
else
{
if (num4 <= this._lowerTranslation)
{
if (this._limitState != LimitState.AtLowerLimit)
{
this._limitState = LimitState.AtLowerLimit;
this._impulse.Z = 0f;
}
}
else
{
if (num4 >= this._upperTranslation)
{
if (this._limitState != LimitState.AtUpperLimit)
{
this._limitState = LimitState.AtUpperLimit;
this._impulse.Z = 0f;
}
}
else
{
this._limitState = LimitState.InactiveLimit;
this._impulse.Z = 0f;
}
}
}
}
if (!this._enableMotor)
{
this._motorImpulse = 0f;
}
if (step.WarmStarting)
{
this._impulse *= step.DtRatio;
this._motorImpulse *= step.DtRatio;
Vec2 v2 = this._impulse.X * this._perp + (this._motorImpulse + this._impulse.Z) * this._axis;
float num5 = this._impulse.X * this._s1 + this._impulse.Y + (this._motorImpulse + this._impulse.Z) * this._a1;
float num6 = this._impulse.X * this._s2 + this._impulse.Y + (this._motorImpulse + this._impulse.Z) * this._a2;
Body expr_4BB = body;
expr_4BB._linearVelocity -= this._invMass1 * v2;
body._angularVelocity -= this._invI1 * num5;
Body expr_4EF = body2;
expr_4EF._linearVelocity += this._invMass2 * v2;
body2._angularVelocity += this._invI2 * num6;
}
else
{
this._impulse.SetZero();
this._motorImpulse = 0f;
}
}
public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
// Put the ray into the edge's frame of reference.
Vec2 p1 = pool0.Set(input.P1).SubLocal(xf.P);
Rot.MulTrans(xf.Q, p1, p1);
Vec2 p2 = pool1.Set(input.P2).SubLocal(xf.P);
Rot.MulTrans(xf.Q, p1, p1);
Vec2 d = p2.SubLocal(p1); // we don't use p2 later
Vec2 v1 = Vertex1;
Vec2 v2 = Vertex2;
Vec2 normal = pool2.Set(v2).SubLocal(v1);
normal.Set(normal.Y, -normal.X);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
pool3.Set(v1).SubLocal(p1);
float numerator = Vec2.Dot(normal, pool3);
float denominator = Vec2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return(false);
}
Vec2 q = pool3;
Vec2 r = pool4;
// Vec2 q = p1 + t * d;
q.Set(d).MulLocal(t).AddLocal(p1);
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
// Vec2 r = v2 - v1;
r.Set(v2).SubLocal(v1);
float rr = Vec2.Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
pool5.Set(q).SubLocal(v1);
float s = Vec2.Dot(pool5, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.Fraction = t;
if (numerator > 0.0f)
{
// argOutput.normal = -normal;
output.Normal.Set(normal).NegateLocal();
}
else
{
// output.normal = normal;
output.Normal.Set(normal);
}
return(true);
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < Count; ++i)
{
ContactVelocityConstraint vc = VelocityConstraints[i];
int indexA = vc.IndexA;
int indexB = vc.IndexB;
float mA = vc.InvMassA;
float mB = vc.InvMassB;
float iA = vc.InvIA;
float iB = vc.InvIB;
int pointCount = vc.PointCount;
Vec2 vA = Velocities[indexA].V;
float wA = Velocities[indexA].W;
Vec2 vB = Velocities[indexB].V;
float wB = Velocities[indexB].W;
//Debug.Assert(wA == 0);
//Debug.Assert(wB == 0);
Vec2 normal = vc.Normal;
//Vec2.crossToOutUnsafe(normal, 1f, tangent);
tangent.X = 1.0f * vc.Normal.Y;
tangent.Y = (-1.0f) * vc.Normal.X;
float friction = vc.Friction;
Debug.Assert(pointCount == 1 || pointCount == 2);
// Solve tangent constraints
for (int j = 0; j < pointCount; ++j)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
//Vec2.crossToOutUnsafe(wA, vcp.rA, temp);
//Vec2.crossToOutUnsafe(wB, vcp.rB, dv);
//dv.addLocal(vB).subLocal(vA).subLocal(temp);
Vec2 a = vcp.RA;
dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a.Y;
dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a.X;
// Compute tangent force
float vt = dv.X * tangent.X + dv.Y * tangent.Y - vc.TangentSpeed;
float lambda = vcp.TangentMass * (-vt);
// Clamp the accumulated force
float maxFriction = friction * vcp.NormalImpulse;
float newImpulse = MathUtils.Clamp(vcp.TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp.TangentImpulse;
vcp.TangentImpulse = newImpulse;
// Apply contact impulse
// Vec2 P = lambda * tangent;
float Px = tangent.X * lambda;
float Py = tangent.Y * lambda;
// vA -= invMassA * P;
vA.X -= Px * mA;
vA.Y -= Py * mA;
wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px);
// vB += invMassB * P;
vB.X += Px * mB;
vB.Y += Py * mB;
wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px);
//Console.WriteLine("tangent solve velocity (point "+j+") for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA);
//Console.WriteLine("tangent solve velocity (point "+j+") for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB);
}
// Solve normal constraints
if (vc.PointCount == 1)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[0];
Vec2 a1 = vcp.RA;
// Relative velocity at contact
//Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA);
//Vec2.crossToOut(wA, vcp.rA, temp1);
//Vec2.crossToOut(wB, vcp.rB, dv);
//dv.addLocal(vB).subLocal(vA).subLocal(temp1);
dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a1.Y;
dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a1.X;
// Compute normal impulse
float vn = dv.X * normal.X + dv.Y * normal.Y;
float lambda = (-vcp.NormalMass) * (vn - vcp.VelocityBias);
// Clamp the accumulated impulse
float a = vcp.NormalImpulse + lambda;
float newImpulse = (a > 0.0f ? a : 0.0f);
lambda = newImpulse - vcp.NormalImpulse;
//Debug.Assert(newImpulse == 0);
vcp.NormalImpulse = newImpulse;
// Apply contact impulse
float Px = normal.X * lambda;
float Py = normal.Y * lambda;
// vA -= invMassA * P;
vA.X -= Px * mA;
vA.Y -= Py * mA;
wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px);
//Debug.Assert(vA.x == 0);
// vB += invMassB * P;
vB.X += Px * mB;
vB.Y += Py * mB;
wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px);
//Debug.Assert(vB.x == 0);
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on
// Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary
// constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D
// contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be
// tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of
// the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the
// impulse variable (x_i).
//
// Substitute:
//
// x = a + d
//
// a := old total impulse
// x := new total impulse
// d := incremental impulse
//
// For the current iteration we extend the formula for the incremental impulse
// to compute the new total impulse:
//
// vn = A * d + b
// = A * (x - a) + b
// = A * x + b - A * a
// = A * x + b'
// b' = b - A * a;
ContactVelocityConstraint.VelocityConstraintPoint cp1 = vc.Points[0];
ContactVelocityConstraint.VelocityConstraintPoint cp2 = vc.Points[1];
a.X = cp1.NormalImpulse;
a.Y = cp2.NormalImpulse;
Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
// Relative velocity at contact
// Vec2 dv1 = vB + Cross(wB, cp1.rB) - vA - Cross(wA, cp1.rA);
dv1.X = (-wB) * cp1.RB.Y + vB.X - vA.X + wA * cp1.RA.Y;
dv1.Y = wB * cp1.RB.X + vB.Y - vA.Y - wA * cp1.RA.X;
// Vec2 dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
dv2.X = (-wB) * cp2.RB.Y + vB.X - vA.X + wA * cp2.RA.Y;
dv2.Y = wB * cp2.RB.X + vB.Y - vA.Y - wA * cp2.RA.X;
// Compute normal velocity
float vn1 = dv1.X * normal.X + dv1.Y * normal.Y;
float vn2 = dv2.X * normal.X + dv2.Y * normal.Y;
b.X = vn1 - cp1.VelocityBias;
b.Y = vn2 - cp2.VelocityBias;
//Console.WriteLine("b is " + b.x + "," + b.y);
// Compute b'
Mat22 R = vc.K;
b.X -= (R.Ex.X * a.X + R.Ey.X * a.Y);
b.Y -= (R.Ex.Y * a.X + R.Ey.Y * a.Y);
//Console.WriteLine("b' is " + b.x + "," + b.y);
// final float k_errorTol = 1e-3f;
// B2_NOT_USED(k_errorTol);
for (; ;)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
// Vec2 x = - Mul(c.normalMass, b);
Mat22.MulToOutUnsafe(vc.NormalMass, b, x);
x.MulLocal(-1);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
//Console.WriteLine("case 1");
// Get the incremental impulse
// Vec2 d = x - a;
d.Set(x).SubLocal(a);
// Apply incremental impulse
// Vec2 P1 = d.x * normal;
// Vec2 P2 = d.y * normal;
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
/*
* vA -= invMassA * (P1 + P2); wA -= invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
* Cross(wA, cp1.rA); dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
*
* // Compute normal velocity vn1 = Dot(dv1, normal); vn2 = Dot(dv2, normal);
*
* Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); Debug.Assert(Abs(vn2 - cp2.velocityBias)
* < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA)));
Vec2 _dv2 = vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA)));
// Compute normal velocity
vn1 = Vec2.Dot(_dv1, normal);
vn2 = Vec2.Dot(_dv2, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I);
Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I);
}
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + '
//
x.X = (-cp1.NormalMass) * b.X;
x.Y = 0.0f;
vn1 = 0.0f;
vn2 = vc.K.Ex.Y * x.X + b.Y;
if (x.X >= 0.0f && vn2 >= 0.0f)
{
//Console.WriteLine("case 2");
// Get the incremental impulse
d.Set(x).SubLocal(a);
// Apply incremental impulse
// Vec2 P1 = d.x * normal;
// Vec2 P2 = d.y * normal;
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
//Debug.Assert(x.x == 0 && x.y == 0);
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
* Cross(wA, cp1.rA);
*
* // Compute normal velocity vn1 = Dot(dv1, normal);
*
* Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA)));
// Compute normal velocity
vn1 = Vec2.Dot(_dv1, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I);
}
break;
}
//
// Case 3: wB = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + '
//
x.X = 0.0f;
x.Y = (-cp2.NormalMass) * b.Y;
vn1 = vc.K.Ey.X * x.Y + b.X;
vn2 = 0.0f;
if (x.Y >= 0.0f && vn1 >= 0.0f)
{
//Console.WriteLine("case 3");
// Resubstitute for the incremental impulse
d.Set(x).SubLocal(a);
// Apply incremental impulse
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
//Debug.Assert(x.x == 0 && x.y == 0);
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + Cross(wB, cp2.rB) - vA -
* Cross(wA, cp2.rA);
*
* // Compute normal velocity vn2 = Dot(dv2, normal);
*
* Debug.Assert(Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 _dv2 =
vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA)));
// Compute normal velocity
vn2 = Vec2.Dot(_dv2, normal);
Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I);
}
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = ;
x.X = 0.0f;
x.Y = 0.0f;
vn1 = b.X;
vn2 = b.Y;
if (vn1 >= 0.0f && vn2 >= 0.0f)
{
//Console.WriteLine("case 4");
// Resubstitute for the incremental impulse
d.Set(x).SubLocal(a);
// Apply incremental impulse
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
//Debug.Assert(x.x == 0 && x.y == 0);
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
Velocities[indexA].V.Set(vA);
Velocities[indexA].W = wA;
Velocities[indexB].V.Set(vB);
Velocities[indexB].W = wB;
//Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA);
//Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB);
}
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body b1 = c.Body1;
Body b2 = c.Body2;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
Vec2 v1 = b1._linearVelocity;
Vec2 v2 = b2._linearVelocity;
float invMass1 = b1._invMass;
float invI1 = b1._invI;
float invMass2 = b2._invMass;
float invI2 = b2._invI;
Vec2 normal = c.Normal;
Vec2 tangent = Vec2.Cross(normal, 1.0f);
float friction = c.Friction;
Box2DXDebug.Assert(c.PointCount == 1 || c.PointCount == 2);
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1);
// 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;
v1 -= invMass1 * P;
w1 -= invI1 * Vec2.Cross(ccp.R1, P);
v2 += invMass2 * P;
w2 += invI2 * Vec2.Cross(ccp.R2, 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 = c.Points[0];
ContactConstraintPoint cp2 = c.Points[1];
Vec2 a = new Vec2(cp1.NormalImpulse, cp2.NormalImpulse);
Box2DXDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
// Relative velocity at contact
Vec2 dv1 = v2 + Vec2.Cross(w2, cp1.R2) - v1 - Vec2.Cross(w1, cp1.R1);
Vec2 dv2 = v2 + Vec2.Cross(w2, cp2.R2) - v1 - Vec2.Cross(w1, cp2.R1);
// 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;
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;
v1 -= invMass1 * (P1 + P2);
w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
v2 += invMass2 * (P1 + P2);
w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1);
dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
vn2 = Vector2.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;
v1 -= invMass1 * (P1 + P2);
w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
v2 += invMass2 * (P1 + P2);
w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1);
// Compute normal velocity
vn1 = Vector2.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;
v1 -= invMass1 * (P1 + P2);
w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
v2 += invMass2 * (P1 + P2);
w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1);
// Compute normal velocity
vn2 = Vector2.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;
v1 -= invMass1 * (P1 + P2);
w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));
v2 += invMass2 * (P1 + P2);
w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, 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;
}
}
// Solve tangent constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
// Relative velocity at contact
Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1);
// Compute tangent force
float vt = Vec2.Dot(dv, tangent);
float lambda = ccp.TangentMass * (-vt);
// Clamp 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;
v1 -= invMass1 * P;
w1 -= invI1 * Vec2.Cross(ccp.R1, P);
v2 += invMass2 * P;
w2 += invI2 * Vec2.Cross(ccp.R2, P);
ccp.TangentImpulse = newImpulse;
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// Compute the effective masses.
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Compute point to line constraint effective mass.
// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
Vec2 e = b2._sweep.C + r2 - b1._sweep.C; // e = d + r1
_linearJacobian.Set(-ay1, -Vec2.Cross(e, ay1), ay1, Vec2.Cross(r2, ay1));
_linearMass = invMass1 + invI1 * _linearJacobian.Angular1 * _linearJacobian.Angular1 +
invMass2 + invI2 * _linearJacobian.Angular2 * _linearJacobian.Angular2;
Box2DXDebug.Assert(_linearMass > Settings.FLT_EPSILON);
_linearMass = 1.0f / _linearMass;
// Compute angular constraint effective mass.
_angularMass = invI1 + invI2;
if (_angularMass > Settings.FLT_EPSILON)
{
_angularMass = 1.0f / _angularMass;
}
// Compute motor and limit terms.
if (_enableLimit || _enableMotor)
{
// The motor and limit share a Jacobian and effective mass.
Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
_motorJacobian.Set(-ax1, -Vec2.Cross(e, ax1), ax1, Vec2.Cross(r2, ax1));
_motorMass = invMass1 + invI1 * _motorJacobian.Angular1 * _motorJacobian.Angular1 +
invMass2 + invI2 * _motorJacobian.Angular2 * _motorJacobian.Angular2;
Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
if (_enableLimit)
{
Vec2 d = e - r1; // p2 - p1
float jointTranslation = Vec2.Dot(ax1, d);
if (Box2DXMath.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitForce = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitForce = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_limitForce = 0.0f;
}
}
}
if (_enableMotor == false)
{
_motorForce = 0.0f;
}
if (_enableLimit == false)
{
_limitForce = 0.0f;
}
if (step.WarmStarting)
{
Vec2 P1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear1 + (_motorForce + _limitForce) * _motorJacobian.Linear1);
Vec2 P2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear2 + (_motorForce + _limitForce) * _motorJacobian.Linear2);
float L1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular1 - _torque + (_motorForce + _limitForce) * _motorJacobian.Angular1);
float L2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular2 + _torque + (_motorForce + _limitForce) * _motorJacobian.Angular2);
b1._linearVelocity += invMass1 * P1;
b1._angularVelocity += invI1 * L1;
b2._linearVelocity += invMass2 * P2;
b2._angularVelocity += invI2 * L2;
}
else
{
_force = 0.0f;
_torque = 0.0f;
_limitForce = 0.0f;
_motorForce = 0.0f;
}
_limitPositionImpulse = 0.0f;
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
_step = step;
_constraintCount = 0;
for (int i = 0; i < contactCount; ++i)
{
Box2DXDebug.Assert(contacts[i].IsSolid());
_constraintCount += contacts[i].GetManifoldCount();
}
_constraints = new ContactConstraint[_constraintCount];
for (int i = 0; i < _constraintCount; i++)
{
_constraints[i] = new ContactConstraint();
}
int count = 0;
for (int i = 0; i < contactCount; ++i)
{
Contact contact = contacts[i];
Shape shape1 = contact._shape1;
Shape shape2 = contact._shape2;
Body b1 = shape1.GetBody();
Body b2 = shape2.GetBody();
int manifoldCount = contact.GetManifoldCount();
Manifold[] manifolds = contact.GetManifolds();
float friction = Settings.MixFriction(shape1.Friction, shape2.Friction);
float restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution);
Vec2 v1 = b1._linearVelocity;
Vec2 v2 = b2._linearVelocity;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
for (int j = 0; j < manifoldCount; ++j)
{
Manifold manifold = manifolds[j];
Box2DXDebug.Assert(manifold.PointCount > 0);
Vec2 normal = manifold.Normal;
Box2DXDebug.Assert(count < _constraintCount);
ContactConstraint cc = _constraints[count];
cc.Body1 = b1;
cc.Body2 = b2;
cc.Manifold = manifold;
cc.Normal = normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.Restitution = restitution;
for (int k = 0; k < cc.PointCount; ++k)
{
ManifoldPoint cp = manifold.Points[k];
ContactConstraintPoint ccp = cc.Points[k];
ccp.NormalImpulse = cp.NormalImpulse;
ccp.TangentImpulse = cp.TangentImpulse;
ccp.Separation = cp.Separation;
ccp.LocalAnchor1 = cp.LocalPoint1;
ccp.LocalAnchor2 = cp.LocalPoint2;
ccp.R1 = Common.Math.Mul(b1.GetXForm().R, cp.LocalPoint1 - b1.GetLocalCenter());
ccp.R2 = Common.Math.Mul(b2.GetXForm().R, cp.LocalPoint2 - b2.GetLocalCenter());
float rn1 = Vec2.Cross(ccp.R1, normal);
float rn2 = Vec2.Cross(ccp.R2, normal);
rn1 *= rn1;
rn2 *= rn2;
float kNormal = b1._invMass + b2._invMass + b1._invI * rn1 + b2._invI * rn2;
Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
ccp.NormalMass = 1.0f / kNormal;
float kEqualized = b1._mass * b1._invMass + b2._mass * b2._invMass;
kEqualized += b1._mass * b1._invI * rn1 + b2._mass * b2._invI * rn2;
Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
ccp.EqualizedMass = 1.0f / kEqualized;
Vec2 tangent = Vec2.Cross(normal, 1.0f);
float rt1 = Vec2.Cross(ccp.R1, tangent);
float rt2 = Vec2.Cross(ccp.R2, tangent);
rt1 *= rt1;
rt2 *= rt2;
float kTangent = b1._invMass + b2._invMass + b1._invI * rt1 + b2._invI * rt2;
Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
if (ccp.Separation > 0.0f)
{
ccp.VelocityBias = -step.Inv_Dt * ccp.Separation; // TODO_ERIN b2TimeStep
}
else
{
float vRel = Vec2.Dot(cc.Normal, v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1));
if (vRel < -Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.Restitution * vRel;
}
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = cc.Points[0];
ContactConstraintPoint ccp2 = cc.Points[1];
float invMass1 = b1._invMass;
float invI1 = b1._invI;
float invMass2 = b2._invMass;
float invI2 = b2._invI;
float rn11 = Vec2.Cross(ccp1.R1, normal);
float rn12 = Vec2.Cross(ccp1.R2, normal);
float rn21 = Vec2.Cross(ccp2.R1, normal);
float rn22 = Vec2.Cross(ccp2.R2, normal);
float k11 = invMass1 + invMass2 + invI1 * rn11 * rn11 + invI2 * rn12 * rn12;
float k22 = invMass1 + invMass2 + invI1 * rn21 * rn21 + invI2 * rn22 * rn22;
float k12 = invMass1 + invMass2 + invI1 * rn11 * rn21 + invI2 * rn12 * rn22;
// 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.Set(k11, k12);
cc.K.Col2.Set(k12, k22);
cc.NormalMass = cc.K.Invert();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
++count;
}
}
Box2DXDebug.Assert(count == _constraintCount);
}
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
if (!body.IsStatic())
{
Body expr_27 = body;
//expr_27._linearVelocity += step.Dt * (gravity + body._invMass * body._force);
expr_27._linearVelocity += step.Dt * (body._useGravity ? body._gravity : gravity + body._invMass * body._force); //Steve body gravity
body._angularVelocity += step.Dt * body._invI * body._torque;
body._force.Set(0f, 0f);
body._torque = 0f;
Body expr_9E = body;
expr_9E._linearVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._linearDamping, 0f, 1f);
body._angularVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._angularDamping, 0f, 1f);
if (Vec2.Dot(body._linearVelocity, body._linearVelocity) > Settings.MaxLinearVelocitySquared)
{
body._linearVelocity.Normalize();
Body expr_130 = body;
expr_130._linearVelocity *= Settings.MaxLinearVelocity;
}
if (body._angularVelocity * body._angularVelocity > Settings.MaxAngularVelocitySquared)
{
if (body._angularVelocity < 0f)
{
body._angularVelocity = -Settings.MaxAngularVelocity;
}
else
{
body._angularVelocity = Settings.MaxAngularVelocity;
}
}
}
}
ContactSolver contactSolver = new ContactSolver(step, this._contacts, this._contactCount);
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < this._jointCount; i++)
{
this._joints[i].InitVelocityConstraints(step);
}
for (int i = 0; i < step.VelocityIterations; i++)
{
for (int j = 0; j < this._jointCount; j++)
{
this._joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
contactSolver.FinalizeVelocityConstraints();
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
if (!body.IsStatic())
{
body._sweep.C0 = body._sweep.C;
body._sweep.A0 = body._sweep.A;
Body expr_296_cp_0 = body;
expr_296_cp_0._sweep.C = expr_296_cp_0._sweep.C + step.Dt * body._linearVelocity;
Body expr_2BE_cp_0 = body;
expr_2BE_cp_0._sweep.A = expr_2BE_cp_0._sweep.A + step.Dt * body._angularVelocity;
body.SynchronizeTransform();
}
}
for (int k = 0; k < step.PositionIterations; k++)
{
bool flag = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool flag2 = true;
for (int i = 0; i < this._jointCount; i++)
{
bool flag3 = this._joints[i].SolvePositionConstraints(Settings.ContactBaumgarte);
flag2 = (flag2 && flag3);
}
if (flag && flag2)
{
break;
}
}
this.Report(contactSolver._constraints);
if (allowSleep)
{
float num = Settings.FLT_MAX;
float num2 = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float num3 = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
if (body._invMass != 0f)
{
if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags) 0)
{
body._sleepTime = 0f;
num = 0f;
}
if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags) 0 || body._angularVelocity * body._angularVelocity > num3 || Vec2.Dot(body._linearVelocity, body._linearVelocity) > num2)
{
body._sleepTime = 0f;
num = 0f;
}
else
{
body._sleepTime += step.Dt;
num = Box2DX.Common.Math.Min(num, body._sleepTime);
}
}
}
if (num >= Settings.TimeToSleep)
{
for (int i = 0; i < this._bodyCount; i++)
{
Body body = this._bodies[i];
body._flags |= Body.BodyFlags.Sleep;
body._linearVelocity = Vec2.Zero;
body._angularVelocity = 0f;
}
}
}
}
/// <summary>
/// update override
/// </summary>
/// <param name="gameTime"></param>
public override void Update(GameTime gameTime)
{
if (Active)
{
var rotationDegrees = RigidBody.GetAngle() * 180 / System.Math.PI;
if (Keyboard.GetState().IsKeyDown(Keys.OemOpenBrackets))
{
if (Vec2.Distance(Vec2.Zero, RigidBody.GetLinearVelocity()) < GameData.PlayerMaxSpeed)
{
//apply impulse to push the player to left
var impulseVec = GameUtils.RotationToVec2((float)rotationDegrees - 90);
RigidBody.ApplyImpulse(impulseVec * GameData.PlayerLateralImpulse
, RigidBody.GetPosition());
}
}
if (Keyboard.GetState().IsKeyDown(Keys.OemCloseBrackets))
{
if (Vec2.Distance(Vec2.Zero, RigidBody.GetLinearVelocity()) < GameData.PlayerMaxSpeed)
{
//apply impulse to push the player to right
var impulseVec = GameUtils.RotationToVec2((float)rotationDegrees + 90);
RigidBody.ApplyImpulse(impulseVec * GameData.PlayerLateralImpulse
, RigidBody.GetPosition());
}
}
if (FilteredInputListener.WasKeyPressed(Keys.Left))
{
WeaponInventory.SelectPreviousWeapon();
FilteredInputListener.ResetKey(Keys.Left);
}
if (FilteredInputListener.WasKeyPressed(Keys.Right))
{
WeaponInventory.SelectNextWeapon();
FilteredInputListener.ResetKey(Keys.Right);
}
if (Keyboard.GetState().IsKeyDown(Keys.W))
{
if (Vec2.Distance(Vec2.Zero, RigidBody.GetLinearVelocity()) < GameData.PlayerMaxSpeed)
{
var impulseVec = GameUtils.RotationToVec2((float)rotationDegrees);
if (Vec2.Dot(impulseVec, RigidBody.GetLinearVelocity()) == 0)
{
RigidBody.SetLinearVelocity(Vec2.Zero);
}
RigidBody.ApplyImpulse(impulseVec * GameData.PlayerImpulse
, RigidBody.GetPosition());
}
}
if (Keyboard.GetState().IsKeyDown(Keys.S))
{
if (Vec2.Distance(Vec2.Zero, RigidBody.GetLinearVelocity()) < GameData.PlayerMaxSpeed)
{
var impulseVec = GameUtils.RotationToVec2((float)rotationDegrees);
RigidBody.ApplyImpulse(impulseVec * -GameData.PlayerImpulse
, RigidBody.GetPosition());
}
}
if (Keyboard.GetState().IsKeyDown(Keys.D))
{
//DecreaseLinearVelocity(GameData.PlayerTurnVelocityDecrement, 1);
RigidBody.ApplyTorque(GameData.PlayerTurnTorque);
}
if (Keyboard.GetState().IsKeyDown(Keys.A))
{
//DecreaseLinearVelocity(GameData.PlayerTurnVelocityDecrement, 1);
RigidBody.ApplyTorque(-GameData.PlayerTurnTorque);
}
if (Keyboard.GetState().IsKeyDown(Keys.Space))
{
var weapon = WeaponInventory.GetSelectedWeapon();
if (weapon != null && weapon.RemainingAmmo > 0)
{
if (DateTime.Now - _lastProjectileTime > TimeSpan.FromMilliseconds(100))
{
ShootingEffect.Play();
_lastProjectileTime = DateTime.Now;
WeaponInventory.DecreaseAmmo(1);
SpawnProjectile(weapon.ProjectileName, ProjectileSource.Player);
}
}
}
if (Keyboard.GetState().IsKeyUp(Keys.W) &&
Keyboard.GetState().IsKeyUp(Keys.A) &&
Keyboard.GetState().IsKeyUp(Keys.S) &&
Keyboard.GetState().IsKeyUp(Keys.D))
{
DecreaseLinearVelocity(GameData.PlayerTurnVelocityDecrement, 0);
}
if (Keyboard.GetState().IsKeyUp(Keys.A) && Keyboard.GetState().IsKeyUp(Keys.D))
{
RigidBody.SetAngularVelocity(0);
}
}
}
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 = Box2DNet.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
Vec2 r2 = Box2DNet.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 = Box2DNet.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;
Box2DNetDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic constraint.
{
_perp = Box2DNet.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 (Box2DNet.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;
}
}
// 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;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
{
return;
}
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
XForm xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = ManifoldType.FaceA;
flip = 0;
}
ClipVertex[] incidentEdge;
Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1._vertexCount;
Vec2[] vertices1 = poly1._vertices;
Vec2 v11 = vertices1[edge1];
Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vec2 dv = v12 - v11;
Vec2 localNormal = Vec2.Cross(dv, 1.0f);
localNormal.Normalize();
Vec2 planePoint = 0.5f * (v11 + v12);
Vec2 sideNormal = Common.Math.Mul(xf1.R, v12 - v11);
sideNormal.Normalize();
Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f);
v11 = Common.Math.Mul(xf1, v11);
v12 = Common.Math.Mul(xf1, v12);
float frontOffset = Vec2.Dot(frontNormal, v11);
float sideOffset1 = -Vec2.Dot(sideNormal, v11);
float sideOffset2 = Vec2.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = Collision.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.LocalPlaneNormal = localNormal;
manifold.LocalPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vec2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = Common.Math.MulT(xf2, 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;
Vec2 c1 = b1._sweep.C;
float a1 = b1._sweep.A;
Vec2 c2 = b2._sweep.C;
float a2 = b2._sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);
Vec2 r1 = Box2DNet.Common.Math.Mul(R1, _localAnchor1 - _localCenter1);
Vec2 r2 = Box2DNet.Common.Math.Mul(R2, _localAnchor2 - _localCenter2);
Vec2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = Box2DNet.Common.Math.Mul(R1, _localXAxis1);
_a1 = Vec2.Cross(d + r1, _axis);
_a2 = Vec2.Cross(r2, _axis);
float translation = Vec2.Dot(_axis, d);
if (Box2DNet.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = Box2DNet.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Box2DNet.Common.Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DNet.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DNet.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = Box2DNet.Common.Math.Mul(R1, _localYAxis1);
_s1 = Vec2.Cross(d + r1, _perp);
_s2 = Vec2.Cross(r2, _perp);
Vec2 impulse;
float C1;
C1 = Vec2.Dot(_perp, d);
linearError = Box2DNet.Common.Math.Max(linearError, Box2DNet.Common.Math.Abs(C1));
angularError = 0.0f;
if (active)
{
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);
Vec2 C = new Vec2();
C.X = C1;
C.Y = C2;
impulse = _K.Solve(-C);
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float impulse1 = (-C1) / k11;
impulse.X = impulse1;
impulse.Y = 0.0f;
}
Vec2 P = impulse.X * _perp + impulse.Y * _axis;
float L1 = impulse.X * _s1 + impulse.Y * _a1;
float L2 = impulse.X * _s2 + impulse.Y * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.C = c1;
b1._sweep.A = a1;
b2._sweep.C = c2;
b2._sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/// <summary>
/// Find the max separation between poly1 and poly2 using edge normals from poly1.
/// </summary>
public static float FindMaxSeparation(ref int edgeIndex, PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1._vertexCount;
Vec2[] normals1 = poly1._normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Vec2 d = Common.Math.Mul(xf2, poly2._centroid) - Common.Math.Mul(xf1, poly1._centroid);
Vec2 dLocal1 = Common.Math.MulT(xf1.R, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -Common.Settings.FLT_MAX;
for (int i = 0; i < count1; ++i)
{
float dot = Vec2.Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = Collision.EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = Collision.EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
edgeIndex = edge;
return(s);
}
// Perform a local search for the best edge normal.
for (; ;)
{
if (increment == -1)
{
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
}
else
{
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
}
s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return(bestSeparation);
}
public override Vec2 GetSupportVertex(Vec2 d)
{
return(Vec2.Dot(_v1, d) > Vec2.Dot(_v2, d) ? _v1 : _v2);
}
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
{
continue;
}
// Integrate velocities.
b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.Dt * b._invI * b._torque;
// Reset forces.
b._force.Set(0.0f, 0.0f);
b._torque = 0.0f;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
// Check for large velocities.
#if TARGET_FLOAT32_IS_FIXED
// Fixed point code written this way to prevent
// overflows, float code is optimized for speed
float vMagnitude = b._linearVelocity.Length();
if (vMagnitude > Settings.MaxLinearVelocity)
{
b._linearVelocity *= Settings.MaxLinearVelocity / vMagnitude;
}
b._angularVelocity = Vector2.Clamp(b._angularVelocity,
-Settings.MaxAngularVelocity, Settings.MaxAngularVelocity);
#else
if (Vec2.Dot(b._linearVelocity, b._linearVelocity) > Settings.MaxLinearVelocitySquared)
{
b._linearVelocity.Normalize();
b._linearVelocity *= Settings.MaxLinearVelocity;
}
if (b._angularVelocity * b._angularVelocity > Settings.MaxAngularVelocitySquared)
{
if (b._angularVelocity < 0.0f)
{
b._angularVelocity = -Settings.MaxAngularVelocity;
}
else
{
b._angularVelocity = Settings.MaxAngularVelocity;
}
}
#endif
}
ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
{
continue;
}
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
// Integrate
b._sweep.C += step.Dt * b._linearVelocity;
b._sweep.A += step.Dt * b._angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int ii = 0; ii < step.PositionIterations; ++ii)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int i = 0; i < _jointCount; ++i)
{
bool jointOkay = _joints[i].SolvePositionConstraints(/*Settings.ContactBaumgarte*/);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Common.Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b._angularVelocity *b._angularVelocity > angTolSqr ||
Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.Dt;
minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
}
}
public override float ComputeSubmergedArea(Vec2 normal, float offset, XForm xf, out Vec2 c)
{
//Transform plane into shape co-ordinates
Vec2 normalL = Box2DX.Common.Math.MulT(xf.R, normal);
float offsetL = offset - Vec2.Dot(normal, xf.Position);
float[] depths = new float[Common.Settings.MaxPolygonVertices];
int diveCount = 0;
int intoIndex = -1;
int outoIndex = -1;
bool lastSubmerged = false;
int i;
for (i = 0; i < _vertexCount; i++)
{
depths[i] = Vec2.Dot(normalL, _vertices[i]) - offsetL;
bool isSubmerged = depths[i] < -Common.Settings.FLT_EPSILON;
if (i > 0)
{
if (isSubmerged)
{
if (!lastSubmerged)
{
intoIndex = i - 1;
diveCount++;
}
}
else
{
if (lastSubmerged)
{
outoIndex = i - 1;
diveCount++;
}
}
}
lastSubmerged = isSubmerged;
}
switch (diveCount)
{
case 0:
if (lastSubmerged)
{
//Completely submerged
MassData md;
ComputeMass(out md, 1f);
c = Common.Math.Mul(xf, md.Center);
return(md.Mass);
}
else
{
//Completely dry
c = new Vec2();
return(0);
}
break;
case 1:
if (intoIndex == -1)
{
intoIndex = _vertexCount - 1;
}
else
{
outoIndex = _vertexCount - 1;
}
break;
}
int intoIndex2 = (intoIndex + 1) % _vertexCount;
int outoIndex2 = (outoIndex + 1) % _vertexCount;
float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
Vec2 intoVec = new Vec2(_vertices[intoIndex].X * (1 - intoLambda) + _vertices[intoIndex2].X * intoLambda,
_vertices[intoIndex].Y * (1 - intoLambda) + _vertices[intoIndex2].Y * intoLambda);
Vec2 outoVec = new Vec2(_vertices[outoIndex].X * (1 - outoLambda) + _vertices[outoIndex2].X * outoLambda,
_vertices[outoIndex].Y * (1 - outoLambda) + _vertices[outoIndex2].Y * outoLambda);
//Initialize accumulator
float area = 0;
Vec2 center = new Vec2(0, 0);
Vec2 p2 = _vertices[intoIndex2];
Vec2 p3;
const float k_inv3 = 1.0f / 3.0f;
//An awkward loop from intoIndex2+1 to outIndex2
i = intoIndex2;
while (i != outoIndex2)
{
i = (i + 1) % _vertexCount;
if (i == outoIndex2)
{
p3 = outoVec;
}
else
{
p3 = _vertices[i];
}
//Add the triangle formed by intoVec,p2,p3
{
Vec2 e1 = p2 - intoVec;
Vec2 e2 = p3 - intoVec;
float D = Vec2.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (intoVec + p2 + p3);
}
//
p2 = p3;
}
//Normalize and transform centroid
center *= 1.0f / area;
c = Common.Math.Mul(xf, center);
return(area);
}
public void InitializeVelocityConstraints()
{
//Console.WriteLine("Initializing velocity constraints for " + m_count + " contacts");
// Warm start.
for (int i = 0; i < Count; ++i)
{
ContactVelocityConstraint vc = VelocityConstraints[i];
ContactPositionConstraint pc = PositionConstraints[i];
float radiusA = pc.RadiusA;
float radiusB = pc.RadiusB;
Manifold manifold = Contacts[vc.ContactIndex].Manifold;
int indexA = vc.IndexA;
int indexB = vc.IndexB;
float mA = vc.InvMassA;
float mB = vc.InvMassB;
float iA = vc.InvIA;
float iB = vc.InvIB;
Vec2 localCenterA = pc.LocalCenterA;
Vec2 localCenterB = pc.LocalCenterB;
Vec2 cA = Positions[indexA].C;
float aA = Positions[indexA].A;
Vec2 vA = Velocities[indexA].V;
float wA = Velocities[indexA].W;
Vec2 cB = Positions[indexB].C;
float aB = Positions[indexB].A;
Vec2 vB = Velocities[indexB].V;
float wB = Velocities[indexB].W;
Debug.Assert(manifold.PointCount > 0);
xfA.Q.Set(aA);
xfB.Q.Set(aB);
Rot.MulToOutUnsafe(xfA.Q, localCenterA, temp);
xfA.P.Set(cA).SubLocal(temp);
Rot.MulToOutUnsafe(xfB.Q, localCenterB, temp);
xfB.P.Set(cB).SubLocal(temp);
worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
vc.Normal.Set(worldManifold.Normal);
int pointCount = vc.PointCount;
for (int j = 0; j < pointCount; ++j)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
vcp.RA.Set(worldManifold.Points[j]).SubLocal(cA);
vcp.RB.Set(worldManifold.Points[j]).SubLocal(cB);
float rnA = Vec2.Cross(vcp.RA, vc.Normal);
float rnB = Vec2.Cross(vcp.RB, vc.Normal);
float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
Vec2.CrossToOutUnsafe(vc.Normal, 1.0f, tangent);
float rtA = Vec2.Cross(vcp.RA, tangent);
float rtB = Vec2.Cross(vcp.RB, tangent);
float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
// Setup a velocity bias for restitution.
vcp.VelocityBias = 0.0f;
Vec2.CrossToOutUnsafe(wB, vcp.RB, temp1);
Vec2.CrossToOutUnsafe(wA, vcp.RA, temp2);
temp.Set(vB).AddLocal(temp1).SubLocal(vA).SubLocal(temp2);
float vRel = Vec2.Dot(vc.Normal, temp);
if (vRel < -Settings.VELOCITY_THRESHOLD)
{
vcp.VelocityBias = (-vc.Restitution) * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc.PointCount == 2)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp1 = vc.Points[0];
ContactVelocityConstraint.VelocityConstraintPoint vcp2 = vc.Points[1];
float rn1A = Vec2.Cross(vcp1.RA, vc.Normal);
float rn1B = Vec2.Cross(vcp1.RB, vc.Normal);
float rn2A = Vec2.Cross(vcp2.RA, vc.Normal);
float rn2B = Vec2.Cross(vcp2.RB, vc.Normal);
float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
if (k11 * k11 < MAX_CONDITION_NUMBER * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
vc.K.Ex.Set(k11, k12);
vc.K.Ey.Set(k12, k22);
vc.K.InvertToOut(vc.NormalMass);
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc.PointCount = 1;
}
}
}
}
public float Compute(Vec2 x1, float a1, Vec2 x2, float a2)
{
return(Vec2.Dot(Linear1, x1) + Angular1 * a1 + Vec2.Dot(Linear2, x2) + Angular2 * a2);
}
/// <summary>
/// Gets the support vertex using the specified d
/// </summary>
/// <param name="d">The </param>
/// <returns>The vec</returns>
public override Vec2 GetSupportVertex(Vec2 d)
{
return(Vec2.Dot(Vertex1, d) > Vec2.Dot(Vertex2, d) ? Vertex1 : Vertex2);
}
