| public static Clamp ( double value, double min, double max ) : double | ||
| value | double | |
| min | double | |
| max | double | |
| return | double | |
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);
}
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false)
{
float Cdot = wB - wA - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = data.step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = -_mass.Solve33(Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y);
Vector2 reduced = _mass.Solve22(rhs);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
else
{
_impulse += impulse;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y);
Vector2 reduced = _mass.Solve22(rhs);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
else
{
_impulse += impulse;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z);
}
else
{
// Solve point-to-point constraint
Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(_rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(_rB, impulse);
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b2 = BodyB;
Vector2 v1 = Vector2.Zero;
float w1 = 0.0f;
Vector2 v2 = b2.LinearVelocityInternal;
float w2 = b2.AngularVelocityInternal;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorForce;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
float Cdot1 = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector2 Cdot = new Vector2(Cdot1, Cdot2);
Vector2 f1 = _impulse;
Vector2 df = _K.Solve(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Y = Math.Max(_impulse.Y, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Y = 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;
if (_K.col1.X != 0.0f)
{
f2r = b / _K.col1.X + f1.X;
}
else
{
f2r = f1.X;
}
_impulse.X = f2r;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Y * _axis;
float L2 = df.X * _s2 + df.Y * _a2;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
float df;
if (_K.col1.X != 0.0f)
{
df = -Cdot1 / _K.col1.X;
}
else
{
df = 0.0f;
}
_impulse.X += df;
Vector2 P = df * _perp;
float L2 = df * _s2;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
b2.LinearVelocityInternal = v2;
b2.AngularVelocityInternal = w2;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = Complex.Multiply(ref _localXAxis, ref qA);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(ref rB, ref axis);
Vector2 perp = Complex.Multiply(ref _localYAxisA, ref qA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(ref rB, ref perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.X = Vector2.Dot(perp, d);
C1.Y = aB - aA - ReferenceAngle;
float linearError = Math.Abs(C1.X);
float angularError = Math.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (_enableLimit)
{
float translation = Vector2.Dot(axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, Math.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k13 = iA * s1 * a1 + iB * s2 * a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new Vector3(k11, k12, k13);
K.ey = new Vector3(k12, k22, k23);
K.ez = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = K.Solve33(-C);
}
else
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new Vector2(k11, k12);
K.ey = new Vector2(k12, k22);
Vector2 impulse1 = K.Solve(-C1);
impulse = new Vector3();
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * perp + impulse.Z * axis;
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 vA = bA.LinearVelocity;
float wA = bA.AngularVelocityInternal;
Vector2 vB = bB.LinearVelocityInternal;
float wB = bB.AngularVelocityInternal;
// Solve spring constraint
{
float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
_springImpulse += impulse;
Vector2 P = impulse * _ax;
float LA = impulse * _sAx;
float LB = impulse * _sBx;
vA -= InvMassA * P;
wA -= InvIA * LA;
vB += InvMassB * P;
wB += InvIB * LB;
}
// Solve rotational motor constraint
{
float Cdot = wB - wA - _motorSpeed;
float impulse = -_motorMass * Cdot;
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= InvIA * impulse;
wB += InvIB * impulse;
}
// Solve point to line constraint
{
float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
float impulse = _mass * (-Cdot);
_impulse += impulse;
Vector2 P = impulse * _ay;
float LA = impulse * _sAy;
float LB = impulse * _sBy;
vA -= InvMassA * P;
wA -= InvIA * LA;
vB += InvMassB * P;
wB += InvIB * LB;
}
bA.LinearVelocityInternal = vA;
bA.AngularVelocityInternal = wA;
bB.LinearVelocityInternal = vB;
bB.AngularVelocityInternal = wB;
}
private void SolveVelocityConstraints(int start, int end)
{
for (int i = start; i < end; ++i)
{
ContactVelocityConstraint vc = _velocityConstraints[i];
#if NET40 || NET45 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
// find lower order item
int orderedIndexA = vc.indexA;
int orderedIndexB = vc.indexB;
if (orderedIndexB < orderedIndexA)
{
orderedIndexA = vc.indexB;
orderedIndexB = vc.indexA;
}
for (; ;)
{
if (Interlocked.CompareExchange(ref _locks[orderedIndexA], 1, 0) == 0)
{
if (Interlocked.CompareExchange(ref _locks[orderedIndexB], 1, 0) == 0)
{
break;
}
System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0);
}
#if NET40 || NET45 || NETSTANDARD2_0
Thread.Sleep(0);
#endif
}
#endif
int indexA = vc.indexA;
int indexB = vc.indexB;
float mA = vc.invMassA;
float iA = vc.invIA;
float mB = vc.invMassB;
float iB = vc.invIB;
int pointCount = vc.pointCount;
Vector2 vA = _velocities[indexA].v;
float wA = _velocities[indexA].w;
Vector2 vB = _velocities[indexB].v;
float wB = _velocities[indexB].w;
Vector2 normal = vc.normal;
Vector2 tangent = MathUtils.Rot270(ref normal);
float friction = vc.friction;
Debug.Assert(pointCount == 1 || pointCount == 2);
// Solve tangent constraints first because non-penetration is more important
// than friction.
for (int j = 0; j < pointCount; ++j)
{
VelocityConstraintPoint vcp = vc.points[j];
// Relative velocity at contact
Vector2 dv = vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA);
// Compute tangent force
float vt = Vector2.Dot(dv, tangent) - vc.tangentSpeed;
float lambda = vcp.tangentMass * (-vt);
// b2Clamp 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
Vector2 P = lambda * tangent;
vA -= mA * P;
wA -= iA * MathUtils.Cross(ref vcp.rA, ref P);
vB += mB * P;
wB += iB * MathUtils.Cross(ref vcp.rB, ref P);
}
// Solve normal constraints
if (vc.pointCount == 1)
{
VelocityConstraintPoint vcp = vc.points[0];
// Relative velocity at contact
Vector2 dv = vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA);
// Compute normal impulse
float vn = Vector2.Dot(dv, normal);
float lambda = -vcp.normalMass * (vn - vcp.velocityBias);
// b2Clamp the accumulated impulse
float newImpulse = Math.Max(vcp.normalImpulse + lambda, 0.0f);
lambda = newImpulse - vcp.normalImpulse;
vcp.normalImpulse = newImpulse;
// Apply contact impulse
Vector2 P = lambda * normal;
vA -= mA * P;
wA -= iA * MathUtils.Cross(ref vcp.rA, ref P);
vB += mB * P;
wB += iB * MathUtils.Cross(ref vcp.rB, ref P);
}
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 = vn0 - 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;
VelocityConstraintPoint cp1 = vc.points[0];
VelocityConstraintPoint cp2 = vc.points[1];
Vector2 a = new Vector2(cp1.normalImpulse, cp2.normalImpulse);
Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
// Relative velocity at contact
Vector2 dv1 = vB + MathUtils.Cross(wB, ref cp1.rB) - vA - MathUtils.Cross(wA, ref cp1.rA);
Vector2 dv2 = vB + MathUtils.Cross(wB, ref cp2.rB) - vA - MathUtils.Cross(wA, ref cp2.rA);
// Compute normal velocity
float vn1 = Vector2.Dot(dv1, normal);
float vn2 = Vector2.Dot(dv2, normal);
Vector2 b = new Vector2();
b.X = vn1 - cp1.velocityBias;
b.Y = vn2 - cp2.velocityBias;
// Compute b'
b -= MathUtils.Mul(ref vc.K, ref 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 = -MathUtils.Mul(ref vc.normalMass, ref b);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
// Get the incremental impulse
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
// Accumulate
cp1.normalImpulse = x.X;
cp2.normalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
vn2 = Vector2.Dot(dv2, normal);
b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol);
b2Assert(b2Abs(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 = vc.K.ex.Y * x.X + b.Y;
if (x.X >= 0.0f && vn2 >= 0.0f)
{
// Get the incremental impulse
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
// Accumulate
cp1.normalImpulse = x.X;
cp2.normalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 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 = vc.K.ey.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 -= mA * (P1 + P2);
wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref P2));
// Accumulate
cp1.normalImpulse = x.X;
cp2.normalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn2 = Vector2.Dot(dv2, normal);
b2Assert(b2Abs(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 -= mA * (P1 + P2);
wA -= iA * (MathUtils.Cross(ref cp1.rA, ref P1) + MathUtils.Cross(ref cp2.rA, ref P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(ref cp1.rB, ref P1) + MathUtils.Cross(ref cp2.rB, ref 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;
}
}
_velocities[indexA].v = vA;
_velocities[indexA].w = wA;
_velocities[indexB].v = vB;
_velocities[indexB].w = wB;
#if NET40 || NET45 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
System.Threading.Interlocked.Exchange(ref _locks[orderedIndexB], 0);
System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0);
#endif
}
}
// Sequential position solver for position constraints.
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
{
float minSeparation = 0.0f;
for (int i = 0; i < _count; ++i)
{
ContactPositionConstraint pc = _positionConstraints[i];
int indexA = pc.indexA;
int indexB = pc.indexB;
Vector2 localCenterA = pc.localCenterA;
Vector2 localCenterB = pc.localCenterB;
int pointCount = pc.pointCount;
float mA = 0.0f;
float iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc.invMassA;
iA = pc.invIA;
}
float mB = 0.0f;
float iB = 0.0f;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc.invMassB;
iB = pc.invIB;
}
Vector2 cA = _positions[indexA].c;
float aA = _positions[indexA].a;
Vector2 cB = _positions[indexB].c;
float aB = _positions[indexB].a;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j)
{
Transform xfA = new Transform(Vector2.Zero, aA);
Transform xfB = new Transform(Vector2.Zero, aB);
xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q);
xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q);
Vector2 normal;
Vector2 point;
float separation;
PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation);
Vector2 rA = point - cA;
Vector2 rB = point - cB;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = MathUtils.Cross(ref rA, ref normal);
float rnB = MathUtils.Cross(ref rB, ref normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
Vector2 P = impulse * normal;
cA -= mA * P;
aA -= iA * MathUtils.Cross(ref rA, ref P);
cB += mB * P;
aB += iB * MathUtils.Cross(ref rB, ref P);
}
_positions[indexA].c = cA;
_positions[indexA].a = aA;
_positions[indexB].c = cB;
_positions[indexB].a = aB;
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
protected virtual float PlaceRoom(List <RoomDef> Collision, RoomDef Prev, RoomDef Next, float Angle)
{
LastSpace = null;
if (Next == null)
{
Log.Error("Null next room");
return(-1);
}
Angle %= 360f;
if (Angle < 0)
{
Angle += 360f;
}
var PrevCenter = new Vector2((Prev.Left + Prev.Right) / 2f, (Prev.Top + Prev.Bottom) / 2f);
var M = Math.Tan(Angle / A + Math.PI / 2.0);
var B = PrevCenter.Y - M * PrevCenter.X;
Vector2 Start;
RoomDef.Connection Direction;
if (Math.Abs(M) >= 1)
{
if (Angle < 90 || Angle > 270)
{
Direction = RoomDef.Connection.Top;
Start = new Vector2((int)Math.Round((Prev.Top - B) / M), Prev.Top);
}
else
{
Direction = RoomDef.Connection.Bottom;
Start = new Vector2((int)Math.Round((Prev.Bottom - B) / M), Prev.Bottom);
}
}
else
{
if (Angle < 180)
{
Direction = RoomDef.Connection.Right;
Start = new Vector2(Prev.Right, (int)Math.Round(M * Prev.Right + B));
}
else
{
Direction = RoomDef.Connection.Left;
Start = new Vector2(Prev.Left, (int)Math.Round(M * Prev.Left + B));
}
}
if (Direction == RoomDef.Connection.Top || Direction == RoomDef.Connection.Bottom)
{
Start.X = (int)MathUtils.Clamp(Prev.Left + 1, Prev.Right - 1, Start.X);
}
else
{
Start.Y = (int)MathUtils.Clamp(Prev.Top + 1, Prev.Bottom - 1, Start.Y);
}
var Space = LastSpace = FindFreeSpace(Start, Collision, Math.Max(Next.GetMaxWidth(), Next.GetMaxHeight()));
if (!Next.SetSizeWithLimit(Space.GetWidth() + 1, Space.GetHeight() + 1))
{
return(-1);
}
var TargetCenter = new Dot();
if (Direction == RoomDef.Connection.Top)
{
TargetCenter.Y = (int)(Prev.Top - (Next.GetHeight() - 1) / 2f);
TargetCenter.X = (int)((TargetCenter.Y - B) / M);
Next.SetPos((int)Math.Round(TargetCenter.X - (Next.GetWidth() - 1) / 2f), Prev.Top - (Next.GetHeight() - 1));
}
else if (Direction == RoomDef.Connection.Bottom)
{
TargetCenter.Y = (int)(Prev.Bottom + (Next.GetHeight() - 1) / 2f);
TargetCenter.X = (int)((TargetCenter.Y - B) / M);
Next.SetPos((int)Math.Round(TargetCenter.X - (Next.GetWidth() - 1) / 2f), Prev.Bottom);
}
else if (Direction == RoomDef.Connection.Right)
{
TargetCenter.X = (int)(Prev.Right + (Next.GetWidth() - 1) / 2f);
TargetCenter.Y = (int)(M * TargetCenter.X + B);
Next.SetPos(Prev.Right, (int)Math.Round(TargetCenter.Y - (Next.GetHeight() - 1) / 2f));
}
else if (Direction == RoomDef.Connection.Left)
{
TargetCenter.X = (int)(Prev.Left - (Next.GetWidth() - 1) / 2f);
TargetCenter.Y = (int)(M * TargetCenter.X + B);
Next.SetPos(Prev.Left - (Next.GetWidth() - 1), (int)Math.Round(TargetCenter.Y - (Next.GetHeight() - 1) / 2f));
}
if (Direction == RoomDef.Connection.Top || Direction == RoomDef.Connection.Bottom)
{
if (Next.Right < Prev.Left + 2)
{
Next.Shift(Prev.Left + 2 - Next.Right, 0);
}
else if (Next.Left > Prev.Right - 2)
{
Next.Shift(Prev.Right - 2 - Next.Left, 0);
}
if (Next.Right > Space.Right)
{
Next.Shift(Space.Right - Next.Right, 0);
}
else if (Next.Left < Space.Left)
{
Next.Shift(Space.Left - Next.Left, 0);
}
}
else
{
if (Next.Bottom < Prev.Top + 2)
{
Next.Shift(0, Prev.Top + 2 - Next.Bottom);
}
else if (Next.Top > Prev.Bottom - 2)
{
Next.Shift(0, Prev.Bottom - 2 - Next.Top);
}
if (Next.Bottom > Space.Bottom)
{
Next.Shift(0, Space.Bottom - Next.Bottom);
}
else if (Next.Top < Space.Top)
{
Next.Shift(0, Space.Top - Next.Top);
}
}
if (Next.ConnectWithRoom(Prev))
{
if (Next is ConnectionRoom || Next is BossRoom || Next is ShopRoom)
{
Next.Id = Prev.Id;
}
else
{
Next.Id = Prev.Id + 1;
}
return(AngleBetweenRooms(Prev, Next));
}
return(-1);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = Vector2.zero;
const float w2 = 0;
float m1 = b1.InvMass;
float i1 = b1.InvI;
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
// Solve point-to-point constraint
Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.x;
impulse.Y = reduced.y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.x;
_impulse.Y += reduced.y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.x;
impulse.Y = reduced.y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.x;
_impulse.Y += reduced.y;
_impulse.Z = 0.0f;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
v1 -= m1 * P;
w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z);
}
else
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
// Solve point-to-point constraint
Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.x;
_impulse.Y += impulse.y;
v1 -= m1 * impulse;
w1 -= i1 * MathUtils.Cross(r1, impulse);
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
}
public void Solve(ref TimeStep step, ref Vector2 gravity)
{
GGame.Math.Fix64 h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
Vector2 c = b._sweep.C;
GGame.Math.Fix64 a = b._sweep.A;
Vector2 v = b._linearVelocity;
GGame.Math.Fix64 w = b._angularVelocity;
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
if (b.BodyType == BodyType.Dynamic)
{
// Integrate velocities.
//Velcro: Only apply gravity if the body wants it.
if (b.IgnoreGravity)
{
v += h * (b._invMass * b._force);
}
else
{
v += h * (b.GravityScale * gravity + b._invMass * b._force);
}
w += h * b._invI * b._torque;
// 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
v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
}
Positions[i].C = c;
Positions[i].A = a;
Velocities[i].V = v;
Velocities[i].W = w;
}
// Solver data
SolverData solverData = new SolverData();
solverData.Step = step;
solverData.Positions = Positions;
solverData.Velocities = Velocities;
_contactSolver.Reset(step, ContactCount, _contacts, Positions, Velocities);
_contactSolver.InitializeVelocityConstraints();
if (Settings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref solverData);
}
}
if (Settings.EnableDiagnostics)
{
_watch.Stop();
}
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
joint.SolveVelocityConstraints(ref solverData);
joint.Validate(step.inv_dt);
if (Settings.EnableDiagnostics)
{
_watch.Stop();
}
}
_contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting.
_contactSolver.StoreImpulses();
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
Vector2 c = Positions[i].C;
GGame.Math.Fix64 a = Positions[i].A;
Vector2 v = Velocities[i].V;
GGame.Math.Fix64 w = Velocities[i].W;
// Check for large velocities
Vector2 translation = h * v;
if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
GGame.Math.Fix64 ratio = Settings.MaxTranslation / translation.Length();
v *= ratio;
}
GGame.Math.Fix64 rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
GGame.Math.Fix64 ratio = Settings.MaxRotation / Fix64.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
Positions[i].C = c;
Positions[i].A = a;
Velocities[i].V = v;
Velocities[i].W = w;
}
// Solve position constraints
bool positionSolved = false;
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
bool jointOkay = joint.SolvePositionConstraints(ref solverData);
if (Settings.EnableDiagnostics)
{
_watch.Stop();
}
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
if (Settings.EnableDiagnostics)
{
JointUpdateTime = _watch.ElapsedTicks;
_watch.Reset();
}
// Copy state buffers back to the bodies
for (int i = 0; i < BodyCount; ++i)
{
Body body = Bodies[i];
body._sweep.C = Positions[i].C;
body._sweep.A = Positions[i].A;
body._linearVelocity = Velocities[i].V;
body._angularVelocity = Velocities[i].W;
body.SynchronizeTransform();
}
Report(_contactSolver.VelocityConstraints);
if (Settings.AllowSleep)
{
GGame.Math.Fix64 minSleepTime = Settings.MaxFloat;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += h;
minSleepTime = Math.Min((float)minSleepTime, (float)b.SleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep && positionSolved)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
public bool SolvePositionConstraints()
{
float minSeparation = 0.0f;
for (int i = 0; i < _count; ++i)
{
ContactPositionConstraint pc = _positionConstraints[i];
int indexA = pc.IndexA;
int indexB = pc.IndexB;
Vector2 localCenterA = pc.LocalCenterA;
float mA = pc.InvMassA;
float iA = pc.InvIA;
Vector2 localCenterB = pc.LocalCenterB;
float mB = pc.InvMassB;
float iB = pc.InvIB;
int pointCount = pc.PointCount;
Vector2 cA = _positions[indexA].C;
float aA = _positions[indexA].A;
Vector2 cB = _positions[indexB].C;
float aB = _positions[indexB].A;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j)
{
Transform xfA = new Transform();
Transform xfB = new Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - MathUtils.Mul(xfA.q, localCenterA);
xfB.p = cB - MathUtils.Mul(xfB.q, localCenterB);
PositionSolverManifold.Initialize(pc, xfA, xfB, j, out Vector2 normal, out Vector2 point, out float separation);
Vector2 rA = point - cA;
Vector2 rB = point - cB;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = MathUtils.Cross(rA, normal);
float rnB = MathUtils.Cross(rB, normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
Vector2 P = impulse * normal;
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
_positions[indexA].C = cA;
_positions[indexA].A = aA;
_positions[indexB].C = cB;
_positions[indexB].A = aB;
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -3.0f * Settings.LinearSlop);
}
public void Update(float dt)
{
var dialog = m_game.Screen.ModalDialog;
var allowButtonInput = (dialog == null) || (dialog is DialogBox && !((DialogBox)dialog).BlockInput);
if (AllowUserRotate)
{
// Rotate camera
if (m_game.Mouse.Buttons[MouseButton.Right].Held)
{
m_game.Screen.InputMethod = InputMethod.Mouse;
Yaw += 12.0f * ((float)m_game.Mouse.DX / (float)m_game.Window.Width) * m_game.Camera.FOV;
Pitch += 6.0f * ((float)m_game.Mouse.DY / (float)m_game.Window.Height) * m_game.Camera.FOV;
}
if (m_game.ActiveSteamController != null)
{
var joystick = m_game.ActiveSteamController.Joysticks[SteamControllerJoystick.InGameCamera.GetID()];
var dx = joystick.X;
var dy = joystick.Y;
if (dx != 0.0f || dy != 0.0f)
{
m_game.Screen.InputMethod = InputMethod.SteamController;
Yaw = Yaw - 2.0f * dx * dt;
Pitch = Pitch + 2.0f * dy * dt;
}
}
if (m_game.ActiveGamepad != null)
{
var joystick = m_game.ActiveGamepad.Joysticks[GamepadJoystick.Right];
var dx = joystick.X;
var dy = joystick.Y;
if (dx != 0.0f || dy != 0.0f)
{
m_game.Screen.InputMethod = InputMethod.Gamepad;
Yaw = Yaw - 2.0f * dx * dt;
Pitch = Pitch - 2.0f * dy * dt;
}
}
// Clamp to allowed limits
if (AllowUserInvert)
{
Pitch = Math.Min(Math.Max(
Pitch,
MathHelper.DegreesToRadians(-85.0f)),
MathHelper.DegreesToRadians(85.0f)
);
}
else
{
Pitch = Math.Min(Math.Max(
Pitch,
MathHelper.DegreesToRadians(30.0f)),
MathHelper.DegreesToRadians(85.0f)
);
}
}
if (AllowUserZoom)
{
// Zoom camera
if (m_game.Mouse.Wheel != 0)
{
m_game.Screen.InputMethod = InputMethod.Mouse;
TargetDistance -= m_game.Mouse.Wheel * 2.0f;
}
if (allowButtonInput && m_game.ActiveGamepad != null)
{
if (m_game.ActiveGamepad.Buttons[GamepadButton.Down].Held)
{
m_game.Screen.InputMethod = InputMethod.Gamepad;
TargetDistance += 16.0f * dt;
}
if (m_game.ActiveGamepad.Buttons[GamepadButton.Up].Held)
{
m_game.Screen.InputMethod = InputMethod.Gamepad;
TargetDistance -= 16.0f * dt;
}
}
// Clamp to allowed limits
TargetDistance = MathUtils.Clamp(TargetDistance, 12.0f, 30.0f);
}
if (Distance < TargetDistance)
{
Distance = Math.Min(Distance + 48.0f * dt, TargetDistance);
}
else if (Distance > TargetDistance)
{
Distance = Math.Max(Distance - 48.0f * dt, TargetDistance);
}
if (AllowUserPan)
{
// Scrolling
var margin = 10.0f;
var autoScrollSpeed = 10.0f;
var forward = new Vector3(
-(float)Math.Cos(Yaw),
0.0f,
-(float)Math.Sin(Yaw)
);
var right = new Vector3(
(float)Math.Sin(Yaw),
0.0f,
-(float)Math.Cos(Yaw)
);
var scroll = Vector3.Zero;
var distance = margin;
if (m_game.Cursor.Position.X < margin)
{
scroll -= right;
distance = Math.Min(m_game.Cursor.Position.X, margin);
}
else if (m_game.Cursor.Position.X >= m_game.Screen.Width - margin)
{
scroll += right;
distance = Math.Min(m_game.Screen.Width - m_game.Cursor.Position.X, margin);
}
if (m_game.Cursor.Position.Y < margin)
{
scroll += forward;
distance = Math.Min(m_game.Cursor.Position.Y, margin);
}
else if (m_game.Cursor.Position.Y >= m_game.Screen.Height - margin)
{
scroll -= forward;
distance = Math.Min(m_game.Screen.Height - m_game.Cursor.Position.Y, margin);
}
if (scroll.LengthSquared > 0.0f)
{
scroll.Normalize();
Focus += scroll * dt * (1.0f - (distance / margin)) * autoScrollSpeed;
ApplyBounds();
}
}
}
public static float Angle(Vec3 from, Vec3 to)
{
return(MathUtils.Acos(MathUtils.Clamp(Dot(Normalize(@from), Normalize(to)), -1, 1)) * MathUtils.RAD_TO_DEG);
}
// Sequential position solver for position constraints.
public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
{
float minSeparation = 0.0f;
for (int i = 0; i < Count; ++i)
{
ContactPositionConstraint pc = PositionConstraints[i];
int indexA = pc.IndexA;
int indexB = pc.IndexB;
Vec2 localCenterA = pc.LocalCenterA;
Vec2 localCenterB = pc.LocalCenterB;
int pointCount = pc.PointCount;
float mA = 0.0f;
float iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc.InvMassA;
iA = pc.InvIA;
}
float mB = pc.InvMassB;
float iB = pc.InvIB;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc.InvMassB;
iB = pc.InvIB;
}
Vec2 cA = Positions[indexA].C;
float aA = Positions[indexA].A;
Vec2 cB = Positions[indexB].C;
float aB = Positions[indexB].A;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j)
{
xfA.Q.Set(aA);
xfB.Q.Set(aB);
Rot.MulToOutUnsafe(xfA.Q, localCenterA, xfA.P);
xfA.P.NegateLocal().AddLocal(cA);
Rot.MulToOutUnsafe(xfB.Q, localCenterB, xfB.P);
xfB.P.NegateLocal().AddLocal(cB);
PositionSolverManifold psm = psolver;
psm.Initialize(pc, xfA, xfB, j);
Vec2 normal = psm.Normal;
Vec2 point = psm.Point;
float separation = psm.Separation;
rA.Set(point).SubLocal(cA);
rB.Set(point).SubLocal(cB);
// Track max constraint error.
minSeparation = MathUtils.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(Settings.TOI_BAUGARTE * (separation + Settings.LINEAR_SLOP), -Settings.MAX_LINEAR_CORRECTION, 0.0f);
// Compute the effective mass.
float rnA = Vec2.Cross(rA, normal);
float rnB = Vec2.Cross(rB, normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? (-C) / K : 0.0f;
P.Set(normal).MulLocal(impulse);
cA.SubLocal(temp.Set(P).MulLocal(mA));
aA -= iA * Vec2.Cross(rA, P);
cB.AddLocal(temp.Set(P).MulLocal(mB));
aB += iB * Vec2.Cross(rB, P);
}
Positions[indexA].C.Set(cA);
Positions[indexA].A = aA;
Positions[indexB].C.Set(cB);
Positions[indexB].A = aB;
}
// We can't expect minSpeparation >= -_linearSlop because we don't
// push the separation above -_linearSlop.
return(minSeparation >= (-1.5f) * Settings.LINEAR_SLOP);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bB = BodyB;
Vector2 v1 = Vector2.Zero;
float w1 = 0.0f;
Vector2 v2 = bB.LinearVelocityInternal;
float w2 = bB.AngularVelocityInternal;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = MotorForce;
float maxImpulse = step.dt * _maxMotorForce;
MotorForce = MathUtils.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse);
impulse = MotorForce - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float L2 = df.X * _s2 + df.Y + df.Z * _a2;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float L2 = df.X * _s2 + df.Y;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
bB.LinearVelocityInternal = v2;
bB.AngularVelocityInternal = w2;
}
internal override bool SolvePositionConstraints()
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = BodyA;
float angularError = 0.0f;
float positionError;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
float angle = 0 - b1.Sweep.A - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1.Sweep.A -= b1.InvI * limitImpulse;
b1.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
Vector2 C = Vector2.zero + r2 - b1.Sweep.C - r1;
positionError = C.magnitude;
float invMass1 = b1.InvMass;
const float invMass2 = 0;
float invI1 = b1.InvI;
const float invI2 = 0;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C;
u.Normalize();
float k = invMass1 + invMass2;
Debug.Assert(k > Settings.Epsilon);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
const float k_beta = 0.5f;
b1.Sweep.C -= k_beta * invMass1 * impulse2;
C = Vector2.zero + r2 - b1.Sweep.C - r1;
}
Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
Mat22 K2 = new Mat22(new Vector2(invI1 * r1.y * r1.y, -invI1 * r1.x * r1.y),
new Vector2(-invI1 * r1.x * r1.y, invI1 * r1.x * r1.x));
Mat22 K3 = new Mat22(new Vector2(invI2 * r2.y * r2.y, -invI2 * r2.x * r2.y),
new Vector2(-invI2 * r2.x * r2.y, invI2 * r2.x * r2.x));
Mat22 Ka;
Mat22.Add(ref K1, ref K2, out Ka);
Mat22 K;
Mat22.Add(ref Ka, ref K3, out K);
Vector2 impulse = K.Solve(-C);
b1.Sweep.C -= b1.InvMass * impulse;
b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse);
b1.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override bool SolvePositionConstraints()
{
//Body b1 = BodyA;
Body b2 = BodyB;
Vector2 c1 = Vector2.Zero; // b1._sweep.Center;
float a1 = 0.0f; // b1._sweep.Angle;
Vector2 c2 = b2.Sweep.C;
float a2 = b2.Sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.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 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector3 impulse;
Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle);
linearError = Math.Max(linearError, Math.Abs(C1.X));
float angularError = Math.Abs(C1.Y);
if (active)
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
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);
Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
impulse = _K.Solve33(C); // negated above
}
else
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k22 = i1 + i2;
_K.Col1 = new Vector3(k11, k12, 0.0f);
_K.Col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Z * _axis;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c2 += InvMassB * P;
a2 += InvIB * L2;
// TODO_ERIN remove need for this.
b2.Sweep.C = c2;
b2.Sweep.A = a2;
b2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
public void Solve(ref TimeStep step, ref Vector2 gravity)
{
// Integrate velocities and apply damping.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType != BodyType.Dynamic)
{
continue;
}
// Integrate velocities.
// FPE 3 only - Only apply gravity if the body wants it.
if (b.IgnoreGravity)
{
b.LinearVelocityInternal.X += step.dt * (b.InvMass * b.Force.X);
b.LinearVelocityInternal.Y += step.dt * (b.InvMass * b.Force.Y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
else
{
b.LinearVelocityInternal.X += step.dt * (gravity.X + b.InvMass * b.Force.X);
b.LinearVelocityInternal.Y += step.dt * (gravity.Y + b.InvMass * b.Force.Y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
// 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.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f);
b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.AngularDamping, 0.0f, 1.0f);
}
// Partition contacts so that contacts with static bodies are solved last.
int i1 = -1;
for (int i2 = 0; i2 < ContactCount; ++i2)
{
Fixture fixtureA = _contacts[i2].FixtureA;
Fixture fixtureB = _contacts[i2].FixtureB;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static;
if (nonStatic)
{
++i1;
//TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162
Contact tmp = _contacts[i1];
_contacts[i1] = _contacts[i2];
_contacts[i2] = tmp;
}
}
// Initialize velocity constraints.
_contactSolver.Reset(_contacts, ContactCount, step.dtRatio, Settings.EnableWarmstarting);
_contactSolver.InitializeVelocityConstraints();
if (Settings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
_tmpTime = 0;
}
#endif
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref step);
}
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_tmpTime += _watch.ElapsedTicks;
}
#endif
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref step);
joint.Validate(step.inv_dt);
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
_contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
_contactSolver.StoreImpulses();
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
// Check for large velocities.
float translationX = step.dt * b.LinearVelocityInternal.X;
float translationY = step.dt * b.LinearVelocityInternal.Y;
float result = translationX * translationX + translationY * translationY;
if (result > Settings.MaxTranslationSquared)
{
float sq = (float)Math.Sqrt(result);
float ratio = Settings.MaxTranslation / sq;
b.LinearVelocityInternal.X *= ratio;
b.LinearVelocityInternal.Y *= ratio;
}
float rotation = step.dt * b.AngularVelocityInternal;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MaxRotation / Math.Abs(rotation);
b.AngularVelocityInternal *= ratio;
}
// Store positions for continuous collision.
b.Sweep.C0.X = b.Sweep.C.X;
b.Sweep.C0.Y = b.Sweep.C.Y;
b.Sweep.A0 = b.Sweep.A;
// Integrate
b.Sweep.C.X += step.dt * b.LinearVelocityInternal.X;
b.Sweep.C.Y += step.dt * b.LinearVelocityInternal.Y;
b.Sweep.A += step.dt * b.AngularVelocityInternal;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints();
jointsOkay = jointsOkay && jointOkay;
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
JointUpdateTime = _tmpTime;
}
#endif
Report(_contactSolver.Constraints);
if (Settings.AllowSleep)
{
float minSleepTime = Settings.MaxFloat;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0 ||
b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr ||
Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += step.dt;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
private bool SolvePositionConstraints(int start, int end)
{
float minSeparation = 0.0f;
for (int i = start; i < end; ++i)
{
ContactPositionConstraint pc = _positionConstraints[i];
#if NET40 || NET45 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
// Find lower order item.
int orderedIndexA = pc.indexA;
int orderedIndexB = pc.indexB;
if (orderedIndexB < orderedIndexA)
{
orderedIndexA = pc.indexB;
orderedIndexB = pc.indexA;
}
// Lock bodies.
for (; ;)
{
if (Interlocked.CompareExchange(ref _locks[orderedIndexA], 1, 0) == 0)
{
if (Interlocked.CompareExchange(ref _locks[orderedIndexB], 1, 0) == 0)
{
break;
}
System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0);
}
#if NET40 || NET45 || NETSTANDARD2_0
Thread.Sleep(0);
#endif
}
#endif
int indexA = pc.indexA;
int indexB = pc.indexB;
Vector2 localCenterA = pc.localCenterA;
float mA = pc.invMassA;
float iA = pc.invIA;
Vector2 localCenterB = pc.localCenterB;
float mB = pc.invMassB;
float iB = pc.invIB;
int pointCount = pc.pointCount;
Vector2 cA = _positions[indexA].c;
float aA = _positions[indexA].a;
Vector2 cB = _positions[indexB].c;
float aB = _positions[indexB].a;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j)
{
Transform xfA = new Transform(Vector2.Zero, aA);
Transform xfB = new Transform(Vector2.Zero, aB);
xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q);
xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q);
Vector2 normal;
Vector2 point;
float separation;
PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation);
Vector2 rA = point - cA;
Vector2 rB = point - cB;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = MathUtils.Cross(ref rA, ref normal);
float rnB = MathUtils.Cross(ref rB, ref normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
Vector2 P = impulse * normal;
cA -= mA * P;
aA -= iA * MathUtils.Cross(ref rA, ref P);
cB += mB * P;
aB += iB * MathUtils.Cross(ref rB, ref P);
}
_positions[indexA].c = cA;
_positions[indexA].a = aA;
_positions[indexB].c = cB;
_positions[indexB].a = aB;
#if NET40 || NET45 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1
// Unlock bodies.
System.Threading.Interlocked.Exchange(ref _locks[orderedIndexB], 0);
System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0);
#endif
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -3.0f * Settings.LinearSlop);
}
public override bool ShadowHit(Ray ray, ref double tmin)
{
if (!shadows || !bbox.Hit(ray))
{
return(false);
}
double ox = ray.O.X;
double oy = ray.O.Y;
double oz = ray.O.Z;
double dx = ray.D.X;
double dy = ray.D.Y;
double dz = ray.D.Z;
double x0 = bbox.x0;
double y0 = bbox.y0;
double z0 = bbox.z0;
double x1 = bbox.x0;
double y1 = bbox.y0;
double z1 = bbox.z0;
double txMin = 0, tyMin = 0, tzMin = 0;
double txMax = 0, tyMax = 0, tzMax = 0;
double a = 1.0 / dx;
if (a >= 0)
{
txMin = (x0 - ox) * a;
txMax = (x1 - ox) * a;
}
else
{
txMin = (x1 - ox) * a;
txMax = (x0 - ox) * a;
}
double b = 1.0 / dy;
if (b >= 0)
{
tyMin = (y0 - oy) * b;
tyMax = (y1 - oy) * b;
}
else
{
txMin = (y1 - oy) * b;
txMax = (y0 - oy) * b;
}
double c = 1.0 / dz;
if (c >= 0)
{
tzMin = (z0 - oz) * c;
tzMax = (z1 - oz) * c;
}
else
{
tzMin = (z1 - oz) * c;
tzMax = (z0 - oz) * c;
}
double t0, t1;
if (txMin > tyMin)
{
t0 = txMax;
}
else
{
t0 = tyMin;
}
if (tzMin > t0)
{
t0 = tzMin;
}
if (txMax < tyMax)
{
t1 = txMax;
}
else
{
t1 = tyMax;
}
if (tzMax < t1)
{
t1 = tzMax;
}
if (t0 > t1)
{
return(false);
}
int ix, iy, iz;
if (bbox.Inside(ray.O))
{
ix = (int)MathUtils.Clamp((ox - x0) * nx / (x1 - x0), 0, nx - 1);
iy = (int)MathUtils.Clamp((oy - y0) * ny / (y1 - y0), 0, ny - 1);
iz = (int)MathUtils.Clamp((oz - z0) * nz / (z1 - z0), 0, nz - 1);
}
else
{
Vec3 p = ray.HitPoint(t0);
ix = (int)MathUtils.Clamp((p.X - x0) * nx / (x1 - x0), 0, nx - 1);
iy = (int)MathUtils.Clamp((p.Y - y0) * ny / (y1 - y0), 0, ny - 1);
iz = (int)MathUtils.Clamp((p.Z - z0) * nz / (z1 - z0), 0, nz - 1);
}
double dtx = (txMax - txMin) / nx;
double dty = (tyMax - tyMin) / ny;
double dtz = (tzMax - tzMin) / nz;
double txNext, tyNext, tzNext;
int ixStep, iyStep, izStep;
int ixStop, iyStop, izStop;
if (dx > 0)
{
txNext = txMin + (ix + 1) * dtx;
ixStep = +1;
ixStop = nx;
}
else
{
txNext = txMin + (nx - ix) * dtx;
ixStep = -1;
ixStop = -1;
}
if (dx == 0.0)
{
txNext = MathUtils.HugeValue;
ixStep = -1;
ixStop = -1;
}
if (dy > 0)
{
tyNext = tyMin + (iy + 1) * dty;
iyStep = +1;
iyStop = ny;
}
else
{
tyNext = tyMin + (ny - iy) * dty;
iyStep = -1;
iyStop = -1;
}
if (dy == 0.0)
{
tyNext = MathUtils.HugeValue;
iyStep = -1;
iyStop = -1;
}
if (dz > 0)
{
tzNext = tzMin + (iz + 1) * dtz;
izStep = +1;
izStop = nz;
}
else
{
tzNext = tzMin + (nz - iz) * dtz;
izStep = -1;
izStop = -1;
}
if (dz == 0.0)
{
tzNext = MathUtils.HugeValue;
izStep = -1;
izStop = -1;
}
while (true)
{
GeometricObject obj = cells[ix + nx * iy + nx * ny * iz];
if (txNext < tyNext && txNext < tzNext)
{
if (obj != null && obj.ShadowHit(ray, ref tmin) && tmin < txNext)
{
material = obj.Material;
return(true);
}
txNext += dtx;
ix += ixStep;
if (ix == ixStop)
{
return(false);
}
}
else
{
if (tyNext < tzNext)
{
if (obj != null && obj.ShadowHit(ray, ref tmin) && tmin < tyNext)
{
material = obj.Material;
return(true);
}
tyNext += dty;
iy += iyStep;
if (iy == iyStop)
{
return(false);
}
}
else
{
if (obj != null && obj.ShadowHit(ray, ref tmin) && tmin < tzNext)
{
material = obj.Material;
return(true);
}
tzNext += dtz;
iz += izStep;
if (iz == izStop)
{
return(false);
}
}
}
}
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = MotorImpulse;
float maxImpulse = data.step.dt * _maxMotorForce;
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = MotorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float LA = impulse * _a1;
float LB = impulse * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
Vector2 Cdot1 = new Vector2();
Cdot1.X = Vector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA;
Cdot1.Y = wB - wA;
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
Cdot2 = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.ez.X, _K.ez.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float LA = df.X * _s1 + df.Y + df.Z * _a1;
float LB = df.X * _s2 + df.Y + df.Z * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float LA = df.X * _s1 + df.Y;
float LB = df.X * _s2 + df.Y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public void SetupCells()
{
Vec3 p0 = MinCoordinates();
Vec3 p1 = MaxCoordinates();
bbox = new BBox(p0, p1);
int numObjects = objects.Count;
double wx = p1.X - p0.X;
double wy = p1.Y - p0.Y;
double wz = p1.Z - p0.Z;
double multiplier = 2.0;
double s = Math.Pow(wx * wy * wz / numObjects, 0.3333333);
nx = (int)(multiplier * wx / s + 1);
ny = (int)(multiplier * wy / s + 1);
nz = (int)(multiplier * wz / s + 1);
int numCells = nx * ny * nz;
cells.Capacity = numObjects;
for (int j = 0; j < numCells; j++)
{
cells.Add(null);
}
List <int> counts = new List <int>(numCells);
for (int j = 0; j < numCells; j++)
{
counts.Add(0);
}
BBox objBbox;
int index;
for (int j = 0; j < numObjects; j++)
{
objBbox = objects[j].BoundingBox;
int ixmin = (int)MathUtils.Clamp((objBbox.x0 - p0.X) * nx / (p1.X - p0.X), 0, nx - 1);
int iymin = (int)MathUtils.Clamp((objBbox.y0 - p0.Y) * ny / (p1.Y - p0.Y), 0, ny - 1);
int izmin = (int)MathUtils.Clamp((objBbox.z0 - p0.Z) * nz / (p1.Z - p0.Z), 0, nz - 1);
int ixmax = (int)MathUtils.Clamp((objBbox.x1 - p0.X) * nx / (p1.X - p0.X), 0, nx - 1);
int iymax = (int)MathUtils.Clamp((objBbox.y1 - p0.Y) * ny / (p1.Y - p0.Y), 0, ny - 1);
int izmax = (int)MathUtils.Clamp((objBbox.z1 - p0.Z) * nz / (p1.Z - p0.Z), 0, nz - 1);
for (int iz = izmin; iz <= izmax; iz++)
{
for (int iy = iymin; iy <= iymax; iy++)
{
for (int ix = ixmin; ix <= ixmax; ix++)
{
index = ix + nx * iy + nx * ny * iz;
if (counts[index] == 0)
{
cells[index] = objects[j];
counts[index] += 1;
}
else
{
if (counts[index] == 1)
{
Compound c = new Compound();
c.AddObject(cells[index]);
c.AddObject(objects[j]);
cells[index] = c;
counts[index] += 1;
}
else
{
if (cells[index] is Compound)
{
((Compound)cells[index]).AddObject(objects[j]);
}
counts[index] += 1;
}
}
}
}
}
}
objects.Clear();
counts.Clear();
}
internal override bool SolvePositionConstraints()
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 s1 = GroundAnchorA;
Vector2 s2 = GroundAnchorB;
float linearError = 0.0f;
if (_state == LimitState.AtUpper)
{
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 p1 = b1.Sweep.C + r1;
Vector2 p2 = b2.Sweep.C + r2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.magnitude;
float length2 = _u2.magnitude;
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.zero;
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.zero;
}
float C = _ant - length1 - Ratio * length2;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_pulleyMass * C;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -Ratio * impulse * _u2;
b1.Sweep.C += b1.InvMass * P1;
b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1);
b2.Sweep.C += b2.InvMass * P2;
b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
if (_limitState1 == LimitState.AtUpper)
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 p1 = b1.Sweep.C + r1;
_u1 = p1 - s1;
float length1 = _u1.magnitude;
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.zero;
}
float C = MaxLengthA - length1;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass1 * C;
Vector2 P1 = -impulse * _u1;
b1.Sweep.C += b1.InvMass * P1;
b1.Sweep.A += b1.InvI * MathUtils.Cross(r1, P1);
b1.SynchronizeTransform();
}
if (_limitState2 == LimitState.AtUpper)
{
Transform xf2;
b2.GetTransform(out xf2);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 p2 = b2.Sweep.C + r2;
_u2 = p2 - s2;
float length2 = _u2.magnitude;
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.zero;
}
float C = MaxLengthB - length2;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass2 * C;
Vector2 P2 = -impulse * _u2;
b2.Sweep.C += b2.InvMass * P2;
b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P2);
b2.SynchronizeTransform();
}
return(linearError < Settings.LinearSlop);
}
public void TestClamp()
{
Assert.AreEqual(0, MathUtils.Clamp(-100, 0, 1000), Delta);
Assert.AreEqual(1, MathUtils.Clamp(1, 0, 2), Delta);
Assert.AreEqual(2, MathUtils.Clamp(100, 0, 2), Delta);
}
public void Solve(ref TimeStep step, ref FVector2 gravity)
{
float h = step.dt;
//GABS:PROFILER
UnityEngine.Profiling.Profiler.BeginSample("Int. Velocities");
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
FVector2 c = b.Sweep.C;
float a = b.Sweep.A;
FVector2 v = b.LinearVelocity;
float w = b.AngularVelocity;
// Store positions for continuous collision.
b.Sweep.C0 = b.Sweep.C;
b.Sweep.A0 = b.Sweep.A;
if (b.BodyType == BodyType.Dynamic)
{
// Integrate velocities.
v += h * (b.GravityScale * gravity + b.InvMass * b.Force);
w += h * b.InvI * b.Torque;
// 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
v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
}
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
}
//GABS:PROFILER
UnityEngine.Profiling.Profiler.EndSample();
//GABS:PROFILER
UnityEngine.Profiling.Profiler.BeginSample("Init. VConstraints");
// Solver data
SolverData solverData = new SolverData();
solverData.step = step;
solverData.positions = _positions;
solverData.velocities = _velocities;
// Initialize velocity constraints.
//b2ContactSolverDef contactSolverDef;
//contactSolverDef.step = step;
//contactSolverDef.contacts = m_contacts;
//contactSolverDef.count = m_contactCount;
//contactSolverDef.positions = m_positions;
//contactSolverDef.velocities = m_velocities;
//contactSolverDef.allocator = m_allocator;
_contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
_contactSolver.InitializeVelocityConstraints();
if (Settings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
_tmpTime = 0;
}
#endif
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref solverData);
}
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_tmpTime += _watch.ElapsedTicks;
}
#endif
//GABS:PROFILER
UnityEngine.Profiling.Profiler.EndSample();
//GABS:PROFILER
UnityEngine.Profiling.Profiler.BeginSample("Solve VConstraints");
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
FarseerJoint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref solverData);
//TODO: Move up before solve?
joint.Validate(step.inv_dt);
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
//GABS:PROFILER
UnityEngine.Profiling.Profiler.BeginSample("_contactSolver");
_contactSolver.SolveVelocityConstraints(); // HIGH
//GABS:PROFILER
UnityEngine.Profiling.Profiler.EndSample();
}
//GABS:PROFILER
UnityEngine.Profiling.Profiler.EndSample();
//GABS:PROFILER
UnityEngine.Profiling.Profiler.BeginSample("Store Impulses");
// Store impulses for warm starting.
_contactSolver.StoreImpulses();
//GABS:PROFILER
UnityEngine.Profiling.Profiler.EndSample();
//GABS:PROFILER
UnityEngine.Profiling.Profiler.BeginSample("Integrate Positions");
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
FVector2 c = _positions[i].c;
float a = _positions[i].a;
FVector2 v = _velocities[i].v;
float w = _velocities[i].w;
// Check for large velocities
FVector2 translation = h * v;
if (FVector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
float ratio = Settings.MaxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MaxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
}
//GABS:PROFILER
UnityEngine.Profiling.Profiler.EndSample();
//GABS:PROFILER
UnityEngine.Profiling.Profiler.BeginSample("Solve PConstraints");
// Solve position constraints
bool positionSolved = false;
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
FarseerJoint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints(ref solverData);
jointsOkay = jointsOkay && jointOkay;
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
#if (!SILVERLIGHT)
if (Settings.EnableDiagnostics)
{
JointUpdateTime = _tmpTime;
}
#endif
//GABS:PROFILER
UnityEngine.Profiling.Profiler.EndSample();
// Copy state buffers back to the bodies
for (int i = 0; i < BodyCount; ++i)
{
Body body = Bodies[i];
body.Sweep.C = _positions[i].c;
body.Sweep.A = _positions[i].a;
body.LinearVelocity = _velocities[i].v;
body.AngularVelocity = _velocities[i].w;
body.SynchronizeTransform();
}
Report(_contactSolver._velocityConstraints);
if (Settings.AllowSleep)
{
float minSleepTime = Settings.MaxFloat;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0 ||
b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr ||
FVector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep && positionSolved)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
public static Vector2 CalculateWind()
{
float t = Engine.Time * 0.1f;
double a = Math.Cos(t) * Math.Sin(t * 1.3f) + Math.Cos(t * 1.5f);
return(new Vector2(MathUtils.Clamp(-MaxSpeed, MaxSpeed, (float)Math.Cos(a)), MathUtils.Clamp(-MaxSpeed, MaxSpeed, (float)Math.Sin(a))));
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float angularError = 0.0f;
float positionError;
bool fixedRotation = (_invIA + _invIB == 0.0f);
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
float angle = aB - aA - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
aA -= _invIA * limitImpulse;
aB += _invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 C = cB + rB - cA - rA;
positionError = C.Length;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Mat22 K = new Mat22();
K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;
Vector2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * MathUtils.Cross(rA, impulse);
cB += mB * impulse;
aB += iB * MathUtils.Cross(rB, impulse);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
// Solve spring constraint
{
float Cdot = Vector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
float impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
_springImpulse += impulse;
Vector2 P = impulse * _ax;
float LA = impulse * _sAx;
float LB = impulse * _sBx;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
// Solve rotational motor constraint
{
float Cdot = wB - wA - _motorSpeed;
float impulse = -_motorMass * Cdot;
float oldImpulse = _motorImpulse;
float maxImpulse = data.step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve point to line constraint
{
float Cdot = Vector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
float impulse = -_mass * Cdot;
_impulse += impulse;
Vector2 P = impulse * _ay;
float LA = impulse * _sAy;
float LB = impulse * _sBy;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints()
{
Body b2 = BodyB;
Vector2 c1 = Vector2.Zero;
float a1 = 0.0f;
Vector2 c2 = b2.Sweep.c;
float a2 = b2.Sweep.a;
// Solve linear limit constraint.
float linearError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= LowerLimit)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - LowerLimit + Settings.LinearSlop, -Settings.MaxLinearCorrection,
0.0f);
linearError = LowerLimit - translation;
active = true;
}
else if (translation >= UpperLimit)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - UpperLimit - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - UpperLimit;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector2 impulse;
float C1 = Vector2.Dot(_perp, d);
linearError = Math.Max(linearError, Math.Abs(C1));
const float angularError = 0.0f;
if (active)
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
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 = new Vector2(k11, k12);
_K.col2 = new Vector2(k12, k22);
Vector2 C = new Vector2(-C1, -C2);
impulse = _K.Solve(C); // note i inverted above
}
else
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float impulse1;
if (k11 != 0.0f)
{
impulse1 = -C1 / k11;
}
else
{
impulse1 = 0.0f;
}
impulse.X = impulse1;
impulse.Y = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Y * _axis;
float L2 = impulse.X * _s2 + impulse.Y * _a2;
c2 += InvMassB * P;
a2 += InvIB * L2;
// TODO_ERIN remove need for this.
b2.Sweep.c = c2;
b2.Sweep.a = a2;
b2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = b2.LinearVelocityInternal;
float w2 = b2.AngularVelocityInternal;
float m1 = b1.InvMass, m2 = b2.InvMass;
float i1 = b1.InvI, i2 = b2.InvI;
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
/*Transform xf1, xf2;
* b1.GetTransform(out xf1);
* b2.GetTransform(out xf2);*/
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
// Solve point-to-point constraint
MathUtils.Cross(w2, ref r2, out _tmpVector2);
MathUtils.Cross(w1, ref r1, out _tmpVector1);
Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
Vector2 rhs = -Cdot1 + _impulse.z * new Vector2(_mass.Col3.x, _mass.Col3.y);
Vector2 reduced = _mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -_impulse.z;
_impulse.x += reduced.x;
_impulse.y += reduced.y;
_impulse.z = 0.0f;
}
else
{
_impulse += impulse;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
Vector2 rhs = -Cdot1 + _impulse.z * new Vector2(_mass.Col3.x, _mass.Col3.y);
Vector2 reduced = _mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -_impulse.z;
_impulse.x += reduced.x;
_impulse.y += reduced.y;
_impulse.z = 0.0f;
}
else
{
_impulse += impulse;
}
}
Vector2 P = new Vector2(impulse.x, impulse.y);
v1 -= m1 * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
w1 -= i1 * (/* r1 x P */ _tmpFloat1 + impulse.z);
v2 += m2 * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
w2 += i2 * (/* r2 x P */ _tmpFloat1 + impulse.z);
}
else
{
/*Transform xf1, xf2;
* b1.GetTransform(out xf1);
* b2.GetTransform(out xf2);*/
_tmpVector1 = LocalAnchorA - b1.LocalCenter;
_tmpVector2 = LocalAnchorB - b2.LocalCenter;
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2);
// Solve point-to-point constraint
MathUtils.Cross(w2, ref r2, out _tmpVector2);
MathUtils.Cross(w1, ref r1, out _tmpVector1);
Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.x += impulse.x;
_impulse.y += impulse.y;
v1 -= m1 * impulse;
MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
w1 -= i1 * /* r1 x impulse */ _tmpFloat1;
v2 += m2 * impulse;
MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
w2 += i2 * /* r2 x impulse */ _tmpFloat1;
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
b2.LinearVelocityInternal = v2;
b2.AngularVelocityInternal = w2;
}