/// <summary>
/// Set the mass properties to override the mass properties of the fixtures. Note that this changes
/// the center of mass position. Note that creating or destroying fixtures can also alter the mass.
/// This function has no effect if the body isn't dynamic.
/// </summary>
/// <param name="massData">the mass properties.</param>
public void SetMassData(MassData massData)
{
// TODO_ERIN adjust linear velocity and torque to account for movement of center.
Debug.Assert(World.Locked == false);
if (World.Locked)
{
return;
}
if (m_type != BodyType.Dynamic)
{
return;
}
InvMass = 0.0f;
I = 0.0f;
InvI = 0.0f;
Mass = massData.Mass;
if (Mass <= 0.0f)
{
Mass = 1f;
}
InvMass = 1.0f / Mass;
if (massData.I > 0.0f && (Flags & TypeFlags.FixedRotation) == 0)
{
I = massData.I - Mass * Vec2.Dot(massData.Center, massData.Center);
Debug.Assert(I > 0.0f);
InvI = 1.0f / I;
}
Vec2 oldCenter = World.Pool.PopVec2();
// Move center of mass.
oldCenter.Set(Sweep.C);
Sweep.LocalCenter.Set(massData.Center);
// m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter);
Transform.MulToOutUnsafe(Xf, Sweep.LocalCenter, Sweep.C0);
Sweep.C.Set(Sweep.C0);
// Update center of mass velocity.
// m_linearVelocity += Cross(m_angularVelocity, m_sweep.c - oldCenter);
Vec2 temp = World.Pool.PopVec2();
temp.Set(Sweep.C).SubLocal(oldCenter);
Vec2.CrossToOut(m_angularVelocity, temp, temp);
m_linearVelocity.AddLocal(temp);
World.Pool.PushVec2(2);
}
public override bool TestPoint(Transform transform, Vec2 p)
{
Vec2 center = pool1;
Rot.MulToOutUnsafe(transform.Q, P, center);
center.AddLocal(transform.P);
Vec2 d = center.SubLocal(p).NegateLocal();
return(Vec2.Dot(d, d) <= Radius * Radius);
}
public void GetWorldToScreen(Vec2 argWorld, Vec2 argScreen)
{
argScreen.Set(argWorld);
argScreen.SubLocal(Box.Center);
Box.R.MulToOut(argScreen, argScreen);
if (YFlip)
{
yFlipMat.MulToOut(argScreen, argScreen);
}
argScreen.AddLocal(Box.Extents);
}
public override void ComputeAABB(AABB aabb, Transform transform, int childIndex)
{
Vec2 p = pool1;
Rot.MulToOutUnsafe(transform.Q, P, p);
p.AddLocal(transform.P);
aabb.LowerBound.X = p.X - Radius;
aabb.LowerBound.Y = p.Y - Radius;
aabb.UpperBound.X = p.X + Radius;
aabb.UpperBound.Y = p.Y + Radius;
}
public void GetScreenToWorld(Vec2 argScreen, Vec2 argWorld)
{
argWorld.Set(argScreen);
argWorld.SubLocal(Box.Extents);
Box.R.InvertToOut(inv2);
inv2.MulToOut(argWorld, argWorld);
if (YFlip)
{
yFlipMatInv.MulToOut(argWorld, argWorld);
}
argWorld.AddLocal(Box.Center);
}
public void WarmStart()
{
// Warm start.
for (int i = 0; i < Count; ++i)
{
ContactVelocityConstraint vc = VelocityConstraints[i];
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;
Vec2 vA = Velocities[indexA].V;
float wA = Velocities[indexA].W;
Vec2 vB = Velocities[indexB].V;
float wB = Velocities[indexB].W;
Vec2 normal = vc.Normal;
Vec2.CrossToOutUnsafe(normal, 1.0f, tangent);
for (int j = 0; j < pointCount; ++j)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
//Console.WriteLine("vcp normal impulse is " + vcp.normalImpulse);
temp.Set(normal).MulLocal(vcp.NormalImpulse);
P.Set(tangent).MulLocal(vcp.TangentImpulse).AddLocal(temp);
wA -= iA * Vec2.Cross(vcp.RA, P);
vA.SubLocal(temp.Set(P).MulLocal(mA));
//Debug.Assert(vA.x == 0);
//Debug.Assert(wA == 0);
wB += iB * Vec2.Cross(vcp.RB, P);
vB.AddLocal(temp.Set(P).MulLocal(mB));
//Debug.Assert(vB.x == 0);
//Debug.Assert(wB == 0);
}
Velocities[indexA].W = wA;
Velocities[indexB].W = wB;
//Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " - " + wA);
//Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " - " + wB);
}
}
public void ComputeCentroidToOut(Vec2[] vs, int count, Vec2 result)
{
Debug.Assert(count >= 3);
result.Set(0.0f, 0.0f);
float area = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
Vec2 pRef = pool1;
pRef.SetZero();
Vec2 e1 = pool2;
Vec2 e2 = pool3;
const float inv3 = 1.0f / 3.0f;
for (int i = 0; i < count; ++i)
{
// Triangle vertices.
Vec2 p1 = pRef;
Vec2 p2 = vs[i];
Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0];
e1.Set(p2).SubLocal(p1);
e2.Set(p3).SubLocal(p1);
float D = Vec2.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
e1.Set(p1).AddLocal(p2).AddLocal(p3).MulLocal(triangleArea * inv3);
result.AddLocal(e1);
}
// Centroid
Debug.Assert(area > Settings.EPSILON);
result.MulLocal(1.0f / area);
}
public void GetWitnessPoints(Vec2 pA, Vec2 pB)
{
switch (Count)
{
case 0:
Debug.Assert(false);
break;
case 1:
pA.Set(m_v1.WA);
pB.Set(m_v1.WB);
break;
case 2:
case2.Set(m_v1.WA).MulLocal(m_v1.A);
pA.Set(m_v2.WA).MulLocal(m_v2.A).AddLocal(case2);
// m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
// *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
case2.Set(m_v1.WB).MulLocal(m_v1.A);
pB.Set(m_v2.WB).MulLocal(m_v2.A).AddLocal(case2);
break;
case 3:
pA.Set(m_v1.WA).MulLocal(m_v1.A);
case3.Set(m_v2.WA).MulLocal(m_v2.A);
case33.Set(m_v3.WA).MulLocal(m_v3.A);
pA.AddLocal(case3).AddLocal(case33);
pB.Set(pA);
// *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
// *pB = *pA;
break;
default:
Debug.Assert(false);
break;
}
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
Vec2 vpA = Pool.PopVec2();
Vec2 vpB = Pool.PopVec2();
// Cdot = dot(u, v + cross(w, r))
Vec2.CrossToOutUnsafe(wA, RA, vpA);
vpA.AddLocal(vA);
Vec2.CrossToOutUnsafe(wB, RB, vpB);
vpB.AddLocal(vB);
float Cdot = Vec2.Dot(U, vpB.SubLocal(vpA));
float impulse = (-Mass) * (Cdot + Bias + Gamma * Impulse);
Impulse += impulse;
float Px = impulse * U.X;
float Py = impulse * U.Y;
vA.X -= InvMassA * Px;
vA.Y -= InvMassA * Py;
wA -= InvIA * (RA.X * Py - RA.Y * Px);
vB.X += InvMassB * Px;
vB.Y += InvMassB * Py;
wB += InvIB * (RB.X * Py - RB.Y * Px);
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(2);
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override bool Raycast(RayCastOutput output, RayCastInput input, Transform transform, int childIndex)
{
Vec2 position = pool1;
Vec2 s = pool2;
Vec2 r = pool3;
Rot.MulToOutUnsafe(transform.Q, P, position);
position.AddLocal(transform.P);
s.Set(input.P1).SubLocal(position);
float b = Vec2.Dot(s, s) - Radius * Radius;
// Solve quadratic equation.
r.Set(input.P2).SubLocal(input.P1);
float c = Vec2.Dot(s, r);
float rr = Vec2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.EPSILON)
{
return(false);
}
// Find the point of intersection of the line with the circle.
float a = -(c + MathUtils.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.MaxFraction * rr)
{
a /= rr;
output.Fraction = a;
output.Normal.Set(r).MulLocal(a);
output.Normal.AddLocal(s);
output.Normal.Normalize();
return(true);
}
return(false);
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
// Cdot = v + cross(w, r)
Vec2 Cdot = Pool.PopVec2();
Vec2.CrossToOutUnsafe(wB, RB, Cdot);
Cdot.AddLocal(vB);
Vec2 impulse = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
temp.Set(m_impulse).MulLocal(m_gamma).AddLocal(m_C).AddLocal(Cdot).NegateLocal();
Mat22.MulToOutUnsafe(m_mass, temp, impulse);
Vec2 oldImpulse = temp;
oldImpulse.Set(m_impulse);
m_impulse.AddLocal(impulse);
float maxImpulse = data.Step.Dt * m_maxForce;
if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_impulse.MulLocal(maxImpulse / m_impulse.Length());
}
impulse.Set(m_impulse).SubLocal(oldImpulse);
vB.X += InvMassB * m_impulse.X;
vB.Y += InvMassB * m_impulse.Y;
wB += InvIB * Vec2.Cross(RB, impulse);
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(3);
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
Vec2 vpA = Pool.PopVec2();
Vec2 vpB = Pool.PopVec2();
Vec2 PA = Pool.PopVec2();
Vec2 PB = Pool.PopVec2();
Vec2.CrossToOutUnsafe(wA, RA, vpA);
vpA.AddLocal(vA);
Vec2.CrossToOutUnsafe(wB, RB, vpB);
vpB.AddLocal(vB);
float Cdot = -Vec2.Dot(m_uA, vpA) - m_ratio * Vec2.Dot(m_uB, vpB);
float impulse = (-m_mass) * Cdot;
m_impulse += impulse;
PA.Set(m_uA).MulLocal(-impulse);
PB.Set(m_uB).MulLocal((-m_ratio) * impulse);
vA.X += InvMassA * PA.X;
vA.Y += InvMassA * PA.Y;
wA += InvIA * Vec2.Cross(RA, PA);
vB.X += InvMassB * PB.X;
vB.Y += InvMassB * PB.Y;
wB += InvIB * Vec2.Cross(RB, PB);
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(4);
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
float h = data.Step.Dt;
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = (-AngularMass) * Cdot;
float oldImpulse = m_angularImpulse;
float maxImpulse = h * m_maxTorque;
m_angularImpulse = MathUtils.Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vec2 Cdot = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
Vec2.CrossToOutUnsafe(wA, RA, temp);
Vec2.CrossToOutUnsafe(wB, RB, Cdot);
Cdot.AddLocal(vB).SubLocal(vA).SubLocal(temp);
Vec2 impulse = Pool.PopVec2();
Mat22.MulToOutUnsafe(LinearMass, Cdot, impulse);
impulse.NegateLocal();
Vec2 oldImpulse = Pool.PopVec2();
oldImpulse.Set(m_linearImpulse);
m_linearImpulse.AddLocal(impulse);
float maxImpulse = h * m_maxForce;
if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_linearImpulse.Normalize();
m_linearImpulse.MulLocal(maxImpulse);
}
impulse.Set(m_linearImpulse).SubLocal(oldImpulse);
temp.Set(impulse).MulLocal(mA);
vA.SubLocal(temp);
wA -= iA * Vec2.Cross(RA, impulse);
temp.Set(impulse).MulLocal(mB);
vB.AddLocal(temp);
wB += iB * Vec2.Cross(RB, impulse);
}
data.Velocities[IndexA].V.Set(vA);
if (data.Velocities[IndexA].W != wA)
{
Debug.Assert(data.Velocities[IndexA].W != wA);
}
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(4);
}
// 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);
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < Count; ++i)
{
ContactVelocityConstraint vc = VelocityConstraints[i];
int indexA = vc.IndexA;
int indexB = vc.IndexB;
float mA = vc.InvMassA;
float mB = vc.InvMassB;
float iA = vc.InvIA;
float iB = vc.InvIB;
int pointCount = vc.PointCount;
Vec2 vA = Velocities[indexA].V;
float wA = Velocities[indexA].W;
Vec2 vB = Velocities[indexB].V;
float wB = Velocities[indexB].W;
//Debug.Assert(wA == 0);
//Debug.Assert(wB == 0);
Vec2 normal = vc.Normal;
//Vec2.crossToOutUnsafe(normal, 1f, tangent);
tangent.X = 1.0f * vc.Normal.Y;
tangent.Y = (-1.0f) * vc.Normal.X;
float friction = vc.Friction;
Debug.Assert(pointCount == 1 || pointCount == 2);
// Solve tangent constraints
for (int j = 0; j < pointCount; ++j)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
//Vec2.crossToOutUnsafe(wA, vcp.rA, temp);
//Vec2.crossToOutUnsafe(wB, vcp.rB, dv);
//dv.addLocal(vB).subLocal(vA).subLocal(temp);
Vec2 a = vcp.RA;
dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a.Y;
dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a.X;
// Compute tangent force
float vt = dv.X * tangent.X + dv.Y * tangent.Y - vc.TangentSpeed;
float lambda = vcp.TangentMass * (-vt);
// Clamp the accumulated force
float maxFriction = friction * vcp.NormalImpulse;
float newImpulse = MathUtils.Clamp(vcp.TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp.TangentImpulse;
vcp.TangentImpulse = newImpulse;
// Apply contact impulse
// Vec2 P = lambda * tangent;
float Px = tangent.X * lambda;
float Py = tangent.Y * lambda;
// vA -= invMassA * P;
vA.X -= Px * mA;
vA.Y -= Py * mA;
wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px);
// vB += invMassB * P;
vB.X += Px * mB;
vB.Y += Py * mB;
wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px);
//Console.WriteLine("tangent solve velocity (point "+j+") for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA);
//Console.WriteLine("tangent solve velocity (point "+j+") for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB);
}
// Solve normal constraints
if (vc.PointCount == 1)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[0];
Vec2 a1 = vcp.RA;
// Relative velocity at contact
//Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA);
//Vec2.crossToOut(wA, vcp.rA, temp1);
//Vec2.crossToOut(wB, vcp.rB, dv);
//dv.addLocal(vB).subLocal(vA).subLocal(temp1);
dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a1.Y;
dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a1.X;
// Compute normal impulse
float vn = dv.X * normal.X + dv.Y * normal.Y;
float lambda = (-vcp.NormalMass) * (vn - vcp.VelocityBias);
// Clamp the accumulated impulse
float a = vcp.NormalImpulse + lambda;
float newImpulse = (a > 0.0f ? a : 0.0f);
lambda = newImpulse - vcp.NormalImpulse;
//Debug.Assert(newImpulse == 0);
vcp.NormalImpulse = newImpulse;
// Apply contact impulse
float Px = normal.X * lambda;
float Py = normal.Y * lambda;
// vA -= invMassA * P;
vA.X -= Px * mA;
vA.Y -= Py * mA;
wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px);
//Debug.Assert(vA.x == 0);
// vB += invMassB * P;
vB.X += Px * mB;
vB.Y += Py * mB;
wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px);
//Debug.Assert(vB.x == 0);
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on
// Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary
// constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D
// contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be
// tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of
// the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the
// impulse variable (x_i).
//
// Substitute:
//
// x = a + d
//
// a := old total impulse
// x := new total impulse
// d := incremental impulse
//
// For the current iteration we extend the formula for the incremental impulse
// to compute the new total impulse:
//
// vn = A * d + b
// = A * (x - a) + b
// = A * x + b - A * a
// = A * x + b'
// b' = b - A * a;
ContactVelocityConstraint.VelocityConstraintPoint cp1 = vc.Points[0];
ContactVelocityConstraint.VelocityConstraintPoint cp2 = vc.Points[1];
a.X = cp1.NormalImpulse;
a.Y = cp2.NormalImpulse;
Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
// Relative velocity at contact
// Vec2 dv1 = vB + Cross(wB, cp1.rB) - vA - Cross(wA, cp1.rA);
dv1.X = (-wB) * cp1.RB.Y + vB.X - vA.X + wA * cp1.RA.Y;
dv1.Y = wB * cp1.RB.X + vB.Y - vA.Y - wA * cp1.RA.X;
// Vec2 dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
dv2.X = (-wB) * cp2.RB.Y + vB.X - vA.X + wA * cp2.RA.Y;
dv2.Y = wB * cp2.RB.X + vB.Y - vA.Y - wA * cp2.RA.X;
// Compute normal velocity
float vn1 = dv1.X * normal.X + dv1.Y * normal.Y;
float vn2 = dv2.X * normal.X + dv2.Y * normal.Y;
b.X = vn1 - cp1.VelocityBias;
b.Y = vn2 - cp2.VelocityBias;
//Console.WriteLine("b is " + b.x + "," + b.y);
// Compute b'
Mat22 R = vc.K;
b.X -= (R.Ex.X * a.X + R.Ey.X * a.Y);
b.Y -= (R.Ex.Y * a.X + R.Ey.Y * a.Y);
//Console.WriteLine("b' is " + b.x + "," + b.y);
// final float k_errorTol = 1e-3f;
// B2_NOT_USED(k_errorTol);
for (; ;)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
// Vec2 x = - Mul(c.normalMass, b);
Mat22.MulToOutUnsafe(vc.NormalMass, b, x);
x.MulLocal(-1);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
//Console.WriteLine("case 1");
// Get the incremental impulse
// Vec2 d = x - a;
d.Set(x).SubLocal(a);
// Apply incremental impulse
// Vec2 P1 = d.x * normal;
// Vec2 P2 = d.y * normal;
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
/*
* vA -= invMassA * (P1 + P2); wA -= invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
* Cross(wA, cp1.rA); dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
*
* // Compute normal velocity vn1 = Dot(dv1, normal); vn2 = Dot(dv2, normal);
*
* Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); Debug.Assert(Abs(vn2 - cp2.velocityBias)
* < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA)));
Vec2 _dv2 = vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA)));
// Compute normal velocity
vn1 = Vec2.Dot(_dv1, normal);
vn2 = Vec2.Dot(_dv2, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I);
Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I);
}
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + '
//
x.X = (-cp1.NormalMass) * b.X;
x.Y = 0.0f;
vn1 = 0.0f;
vn2 = vc.K.Ex.Y * x.X + b.Y;
if (x.X >= 0.0f && vn2 >= 0.0f)
{
//Console.WriteLine("case 2");
// Get the incremental impulse
d.Set(x).SubLocal(a);
// Apply incremental impulse
// Vec2 P1 = d.x * normal;
// Vec2 P2 = d.y * normal;
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
//Debug.Assert(x.x == 0 && x.y == 0);
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
* Cross(wA, cp1.rA);
*
* // Compute normal velocity vn1 = Dot(dv1, normal);
*
* Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA)));
// Compute normal velocity
vn1 = Vec2.Dot(_dv1, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I);
}
break;
}
//
// Case 3: wB = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + '
//
x.X = 0.0f;
x.Y = (-cp2.NormalMass) * b.Y;
vn1 = vc.K.Ey.X * x.Y + b.X;
vn2 = 0.0f;
if (x.Y >= 0.0f && vn1 >= 0.0f)
{
//Console.WriteLine("case 3");
// Resubstitute for the incremental impulse
d.Set(x).SubLocal(a);
// Apply incremental impulse
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
//Debug.Assert(x.x == 0 && x.y == 0);
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + Cross(wB, cp2.rB) - vA -
* Cross(wA, cp2.rA);
*
* // Compute normal velocity vn2 = Dot(dv2, normal);
*
* Debug.Assert(Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 _dv2 =
vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA)));
// Compute normal velocity
vn2 = Vec2.Dot(_dv2, normal);
Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I);
}
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = ;
x.X = 0.0f;
x.Y = 0.0f;
vn1 = b.X;
vn2 = b.Y;
if (vn1 >= 0.0f && vn2 >= 0.0f)
{
//Console.WriteLine("case 4");
// Resubstitute for the incremental impulse
d.Set(x).SubLocal(a);
// Apply incremental impulse
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
//Debug.Assert(x.x == 0 && x.y == 0);
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
Velocities[indexA].V.Set(vA);
Velocities[indexA].W = wA;
Velocities[indexB].V.Set(vB);
Velocities[indexB].W = wB;
//Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA);
//Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB);
}
}
public override void ComputeMass(MassData massData, float density)
{
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
Debug.Assert(VertexCount >= 3);
Vec2 center = pool1;
center.SetZero();
float area = 0.0f;
float I = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
Vec2 s = pool2;
s.SetZero();
// This code would put the reference point inside the polygon.
for (int i = 0; i < VertexCount; ++i)
{
s.AddLocal(Vertices[i]);
}
s.MulLocal(1.0f / VertexCount);
const float k_inv3 = 1.0f / 3.0f;
Vec2 e1 = pool3;
Vec2 e2 = pool4;
for (int i = 0; i < VertexCount; ++i)
{
// Triangle vertices.
e1.Set(Vertices[i]).SubLocal(s);
e2.Set(s).NegateLocal().AddLocal(i + 1 < VertexCount ? Vertices[i + 1] : Vertices[0]);
float D = Vec2.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center.X += triangleArea * k_inv3 * (e1.X + e2.X);
center.Y += triangleArea * k_inv3 * (e1.Y + e2.Y);
float ex1 = e1.X;
float ey1 = e1.Y;
float ex2 = e2.X;
float ey2 = e2.Y;
float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
}
// Total mass
massData.Mass = density * area;
// Center of mass
Debug.Assert(area > Settings.EPSILON);
center.MulLocal(1.0f / area);
massData.Center.Set(center).AddLocal(s);
// Inertia tensor relative to the local origin (point s)
massData.I = I * density;
// Shift to center of mass then to original body origin.
massData.I += massData.Mass * (Vec2.Dot(massData.Center, massData.Center));
}
public void GetScreenToWorld(Vec2 argScreen, Vec2 argWorld)
{
argWorld.Set(argScreen);
argWorld.SubLocal(Box.Extents);
Box.R.InvertToOut(inv2);
inv2.MulToOut(argWorld, argWorld);
if (YFlip)
{
yFlipMatInv.MulToOut(argWorld, argWorld);
}
argWorld.AddLocal(Box.Center);
}
public void GetWorldToScreen(Vec2 argWorld, Vec2 argScreen)
{
argScreen.Set(argWorld);
argScreen.SubLocal(Box.Center);
Box.R.MulToOut(argScreen, argScreen);
if (YFlip)
{
yFlipMat.MulToOut(argScreen, argScreen);
}
argScreen.AddLocal(Box.Extents);
}
/// <seealso cref="Joint.solveVelocityConstraints(TimeStep)"></seealso>
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[m_indexA].V;
float wA = data.Velocities[m_indexA].W;
Vec2 vB = data.Velocities[m_indexB].V;
float wB = data.Velocities[m_indexB].W;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Vec2 Cdot1 = Pool.PopVec2();
Vec2 P = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
if (Frequency > 0.0f)
{
float Cdot2 = wB - wA;
float impulse2 = (-m_mass.Ez.Z) * (Cdot2 + m_bias + m_gamma * m_impulse.Z);
m_impulse.Z += impulse2;
wA -= iA * impulse2;
wB += iB * impulse2;
Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1);
Vec2.CrossToOutUnsafe(wA, m_rA, temp);
Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp);
Vec2 impulse1 = P;
Mat33.Mul22ToOutUnsafe(m_mass, Cdot1, impulse1);
impulse1.NegateLocal();
m_impulse.X += impulse1.X;
m_impulse.Y += impulse1.Y;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * Vec2.Cross(m_rA, P);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * Vec2.Cross(m_rB, P);
}
else
{
Vec2.CrossToOutUnsafe(wA, m_rA, temp);
Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1);
Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp);
float Cdot2 = wB - wA;
Vec3 Cdot = Pool.PopVec3();
Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 impulse = Pool.PopVec3();
Mat33.MulToOutUnsafe(m_mass, Cdot, impulse);
impulse.NegateLocal();
m_impulse.AddLocal(impulse);
P.Set(impulse.X, impulse.Y);
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * (Vec2.Cross(m_rA, P) + impulse.Z);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * (Vec2.Cross(m_rB, P) + impulse.Z);
Pool.PushVec3(2);
}
data.Velocities[m_indexA].V.Set(vA);
data.Velocities[m_indexA].W = wA;
data.Velocities[m_indexB].V.Set(vB);
data.Velocities[m_indexB].W = wB;
Pool.PushVec2(3);
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
Vec2 temp = Pool.PopVec2();
// Solve linear motor constraint.
if (m_motorEnabled && LimitState != LimitState.Equal)
{
temp.Set(vB).SubLocal(vA);
float Cdot = Vec2.Dot(Axis, temp) + A2 * wB - A1 * wA;
float impulse = MotorMass * (m_motorSpeed - Cdot);
float oldImpulse = MotorImpulse;
float maxImpulse = data.Step.Dt * m_maxMotorForce;
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = MotorImpulse - oldImpulse;
Vec2 P = Pool.PopVec2();
P.Set(Axis).MulLocal(impulse);
float LA = impulse * A1;
float LB = impulse * A2;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Pool.PushVec2(1);
}
Vec2 Cdot1 = Pool.PopVec2();
temp.Set(vB).SubLocal(vA);
Cdot1.X = Vec2.Dot(Perp, temp) + S2 * wB - S1 * wA;
Cdot1.Y = wB - wA;
// System.out.println(Cdot1);
if (m_limitEnabled && LimitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
temp.Set(vB).SubLocal(vA);
Cdot2 = Vec2.Dot(Axis, temp) + A2 * wB - A1 * wA;
Vec3 Cdot = Pool.PopVec3();
Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2);
Cdot.NegateLocal();
Vec3 f1 = Pool.PopVec3();
Vec3 df = Pool.PopVec3();
f1.Set(Impulse);
K.Solve33ToOut(Cdot.NegateLocal(), df);
//Cdot.negateLocal(); not used anymore
Impulse.AddLocal(df);
if (LimitState == LimitState.AtLower)
{
Impulse.Z = MathUtils.Max(Impulse.Z, 0.0f);
}
else if (LimitState == LimitState.AtUpper)
{
Impulse.Z = MathUtils.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)
Vec2 b = Pool.PopVec2();
Vec2 f2r = Pool.PopVec2();
temp.Set(K.Ez.X, K.Ez.Y).MulLocal(Impulse.Z - f1.Z);
b.Set(Cdot1).NegateLocal().SubLocal(temp);
temp.Set(f1.X, f1.Y);
K.Solve22ToOut(b, f2r);
f2r.AddLocal(temp);
Impulse.X = f2r.X;
Impulse.Y = f2r.Y;
df.Set(Impulse).SubLocal(f1);
Vec2 P = Pool.PopVec2();
temp.Set(Axis).MulLocal(df.Z);
P.Set(Perp).MulLocal(df.X).AddLocal(temp);
float LA = df.X * S1 + df.Y + df.Z * A1;
float LB = df.X * S2 + df.Y + df.Z * A2;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Pool.PushVec2(3);
Pool.PushVec3(3);
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vec2 df = Pool.PopVec2();
K.Solve22ToOut(Cdot1.NegateLocal(), df);
Cdot1.NegateLocal();
Impulse.X += df.X;
Impulse.Y += df.Y;
Vec2 P = Pool.PopVec2();
P.Set(Perp).MulLocal(df.X);
float LA = df.X * S1 + df.Y;
float LB = df.X * S2 + df.Y;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Vec2 Cdot10 = Pool.PopVec2();
Cdot10.Set(Cdot1);
Cdot1.X = Vec2.Dot(Perp, temp.Set(vB).SubLocal(vA)) + S2 * wB - S1 * wA;
Cdot1.Y = wB - wA;
if (MathUtils.Abs(Cdot1.X) > 0.01f || MathUtils.Abs(Cdot1.Y) > 0.01f)
{
// djm note: what's happening here?
Mat33.Mul22ToOutUnsafe(K, df, temp);
Cdot1.X += 0.0f;
}
Pool.PushVec2(3);
}
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(2);
}
/// <summary>
/// This resets the mass properties to the sum of the mass properties of the fixtures. This
/// normally does not need to be called unless you called setMassData to override the mass and you
/// later want to reset the mass.
/// </summary>
public void ResetMassData()
{
// Compute mass data from shapes. Each shape has its own density.
Mass = 0.0f;
InvMass = 0.0f;
I = 0.0f;
InvI = 0.0f;
Sweep.LocalCenter.SetZero();
// Static and kinematic bodies have zero mass.
if (m_type == BodyType.Static || m_type == BodyType.Kinematic)
{
// m_sweep.c0 = m_sweep.c = m_xf.position;
Sweep.C0.Set(Xf.P);
Sweep.C.Set(Xf.P);
Sweep.A0 = Sweep.A;
return;
}
Debug.Assert(m_type == BodyType.Dynamic);
// Accumulate mass over all fixtures.
Vec2 localCenter = World.Pool.PopVec2();
localCenter.SetZero();
Vec2 temp = World.Pool.PopVec2();
MassData massData = pmd;
for (Fixture f = FixtureList; f != null; f = f.Next)
{
if (f.Density == 0.0f)
{
continue;
}
f.GetMassData(massData);
Mass += massData.Mass;
// center += massData.mass * massData.center;
temp.Set(massData.Center).MulLocal(massData.Mass);
localCenter.AddLocal(temp);
I += massData.I;
}
// Compute center of mass.
if (Mass > 0.0f)
{
InvMass = 1.0f / Mass;
localCenter.MulLocal(InvMass);
}
else
{
// Force all dynamic bodies to have a positive mass.
Mass = 1.0f;
InvMass = 1.0f;
}
if (I > 0.0f && (Flags & TypeFlags.FixedRotation) == 0)
{
// Center the inertia about the center of mass.
I -= Mass * Vec2.Dot(localCenter, localCenter);
Debug.Assert(I > 0.0f);
InvI = 1.0f / I;
}
else
{
I = 0.0f;
InvI = 0.0f;
}
Vec2 oldCenter = World.Pool.PopVec2();
// Move center of mass.
oldCenter.Set(Sweep.C);
Sweep.LocalCenter.Set(localCenter);
// m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter);
Transform.MulToOutUnsafe(Xf, Sweep.LocalCenter, Sweep.C0);
Sweep.C.Set(Sweep.C0);
// Update center of mass velocity.
// m_linearVelocity += Cross(m_angularVelocity, m_sweep.c - oldCenter);
temp.Set(Sweep.C).SubLocal(oldCenter);
Vec2 temp2 = oldCenter;
Vec2.CrossToOutUnsafe(m_angularVelocity, temp, temp2);
m_linearVelocity.AddLocal(temp2);
World.Pool.PushVec2(3);
}
/// <seealso cref="Joint.initVelocityConstraints(TimeStep)"></seealso>
public override void InitVelocityConstraints(SolverData data)
{
IndexA = BodyA.IslandIndex;
IndexB = BodyB.IslandIndex;
LocalCenterA.Set(BodyA.Sweep.LocalCenter);
LocalCenterB.Set(BodyB.Sweep.LocalCenter);
InvMassA = BodyA.InvMass;
InvMassB = BodyB.InvMass;
InvIA = BodyA.InvI;
InvIB = BodyB.InvI;
float aA = data.Positions[IndexA].A;
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
float aB = data.Positions[IndexB].A;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
Vec2 temp = Pool.PopVec2();
Rot qA = Pool.PopRot();
Rot qB = Pool.PopRot();
qA.Set(aA);
qB.Set(aB);
// Compute the effective mass matrix.
Rot.MulToOutUnsafe(qA, temp.Set(m_localAnchorA).SubLocal(LocalCenterA), RA);
Rot.MulToOutUnsafe(qB, temp.Set(m_localAnchorB).SubLocal(LocalCenterB), RB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
Mat22 K = Pool.PopMat22();
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;
K.InvertToOut(LinearMass);
AngularMass = iA + iB;
if (AngularMass > 0.0f)
{
AngularMass = 1.0f / AngularMass;
}
if (data.Step.WarmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse.MulLocal(data.Step.DtRatio);
m_angularImpulse *= data.Step.DtRatio;
Vec2 P = Pool.PopVec2();
P.Set(m_linearImpulse);
temp.Set(P).MulLocal(mA);
vA.SubLocal(temp);
wA -= iA * (Vec2.Cross(RA, P) + m_angularImpulse);
temp.Set(P).MulLocal(mB);
vB.AddLocal(temp);
wB += iB * (Vec2.Cross(RB, P) + m_angularImpulse);
Pool.PushVec2(1);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 0.0f;
}
data.Velocities[IndexA].V.Set(vA);
if (data.Velocities[IndexA].W != wA)
{
Debug.Assert(data.Velocities[IndexA].W != wA);
}
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushRot(2);
Pool.PushVec2(1);
Pool.PushMat22(1);
}
public void GetLinearVelocityFromWorldPointToOut(Vec2 worldPoint, Vec2 result)
{
result.Set(worldPoint).SubLocal(Sweep.C);
Vec2.CrossToOut(m_angularVelocity, result, result);
result.AddLocal(m_linearVelocity);
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 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 (m_motorEnabled && LimitState != LimitState.Equal && fixedRotation == false)
{
float Cdot = wB - wA - m_motorSpeed;
float impulse = (-MotorMass) * Cdot;
float oldImpulse = MotorImpulse;
float maxImpulse = data.Step.Dt * m_maxMotorTorque;
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = MotorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
Vec2 temp = Pool.PopVec2();
// Solve limit constraint.
if (m_limitEnabled && LimitState != LimitState.Inactive && fixedRotation == false)
{
Vec2 Cdot1 = Pool.PopVec2();
Vec3 Cdot = Pool.PopVec3();
// Solve point-to-point constraint
Vec2.CrossToOutUnsafe(wA, RA, temp);
Vec2.CrossToOutUnsafe(wB, RB, Cdot1);
Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp);
float Cdot2 = wB - wA;
Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 impulse = Pool.PopVec3();
Mass.Solve33ToOut(Cdot, impulse);
impulse.NegateLocal();
if (LimitState == LimitState.Equal)
{
Impulse.AddLocal(impulse);
}
else if (LimitState == LimitState.AtLower)
{
float newImpulse = Impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
//UPGRADE_NOTE: Final was removed from the declaration of 'rhs '. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1003'"
Vec2 rhs = Pool.PopVec2();
rhs.Set(Mass.Ez.X, Mass.Ez.Y).MulLocal(Impulse.Z).SubLocal(Cdot1);
Mass.Solve22ToOut(rhs, temp);
impulse.X = temp.X;
impulse.Y = temp.Y;
impulse.Z = -Impulse.Z;
Impulse.X += temp.X;
Impulse.Y += temp.Y;
Impulse.Z = 0.0f;
Pool.PushVec2(1);
}
else
{
Impulse.AddLocal(impulse);
}
}
else if (LimitState == LimitState.AtUpper)
{
float newImpulse = Impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vec2 rhs = Pool.PopVec2();
rhs.Set(Mass.Ez.X, Mass.Ez.Y).MulLocal(Impulse.Z).SubLocal(Cdot1);
Mass.Solve22ToOut(rhs, temp);
impulse.X = temp.X;
impulse.Y = temp.Y;
impulse.Z = -Impulse.Z;
Impulse.X += temp.X;
Impulse.Y += temp.Y;
Impulse.Z = 0.0f;
Pool.PushVec2(1);
}
else
{
Impulse.AddLocal(impulse);
}
}
Vec2 P = Pool.PopVec2();
P.Set(impulse.X, impulse.Y);
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * (Vec2.Cross(RA, P) + impulse.Z);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * (Vec2.Cross(RB, P) + impulse.Z);
Pool.PushVec2(2);
Pool.PushVec3(2);
}
else
{
// Solve point-to-point constraint
Vec2 Cdot = Pool.PopVec2();
Vec2 impulse = Pool.PopVec2();
Vec2.CrossToOutUnsafe(wA, RA, temp);
Vec2.CrossToOutUnsafe(wB, RB, Cdot);
Cdot.AddLocal(vB).SubLocal(vA).SubLocal(temp);
Mass.Solve22ToOut(Cdot.NegateLocal(), impulse); // just leave negated
Impulse.X += impulse.X;
Impulse.Y += impulse.Y;
vA.X -= mA * impulse.X;
vA.Y -= mA * impulse.Y;
wA -= iA * Vec2.Cross(RA, impulse);
vB.X += mB * impulse.X;
vB.Y += mB * impulse.Y;
wB += iB * Vec2.Cross(RB, impulse);
Pool.PushVec2(2);
}
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(1);
}
