internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// You cannot create a prismatic joint between bodies that
// both have fixed rotation.
Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
Transform xf1 = b1.GetTransform();
Transform xf2 = b2.GetTransform();
// Compute the effective masses.
Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1);
Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2);
Vector2 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 = xf1.TransformDirection(_localXAxis1);
_a1 = (d + r1).Cross(_axis);
_a2 = r2.Cross(_axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic constraint.
{
_perp = xf1.TransformDirection(_localYAxis1);
_s1 = (d + r1).Cross(_perp);
_s2 = r2.Cross(_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 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.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.z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitState = LimitState.AtUpperLimit;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.z = 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;
Vector2 P = _impulse.x * _perp + (_motorImpulse + _impulse.z) * _axis;
float L1 = _impulse.x * _s1 + _impulse.y + (_motorImpulse + _impulse.z) * _a1;
float L2 = _impulse.x * _s2 + _impulse.y + (_motorImpulse + _impulse.z) * _a2;
b1._linearVelocity -= _invMass1 * P;
b1._angularVelocity -= _invI1 * L1;
b2._linearVelocity += _invMass2 * P;
b2._angularVelocity += _invI2 * L2;
}
else
{
_impulse = Vector3.zero;
_motorImpulse = 0.0f;
}
}
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 PolyAndCircleContact(Fixture fixtureA, Fixture fixtureB)
: base(fixtureA, fixtureB)
{
Box2DXDebug.Assert(_fixtureA.GetType() == ShapeType.PolygonShape);
Box2DXDebug.Assert(_fixtureB.GetType() == ShapeType.CircleShape);
}
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;
}
}
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;
}
}
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;
Vector2 vA = bodyA._linearVelocity;
Vector2 vB = bodyB._linearVelocity;
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
Vector2 normal = c.Normal;
Vector2 tangent = normal.CrossScalarPostMultiply(1.0f);
float friction = c.Friction;
Box2DXDebug.Assert(c.PointCount == 1 || c.PointCount == 2);
#if ALLOWUNSAFE
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
Vector2 dv = vB + ccp->RB.CrossScalarPreMultiply(wB) - vA - ccp->RA.CrossScalarPreMultiply(wA);
// Compute tangent force
float vt = Vector2.Dot(dv, tangent);
float lambda = ccp->TangentMass * (-vt);
// b2Clamp the accumulated force
float maxFriction = friction * ccp->NormalImpulse;
float newImpulse = Mathf.Clamp(ccp->TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp->TangentImpulse;
// Apply contact impulse
Vector2 P = lambda * tangent;
vA -= invMassA * P;
wA -= invIA * ccp->RA.Cross(P);
vB += invMassB * P;
wB += invIB * ccp->RB.Cross(P);
ccp->TangentImpulse = newImpulse;
}
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA);
// Compute normal impulse
float vn = Vector2.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
Vector2 P = lambda * normal;
vA -= invMassA * P;
wA -= invIA * ccp.RA.Cross(P);
vB += invMassB * P;
wB += invIB * ccp.RB.Cross(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];
Vector2 a = new Vector2(cp1->NormalImpulse, cp2->NormalImpulse);
Box2DXDebug.Assert(a.x >= 0.0f && a.y >= 0.0f);
// Relative velocity at contact
Vector2 dv1 = vB + cp1->RB.CrossScalarPreMultiply(wB) - vA - cp1->RA.CrossScalarPreMultiply(wA);
Vector2 dv2 = vB + cp2->RB.CrossScalarPreMultiply(wB) - vA - cp2->RA.CrossScalarPreMultiply(wA);
// Compute normal velocity
float vn1 = Vector2.Dot(dv1, normal);
float vn2 = Vector2.Dot(dv2, normal);
Vector2 b = new Vector2(vn1 - cp1->VelocityBias, vn2 - cp2->VelocityBias);
b -= c.K.Multiply(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'
//
Vector2 x = -c.NormalMass.Multiply(b);
if (x.x >= 0.0f && x.y >= 0.0f)
{
// Resubstitute for the incremental impulse
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.x * normal;
Vector2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.x * normal;
Vector2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.x * normal;
Vector2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.x * normal;
Vector2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(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;
}
}
#else
ContactConstraintPoint[] pointsPtr = c.Points;
// Solve tangent constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = pointsPtr[j];
// Relative velocity at contact
Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA);
// Compute tangent force
float vt = Vector2.Dot(dv, tangent);
float lambda = ccp.TangentMass * (-vt);
// b2Clamp the accumulated force
float maxFriction = friction * ccp.NormalImpulse;
float newImpulse = Box2DX.Common.Math.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp.TangentImpulse;
// Apply contact impulse
Vector2 P = lambda * tangent;
vA -= invMassA * P;
wA -= invIA * ccp.RA.Cross(P);
vB += invMassB * P;
wB += invIB * ccp.RB.Cross(P);
ccp.TangentImpulse = newImpulse;
}
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA);
// Compute normal impulse
float vn = Vector2.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
Vector2 P = lambda * normal;
vA -= invMassA * P;
wA -= invIA * ccp.RA.Cross(P);
vB += invMassB * P;
wB += invIB * ccp.RB.Cross(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];
Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse);
Box2DXDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
// Relative velocity at contact
Vector2 dv1 = vB + cp1.RB.CrossScalarPreMultiply(wB) - vA - cp1.RA.CrossScalarPreMultiply(wA);
Vector2 dv2 = vB + cp2.RB.CrossScalarPreMultiply(wB) - vA - cp2.RA.CrossScalarPreMultiply(wA);
// Compute normal velocity
float vn1 = Vector2.Dot(dv1, normal);
float vn2 = Vector2.Dot(dv2, normal);
Vector2 b = new Vector2(vn1 - cp1.VelocityBias, vn2 - cp2.VelocityBias);
b -= c.K.Multiply(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'
//
Vector2 x = -c.NormalMass.Multiply(b);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
// Resubstitute for the incremental impulse
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(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
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));
vB += invMassB * (P1 + P2);
wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(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;
#endif // ALLOWUNSAFE
}
}
// 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);
}
}
}
// 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];
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;
}
}
}
// This algorithm uses conservative advancement to compute the time of
// impact (TOI) of two shapes.
// Refs: Bullet, Young Kim
/// <summary>
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// warning the sweeps must have the same time interval.
/// </summary>
/// <param name="shape1"></param>
/// <param name="sweep1"></param>
/// <param name="shape2"></param>
/// <param name="sweep2"></param>
/// <returns>
/// The fraction between [0,1] in which the shapes first touch.
/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
/// </returns>
#warning : "check params"
public static float TimeOfImpact(Shape shape1, Sweep sweep1, Shape shape2, Sweep sweep2)
{
float r1 = shape1.GetSweepRadius();
float r2 = shape2.GetSweepRadius();
Box2DXDebug.Assert(sweep1.T0 == sweep2.T0);
Box2DXDebug.Assert(1.0f - sweep1.T0 > Common.Settings.FLT_EPSILON);
float t0 = sweep1.T0;
Vec2 v1 = sweep1.C - sweep1.C0;
Vec2 v2 = sweep2.C - sweep2.C0;
float omega1 = sweep1.A - sweep1.A0;
float omega2 = sweep2.A - sweep2.A0;
float alpha = 0.0f;
Vec2 p1, p2;
int k_maxIterations = 20; // TODO_ERIN b2Settings
int iter = 0;
Vec2 normal = Vec2.Zero;
float distance = 0.0f;
float targetDistance = 0.0f;
for (; ;)
{
float t = (1.0f - alpha) * t0 + alpha;
XForm xf1, xf2;
sweep1.GetXForm(out xf1, t);
sweep2.GetXForm(out xf2, t);
// Get the distance between shapes.
distance = Collision.Distance(out p1, out p2, shape1, xf1, shape2, xf2);
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement.
if (distance > 2.0f * Settings.ToiSlop)
{
targetDistance = 1.5f * Settings.ToiSlop;
}
else
{
targetDistance = Common.Math.Max(0.05f * Settings.ToiSlop, distance - 0.5f * Settings.ToiSlop);
}
}
if (distance - targetDistance < 0.05f * Settings.ToiSlop || iter == k_maxIterations)
{
break;
}
normal = p2 - p1;
normal.Normalize();
// Compute upper bound on remaining movement.
float approachVelocityBound = Vec2.Dot(normal, v1 - v2) +
Common.Math.Abs(omega1) * r1 + Common.Math.Abs(omega2) * r2;
if (Common.Math.Abs(approachVelocityBound) < Common.Settings.FLT_EPSILON)
{
alpha = 1.0f;
break;
}
// Get the conservative time increment. Don't advance all the way.
float dAlpha = (distance - targetDistance) / approachVelocityBound;
//float dt = (distance - 0.5f * Settings.LinearSlop) / approachVelocityBound;
float newAlpha = alpha + dAlpha;
// The shapes may be moving apart or a safe distance apart.
if (newAlpha < 0.0f || 1.0f < newAlpha)
{
alpha = 1.0f;
break;
}
// Ensure significant advancement.
if (newAlpha < (1.0f + 100.0f * Common.Settings.FLT_EPSILON) * alpha)
{
break;
}
alpha = newAlpha;
++iter;
}
return(alpha);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
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 v = body._sweep.C + vec;
Vec2 v2 = body2._sweep.C + vec2;
Vec2 v3 = this._ground.GetXForm().Position + this._groundAnchor1;
Vec2 v4 = this._ground.GetXForm().Position + this._groundAnchor2;
this._u1 = v - v3;
this._u2 = v2 - v4;
float num = this._u1.Length();
float num2 = this._u2.Length();
if (num > Settings.LinearSlop)
{
this._u1 *= 1f / num;
}
else
{
this._u1.SetZero();
}
if (num2 > Settings.LinearSlop)
{
this._u2 *= 1f / num2;
}
else
{
this._u2.SetZero();
}
float num3 = this._constant - num - this._ratio * num2;
if (num3 > 0f)
{
this._state = LimitState.InactiveLimit;
this._impulse = 0f;
}
else
{
this._state = LimitState.AtUpperLimit;
}
if (num < this._maxLength1)
{
this._limitState1 = LimitState.InactiveLimit;
this._limitImpulse1 = 0f;
}
else
{
this._limitState1 = LimitState.AtUpperLimit;
}
if (num2 < this._maxLength2)
{
this._limitState2 = LimitState.InactiveLimit;
this._limitImpulse2 = 0f;
}
else
{
this._limitState2 = LimitState.AtUpperLimit;
}
float num4 = Vec2.Cross(vec, this._u1);
float num5 = Vec2.Cross(vec2, this._u2);
this._limitMass1 = body._invMass + body._invI * num4 * num4;
this._limitMass2 = body2._invMass + body2._invI * num5 * num5;
this._pulleyMass = this._limitMass1 + this._ratio * this._ratio * this._limitMass2;
Box2DXDebug.Assert(this._limitMass1 > Settings.FLT_EPSILON);
Box2DXDebug.Assert(this._limitMass2 > Settings.FLT_EPSILON);
Box2DXDebug.Assert(this._pulleyMass > Settings.FLT_EPSILON);
this._limitMass1 = 1f / this._limitMass1;
this._limitMass2 = 1f / this._limitMass2;
this._pulleyMass = 1f / this._pulleyMass;
if (step.WarmStarting)
{
this._impulse *= step.DtRatio;
this._limitImpulse1 *= step.DtRatio;
this._limitImpulse2 *= step.DtRatio;
Vec2 vec3 = -(this._impulse + this._limitImpulse1) * this._u1;
Vec2 vec4 = (-this._ratio * this._impulse - this._limitImpulse2) * this._u2;
Body expr_37B = body;
expr_37B._linearVelocity += body._invMass * vec3;
body._angularVelocity += body._invI * Vec2.Cross(vec, vec3);
Body expr_3B5 = body2;
expr_3B5._linearVelocity += body2._invMass * vec4;
body2._angularVelocity += body2._invI * Vec2.Cross(vec2, vec4);
}
else
{
this._impulse = 0f;
this._limitImpulse1 = 0f;
this._limitImpulse2 = 0f;
}
}
public override Vec2 GetVertex(int index)
{
//B2_NOT_USED(index);
Box2DXDebug.Assert(index == 0);
return(_p);
}
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 * this._a1 + invI2 * this._s2 * this._a2;
float y = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;
this._K.Col1.Set(x, num);
this._K.Col2.Set(num, y);
if (this._enableLimit)
{
float num2 = Vec2.Dot(this._axis, vec2);
if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
{
this._limitState = LimitState.EqualLimits;
}
else
{
if (num2 <= this._lowerTranslation)
{
if (this._limitState != LimitState.AtLowerLimit)
{
this._limitState = LimitState.AtLowerLimit;
this._impulse.Y = 0f;
}
}
else
{
if (num2 >= this._upperTranslation)
{
if (this._limitState != LimitState.AtUpperLimit)
{
this._limitState = LimitState.AtUpperLimit;
this._impulse.Y = 0f;
}
}
else
{
this._limitState = LimitState.InactiveLimit;
this._impulse.Y = 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.Y) * this._axis;
float num3 = this._impulse.X * this._s1 + (this._motorImpulse + this._impulse.Y) * this._a1;
float num4 = this._impulse.X * this._s2 + (this._motorImpulse + this._impulse.Y) * this._a2;
Body expr_457 = body;
expr_457._linearVelocity -= this._invMass1 * v2;
body._angularVelocity -= this._invI1 * num3;
Body expr_48B = body2;
expr_48B._linearVelocity += this._invMass2 * v2;
body2._angularVelocity += this._invI2 * num4;
}
else
{
this._impulse.SetZero();
this._motorImpulse = 0f;
}
}
public PolygonContact(Shape s1, Shape s2) : base(s1, s2)
{
Box2DXDebug.Assert(this._shape1.GetType() == ShapeType.PolygonShape);
Box2DXDebug.Assert(this._shape2.GetType() == ShapeType.PolygonShape);
this._manifold.PointCount = 0;
}
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 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;
}
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 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 void Dispose()
{
Box2DXDebug.Assert(_world._lock == false);
// shapes and joints are destroyed in World.Destroy
}
public override void ComputeMass(out MassData massData, float density)
{
// 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 >= 2);
// A line segment has zero mass.
if (VertexCount == 2)
{
massData.Center = 0.5f * (Vertices[0] + Vertices[1]);
massData.Mass = 0.0f;
massData.I = 0.0f;
return;
}
Vec2 center = new Vec2(); center.Set(0.0f, 0.0f);
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).
Vec2 pRef = new Vec2(0.0f, 0.0f);
#if O
// This code would put the reference point inside the polygon.
for (int i = 0; i < _vertexCount; ++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.
Vec2 p1 = pRef;
Vec2 p2 = Vertices[i];
Vec2 p3 = i + 1 < VertexCount ? Vertices[i + 1] : Vertices[0];
Vec2 e1 = p2 - p1;
Vec2 e2 = p3 - p1;
float D = Vec2.Cross(e1, 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 = density * area;
// Center of mass
Box2DXDebug.Assert(area > Settings.FLT_EPSILON);
center *= 1.0f / area;
massData.Center = center;
// Inertia tensor relative to the local origin.
massData.I = density * I;
}
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;
}
/// <summary>
/// Get a vertex by index.
/// </summary>
public override Vec2 GetVertex(int index)
{
Box2DXDebug.Assert(0 <= index && index < VertexCount);
return(Vertices[index]);
}
public virtual void Dispose()
{
Box2DXDebug.Assert(_proxyId == PairManager.NullProxy);
Box2DXDebug.Assert(_shape == null);
}
// 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 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 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();
}
}
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);
}
// 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 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);
}
internal bool _useGravity; // STEVE Added
internal Body(BodyDef bd, World world)
{
Box2DXDebug.Assert(!world._lock);
this._flags = (Body.BodyFlags) 0;
if (bd.IsBullet)
{
this._flags |= Body.BodyFlags.Bullet;
}
if (bd.FixedRotation)
{
this._flags |= Body.BodyFlags.FixedRotation;
}
if (bd.AllowSleep)
{
this._flags |= Body.BodyFlags.AllowSleep;
}
if (bd.IsSleeping)
{
this._flags |= Body.BodyFlags.Sleep;
}
this._world = world;
this._xf.Position = bd.Position;
this._xf.R.Set(bd.Angle);
this._sweep.LocalCenter = bd.MassData.Center;
this._sweep.T0 = 1f;
this._sweep.A0 = (this._sweep.A = bd.Angle);
this._sweep.C0 = (this._sweep.C = Box2DX.Common.Math.Mul(this._xf, this._sweep.LocalCenter));
this._jointList = null;
this._contactList = null;
this._prev = null;
this._next = null;
this._linearDamping = bd.LinearDamping;
this._angularDamping = bd.AngularDamping;
this._force.Set(0f, 0f);
this._torque = 0f;
this._linearVelocity.SetZero();
this._angularVelocity = 0f;
this._sleepTime = 0f;
this._invMass = 0f;
this._I = 0f;
this._invI = 0f;
this._mass = bd.MassData.Mass;
if (this._mass > 0f)
{
this._invMass = 1f / this._mass;
}
if ((this._flags & Body.BodyFlags.FixedRotation) == (Body.BodyFlags) 0)
{
this._I = bd.MassData.I;
}
if (this._I > 0f)
{
this._invI = 1f / this._I;
}
if (this._invMass == 0f && this._invI == 0f)
{
this._type = Body.BodyType.Static;
}
else
{
this._type = Body.BodyType.Dynamic;
}
this._userData = bd.UserData;
this._shapeList = null;
this._shapeCount = 0;
this._gravity = _world.Gravity; // STEVE Added
this._useGravity = false; // STEVE Added
}
