public override void solveVelocityConstraints(SolverData data)
{
Vec2 vA = data.velocities[m_indexA].v;
double wA = data.velocities[m_indexA].w;
Vec2 vB = data.velocities[m_indexB].v;
double wB = data.velocities[m_indexB].w;
double mA = m_invMassA, mB = m_invMassB;
double iA = m_invIA, iB = m_invIB;
Vec2 Cdot1 = pool.popVec2();
Vec2 P = pool.popVec2();
Vec2 temp = pool.popVec2();
if (m_frequencyHz > 0.0d)
{
double Cdot2 = wB - wA;
double 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);
double 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);
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// 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;
Mat33 K = new Mat33();
K.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
K.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
K.ez.X = -_rA.Y * iA - _rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
K.ez.Y = _rA.X * iA + _rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
float invM = iA + iB;
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
float C = aB - aA - ReferenceAngle;
// Frequency
float omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
float d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
float k = m * omega * omega;
// magic formulas
float h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
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
{
_impulse = Vector3.Zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// 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;
Mat33 K = new Mat33();
K.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
K.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
K.ez.X = -_rA.Y * iA - _rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
K.ez.Y = _rA.X * iA + _rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
float invM = iA + iB;
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
float C = aB - aA - ReferenceAngle;
// Frequency
float omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
float d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
float k = m * omega * omega;
// magic formulas
float h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
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
{
_impulse = Vector3.Zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public override bool solvePositionConstraints(SolverData data)
{
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 rA = pool.popVec2();
Vec2 rB = pool.popVec2();
Vec2 d = pool.popVec2();
Vec2 axis = pool.popVec2();
Vec2 perp = pool.popVec2();
Vec2 temp = pool.popVec2();
Vec2 C1 = pool.popVec2();
Vec3 impulse = pool.popVec3();
Vec2 cA = data.positions[m_indexA].c;
double aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
double aB = data.positions[m_indexB].a;
qA.set(aA);
qB.set(aB);
double mA = m_invMassA, mB = m_invMassB;
double iA = m_invIA, iB = m_invIB;
// Compute fresh Jacobians
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
Rot.mulToOutUnsafe(qA, m_localXAxisA, axis);
double a1 = Vec2.cross(temp.set(d).addLocal(rA), axis);
double a2 = Vec2.cross(rB, axis);
Rot.mulToOutUnsafe(qA, m_localYAxisA, perp);
double s1 = Vec2.cross(temp.set(d).addLocal(rA), perp);
double s2 = Vec2.cross(rB, perp);
C1.x = Vec2.dot(perp, d);
C1.y = aB - aA - m_referenceAngle;
double linearError = MathUtils.abs(C1.x);
double angularError = MathUtils.abs(C1.y);
bool active = false;
double C2 = 0.0d;
if (m_enableLimit)
{
double translation = Vec2.dot(axis, d);
if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0d * Settings.linearSlop)
{
// Prevent large angular corrections
C2 =
MathUtils.clamp(translation, -Settings.maxLinearCorrection,
Settings.maxLinearCorrection);
linearError = MathUtils.max(linearError, MathUtils.abs(translation));
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 =
MathUtils.clamp(translation - m_lowerTranslation + Settings.linearSlop,
-Settings.maxLinearCorrection, 0.0d);
linearError = MathUtils.max(linearError, m_lowerTranslation - translation);
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 =
MathUtils.clamp(translation - m_upperTranslation - Settings.linearSlop, 0.0d,
Settings.maxLinearCorrection);
linearError = MathUtils.max(linearError, translation - m_upperTranslation);
active = true;
}
}
if (active)
{
double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
double k12 = iA * s1 + iB * s2;
double k13 = iA * s1 * a1 + iB * s2 * a2;
double k22 = iA + iB;
if (k22 == 0.0d)
{
// For fixed rotation
k22 = 1.0d;
}
double k23 = iA * a1 + iB * a2;
double k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = pool.popMat33();
K.ex.set(k11, k12, k13);
K.ey.set(k12, k22, k23);
K.ez.set(k13, k23, k33);
Vec3 C = pool.popVec3();
C.x = C1.x;
C.y = C1.y;
C.z = C2;
K.solve33ToOut(C.negateLocal(), impulse);
pool.pushVec3(1);
pool.pushMat33(1);
}
else
{
double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
double k12 = iA * s1 + iB * s2;
double k22 = iA + iB;
if (k22 == 0.0d)
{
k22 = 1.0d;
}
Mat22 K = pool.popMat22();
K.ex.set(k11, k12);
K.ey.set(k12, k22);
// temp is impulse1
K.solveToOut(C1.negateLocal(), temp);
C1.negateLocal();
impulse.x = temp.x;
impulse.y = temp.y;
impulse.z = 0.0d;
pool.pushMat22(1);
}
double Px = impulse.x * perp.x + impulse.z * axis.x;
double Py = impulse.x * perp.y + impulse.z * axis.y;
double LA = impulse.x * s1 + impulse.y + impulse.z * a1;
double LB = impulse.x * s2 + impulse.y + impulse.z * a2;
cA.x -= mA * Px;
cA.y -= mA * Py;
aA -= iA * LA;
cB.x += mB * Px;
cB.y += mB * Py;
aB += iB * LB;
// data.positions[m_indexA].c.set(cA);
data.positions[m_indexA].a = aA;
// data.positions[m_indexB].c.set(cB);
data.positions[m_indexB].a = aB;
pool.pushVec2(7);
pool.pushVec3(1);
pool.pushRot(2);
return(linearError <= Settings.linearSlop && angularError <= Settings.angularSlop);
}
/// <summary>Multiply a matrix times a vector.</summary> public static Vector2 Mul22(Mat33 a, Vector2 v) => new Vector2(a.Ex.X * v.X + a.Ey.X * v.Y, a.Ex.Y * v.X + a.Ey.Y * v.Y);
/// <seealso cref="Joint.initVelocityConstraints(TimeStep)"></seealso>
public override void InitVelocityConstraints(SolverData data)
{
m_indexA = BodyA.IslandIndex;
m_indexB = BodyB.IslandIndex;
m_localCenterA.Set(BodyA.Sweep.LocalCenter);
m_localCenterB.Set(BodyB.Sweep.LocalCenter);
m_invMassA = BodyA.InvMass;
m_invMassB = BodyB.InvMass;
m_invIA = BodyA.InvI;
m_invIB = BodyB.InvI;
// Vec2 cA = data.positions[m_indexA].c;
float aA = data.Positions[m_indexA].A;
Vec2 vA = data.Velocities[m_indexA].V;
float wA = data.Velocities[m_indexA].W;
// Vec2 cB = data.positions[m_indexB].c;
float aB = data.Positions[m_indexB].A;
Vec2 vB = data.Velocities[m_indexB].V;
float wB = data.Velocities[m_indexB].W;
Rot qA = Pool.PopRot();
Rot qB = Pool.PopRot();
Vec2 temp = Pool.PopVec2();
qA.Set(aA);
qB.Set(aB);
// Compute the effective masses.
Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), m_rA);
Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), m_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 = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Mat33 K = Pool.PopMat33();
K.Ex.X = mA + mB + m_rA.Y * m_rA.Y * iA + m_rB.Y * m_rB.Y * iB;
K.Ey.X = (-m_rA.Y) * m_rA.X * iA - m_rB.Y * m_rB.X * iB;
K.Ez.X = (-m_rA.Y) * iA - m_rB.Y * iB;
K.Ex.Y = K.Ey.X;
K.Ey.Y = mA + mB + m_rA.X * m_rA.X * iA + m_rB.X * m_rB.X * iB;
K.Ez.Y = m_rA.X * iA + m_rB.X * iB;
K.Ex.Z = K.Ez.X;
K.Ey.Z = K.Ez.Y;
K.Ez.Z = iA + iB;
if (Frequency > 0.0f)
{
K.GetInverse22(m_mass);
float invM = iA + iB;
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
float C = aB - aA - m_referenceAngle;
// Frequency
float omega = 2.0f * MathUtils.PI * Frequency;
// Damping coefficient
float d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
float k = m * omega * omega;
// magic formulas
float h = data.Step.Dt;
m_gamma = h * (d + h * k);
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
m_bias = C * h * k * m_gamma;
invM += m_gamma;
m_mass.Ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(m_mass);
m_gamma = 0.0f;
m_bias = 0.0f;
}
if (data.Step.WarmStarting)
{
Vec2 P = Pool.PopVec2();
// Scale impulses to support a variable time step.
m_impulse.MulLocal(data.Step.DtRatio);
P.Set(m_impulse.X, m_impulse.Y);
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * (Vec2.Cross(m_rA, P) + m_impulse.Z);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * (Vec2.Cross(m_rB, P) + m_impulse.Z);
Pool.PushVec2(1);
}
else
{
m_impulse.SetZero();
}
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(1);
Pool.PushRot(2);
Pool.PushMat33(1);
}
/// <seealso cref="Joint.solvePositionConstraints(float)"></seealso>
public override bool SolvePositionConstraints(SolverData data)
{
Vec2 cA = data.Positions[m_indexA].C;
float aA = data.Positions[m_indexA].A;
Vec2 cB = data.Positions[m_indexB].C;
float aB = data.Positions[m_indexB].A;
Rot qA = Pool.PopRot();
Rot qB = Pool.PopRot();
Vec2 temp = Pool.PopVec2();
Vec2 rA = Pool.PopVec2();
Vec2 rB = Pool.PopVec2();
qA.Set(aA);
qB.Set(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), rA);
Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), rB);
float positionError, angularError;
Mat33 K = Pool.PopMat33();
Vec2 C1 = Pool.PopVec2();
Vec2 P = Pool.PopVec2();
K.Ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
K.Ey.X = (-rA.Y) * rA.X * iA - rB.Y * rB.X * iB;
K.Ez.X = (-rA.Y) * iA - rB.Y * iB;
K.Ex.Y = K.Ey.X;
K.Ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
K.Ez.Y = rA.X * iA + rB.X * iB;
K.Ex.Z = K.Ez.X;
K.Ey.Z = K.Ez.Y;
K.Ez.Z = iA + iB;
if (Frequency > 0.0f)
{
C1.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA);
positionError = C1.Length();
angularError = 0.0f;
K.Solve22ToOut(C1, P);
P.NegateLocal();
cA.X -= mA * P.X;
cA.Y -= mA * P.Y;
aA -= iA * Vec2.Cross(rA, P);
cB.X += mB * P.X;
cB.Y += mB * P.Y;
aB += iB * Vec2.Cross(rB, P);
}
else
{
C1.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA);
float C2 = aB - aA - m_referenceAngle;
positionError = C1.Length();
angularError = MathUtils.Abs(C2);
Vec3 C = Pool.PopVec3();
Vec3 impulse = Pool.PopVec3();
C.Set(C1.X, C1.Y, C2);
K.Solve33ToOut(C, impulse);
impulse.NegateLocal();
P.Set(impulse.X, impulse.Y);
cA.X -= mA * P.X;
cA.Y -= mA * P.Y;
aA -= iA * (Vec2.Cross(rA, P) + impulse.Z);
cB.X += mB * P.X;
cB.Y += mB * P.Y;
aB += iB * (Vec2.Cross(rB, P) + impulse.Z);
}
data.Positions[m_indexA].C.Set(cA);
data.Positions[m_indexA].A = aA;
data.Positions[m_indexB].C.Set(cB);
data.Positions[m_indexB].A = aB;
Pool.PushVec2(5);
Pool.PushRot(2);
Pool.PushMat33(1);
return(positionError <= Settings.LINEAR_SLOP && angularError <= Settings.ANGULAR_SLOP);
}
// / Multiply a matrix times a vector.
public static Vec3 mul(Mat33 A, Vec3 v)
{
return(new Vec3(v.x * A.ex.x + v.y * A.ey.x + v.z + A.ez.x, v.x * A.ex.y + v.y * A.ey.y + v.z
* A.ez.y, v.x * A.ex.z + v.y * A.ey.z + v.z * A.ez.z));
}
public static Vec2 mul22(Mat33 A, Vec2 v)
{
return(new Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y));
}
/// <summary>Multiply a matrix times a vector.</summary>
public static Vector3 Mul(Mat33 a, Vector3 v)
{
return(v.X * a.ex + v.Y * a.ey + v.Z * a.ez);
}
/// <summary>Multiply a matrix times a vector.</summary>
public static Vector2 Mul22(Mat33 a, Vector2 v)
{
return(new Vector2(a.ex.X * v.X + a.ey.X * v.Y, a.ex.Y * v.X + a.ey.Y * v.Y));
}
internal override bool SolvePositionConstraints()
{
Vector2 cA = _bodyA.GetRelativePoint(m_LocalCenterA);
Vector2 cA0 = cA;
float aA = _bodyA.rotation * Mathf.Deg2Rad;
Vector2 cB = _bodyB.GetRelativePoint(m_LocalCenterB);
Vector2 cB0 = cB;
float aB = _bodyB.rotation * Mathf.Deg2Rad;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = m_InvMassA, mB = m_InvMassB;
float iA = m_InvIA, iB = m_InvIB;
// Compute fresh Jacobians
Vector2 rA = MathUtils.Mul(qA, _localAnchorA - m_LocalCenterA);
Vector2 rB = MathUtils.Mul(qB, _localAnchorB - m_LocalCenterB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.Mul(qA, localXAxisA);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(rB, axis);
Vector2 perp = MathUtils.Mul(qA, m_LocalYAxisA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(rB, perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.x = Vector2.Dot(perp, d);
C1.y = aB - aA - referenceAngle;
float linearError = Mathf.Abs(C1.x);
float angularError = Mathf.Abs(C1.y);
bool active = false;
float C2 = 0.0f;
if (m_EnableLimit)
{
float translation = Vector2.Dot(axis, d);
m_Translation = translation;
if (Mathf.Abs(m_UpperTranslation - m_LowerTranslation) < 2.0f * Constants.linearSlop)
{
// Prevent large angular corrections
C2 = Mathf.Clamp(translation, -Constants.maxLinearCorrection, Constants.maxLinearCorrection);
linearError = Mathf.Max(linearError, Mathf.Abs(translation));
active = true;
}
else if (translation <= m_LowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Mathf.Clamp(translation - m_LowerTranslation + Constants.linearSlop,
-Constants.maxLinearCorrection, 0.0f);
linearError = Mathf.Max(linearError, m_LowerTranslation - translation);
active = true;
}
else if (translation >= m_UpperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Mathf.Clamp(translation - m_UpperTranslation - Constants.linearSlop, 0.0f,
Constants.maxLinearCorrection);
linearError = Mathf.Max(linearError, translation - m_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;
if (!_bodyA.isKinematic)
{
var dcA = cA0 - cA;
_bodyA.position -= dcA;
if (!_bodyA.fixedAngle)
{
_bodyA.rotation = aA * Mathf.Rad2Deg;
}
}
if (!_bodyB.isKinematic)
{
var dcB = cB0 - cB;
_bodyB.position -= dcB;
if (!_bodyB.fixedAngle)
{
_bodyB.rotation = aB * Mathf.Rad2Deg;
}
}
return(linearError < Constants.linearSlop && angularError < Constants.angularSlop);
}
/// Multiply a matrix times a vector.
public static Vector3 Mul(Mat33 A, Vector3 v)
{
return(v.x * A.ex + v.y * A.ey + v.z * A.ez);
}
public override bool solvePositionConstraints(SolverData data)
{
Vec2 cA = data.positions[m_indexA].c;
double aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
double aB = data.positions[m_indexB].a;
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 temp = pool.popVec2();
Vec2 rA = pool.popVec2();
Vec2 rB = pool.popVec2();
qA.set(aA);
qB.set(aB);
double mA = m_invMassA, mB = m_invMassB;
double iA = m_invIA, iB = m_invIB;
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
double positionError, angularError;
Mat33 K = pool.popMat33();
Vec2 C1 = pool.popVec2();
Vec2 P = pool.popVec2();
K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
K.ez.x = -rA.y * iA - rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
K.ez.y = rA.x * iA + rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (m_frequencyHz > 0.0d)
{
C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
positionError = C1.length();
angularError = 0.0d;
K.solve22ToOut(C1, P);
P.negateLocal();
cA.x -= mA * P.x;
cA.y -= mA * P.y;
aA -= iA * Vec2.cross(rA, P);
cB.x += mB * P.x;
cB.y += mB * P.y;
aB += iB * Vec2.cross(rB, P);
}
else
{
C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
double C2 = aB - aA - m_referenceAngle;
positionError = C1.length();
angularError = MathUtils.abs(C2);
Vec3 C = pool.popVec3();
Vec3 impulse = pool.popVec3();
C.set(C1.x, C1.y, C2);
K.solve33ToOut(C, impulse);
impulse.negateLocal();
P.set(impulse.x, impulse.y);
cA.x -= mA * P.x;
cA.y -= mA * P.y;
aA -= iA * (Vec2.cross(rA, P) + impulse.z);
cB.x += mB * P.x;
cB.y += mB * P.y;
aB += iB * (Vec2.cross(rB, P) + impulse.z);
pool.pushVec3(2);
}
// data.positions[m_indexA].c.set(cA);
data.positions[m_indexA].a = aA;
// data.positions[m_indexB].c.set(cB);
data.positions[m_indexB].a = aB;
pool.pushVec2(5);
pool.pushRot(2);
pool.pushMat33(1);
return(positionError <= Settings.linearSlop && angularError <= Settings.angularSlop);
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
TSVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
TSVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FP positionError, angularError;
Mat33 K = new Mat33();
K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
K.ez.x = -rA.y * iA - rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
K.ez.y = rA.x * iA + rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (FrequencyHz > 0.0f)
{
TSVector2 C1 = cB + rB - cA - rA;
positionError = C1.magnitude;
angularError = 0.0f;
TSVector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
TSVector2 C1 = cB + rB - cA - rA;
FP C2 = aB - aA - ReferenceAngle;
positionError = C1.magnitude;
angularError = FP.Abs(C2);
TSVector C = new TSVector(C1.x, C1.y, C2);
TSVector impulse = K.Solve33(C) * -1;
TSVector2 P = new TSVector2(impulse.x, impulse.y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.z);
}
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);
}
public static void mul22ToOutUnsafe(Mat33 A, Vec2 v, Vec2 out_)
{
out_.y = A.ex.y * v.x + A.ey.y * v.y;
out_.x = A.ex.x * v.x + A.ey.x * v.y;
}
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 mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(rB, axis);
Vector2 perp = MathUtils.Mul(qA, _localYAxisA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(rB, 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);
}
public static void mulToOutUnsafe(Mat33 A, Vec3 v, Vec3 out_)
{
out_.x = v.x * A.ex.x + v.y * A.ey.x + v.z * A.ez.x;
out_.y = v.x * A.ex.y + v.y * A.ey.y + v.z * A.ez.y;
out_.z = v.x * A.ex.z + v.y * A.ey.z + v.z * A.ez.z;
}
/// <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);
}
/// <summary>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void InitVelocityConstraints(TimeStep step)
{
Body body1 = Body1;
Body body2 = Body2;
if (IsMotorEnabled || IsLimitEnabled)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Box2DxDebug.Assert(body1.InvI > 0.0f || body2.InvI > 0.0f);
}
// Compute the effective mass matrix.
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = body1.InvMass, m2 = body2.InvMass;
float i1 = body1.InvI, i2 = body2.InvI;
float col1X = m1 + m2 + mulR1.Y * mulR1.Y * i1 + mulR2.Y * mulR2.Y * i2;
float col2X = -mulR1.Y * mulR1.X * i1 - mulR2.Y * mulR2.X * i2;
float col3X = -mulR1.Y * i1 - mulR2.Y * i2;
float col1Y = Mass.Col2.X;
float col2Y = m1 + m2 + mulR1.X * mulR1.X * i1 + mulR2.X * mulR2.X * i2;
float col3Y = mulR1.X * i1 + mulR2.X * i2;
float col1Z = Mass.Col3.X;
float col2Z = Mass.Col3.Y;
float col3Z = i1 + i2;
Mass = new Mat33(new Vec3(col1X, col1Y, col1Z), new Vec3(col2X, col2Y, col2Z),
new Vec3(col3X, col3Y, col3Z));
/*
* _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
* _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
* _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
* _mass.Col1.Y = _mass.Col2.X;
* _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
* _mass.Col3.Y = r1.X * i1 + r2.X * i2;
* _mass.Col1.Z = _mass.Col3.X;
* _mass.Col2.Z = _mass.Col3.Y;
* _mass.Col3.Z = i1 + i2;
*/
MotorMass = 1.0f / (i1 + i2);
if (IsMotorEnabled == false)
{
MotorTorque = 0.0f;
}
if (IsLimitEnabled)
{
float jointAngle = body2.Sweep.A - body1.Sweep.A - ReferenceAngle;
if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.AngularSlop)
{
State = LimitState.EqualLimits;
}
else if (jointAngle <= LowerLimit)
{
if (State != LimitState.AtLowerLimit)
{
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
State = LimitState.AtLowerLimit;
}
else if (jointAngle >= UpperLimit)
{
if (State != LimitState.AtUpperLimit)
{
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
State = LimitState.AtUpperLimit;
}
else
{
State = LimitState.InactiveLimit;
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
}
else
{
State = LimitState.InactiveLimit;
}
if (step.WarmStarting)
{
// Scale impulses to support a variable time step.
Impulse *= step.DtRatio;
MotorTorque *= step.DtRatio;
Vec2 p = new Vec2(Impulse.X, Impulse.Y);
body1.LinearVelocity -= m1 * p;
body1.AngularVelocity -= i1 * (Vec2.Cross(mulR1, p) + MotorTorque + Impulse.Z);
body2.LinearVelocity += m2 * p;
body2.AngularVelocity += i2 * (Vec2.Cross(mulR2, p) + MotorTorque + Impulse.Z);
}
else
{
Impulse.SetZero();
MotorTorque = 0.0f;
}
}
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;
Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
float positionError, angularError;
Mat33 K = new Mat33();
K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
K.ez.X = -rA.Y * iA - rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
K.ez.Y = rA.X * iA + rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
Vector2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
Vector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(ref rA, ref P);
cB += mB * P;
aB += iB * MathUtils.Cross(ref rB, ref P);
}
else
{
Vector2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - ReferenceAngle;
positionError = C1.Length();
angularError = Math.Abs(C2);
Vector3 C = new Vector3(C1.X, C1.Y, C2);
Vector3 impulse;
if (K.ez.Z <= 0.0f)
{
Vector2 impulse2 = -K.Solve22(C1);
impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f);
}
else
{
impulse = -K.Solve33(C);
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(ref rA, ref P) + impulse.Z);
cB += mB * P;
aB += iB * (MathUtils.Cross(ref rB, ref P) + impulse.Z);
}
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);
}
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>Multiply a matrix times a vector.</summary> public static Vector3 Mul(Mat33 a, Vector3 v) => v.X * a.Ex + v.Y * a.Ey + v.Z * a.Ez;
public override bool SolvePositionConstraints(SolverData data)
{
Rot qA = Pool.PopRot();
Rot qB = Pool.PopRot();
Vec2 rA = Pool.PopVec2();
Vec2 rB = Pool.PopVec2();
Vec2 d = Pool.PopVec2();
Vec2 axis = Pool.PopVec2();
Vec2 perp = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
Vec2 C1 = Pool.PopVec2();
Vec3 impulse = Pool.PopVec3();
Vec2 cA = data.Positions[IndexA].C;
float aA = data.Positions[IndexA].A;
Vec2 cB = data.Positions[IndexB].C;
float aB = data.Positions[IndexB].A;
qA.Set(aA);
qB.Set(aB);
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
// Compute fresh Jacobians
Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(LocalCenterA), rA);
Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(LocalCenterB), rB);
d.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA);
Rot.MulToOutUnsafe(qA, LocalXAxisA, axis);
float a1 = Vec2.Cross(temp.Set(d).AddLocal(rA), axis);
float a2 = Vec2.Cross(rB, axis);
Rot.MulToOutUnsafe(qA, LocalYAxisA, perp);
float s1 = Vec2.Cross(temp.Set(d).AddLocal(rA), perp);
float s2 = Vec2.Cross(rB, perp);
C1.X = Vec2.Dot(perp, d);
C1.Y = aB - aA - ReferenceAngle;
float linearError = MathUtils.Abs(C1.X);
float angularError = MathUtils.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (m_limitEnabled)
{
float translation = Vec2.Dot(axis, d);
if (MathUtils.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LINEAR_SLOP)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MAX_LINEAR_CORRECTION, Settings.MAX_LINEAR_CORRECTION);
linearError = MathUtils.Max(linearError, MathUtils.Abs(translation));
active = true;
}
else if (translation <= LowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - LowerTranslation + Settings.LINEAR_SLOP, -Settings.MAX_LINEAR_CORRECTION, 0.0f);
linearError = MathUtils.Max(linearError, LowerTranslation - translation);
active = true;
}
else if (translation >= UpperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - UpperTranslation - Settings.LINEAR_SLOP, 0.0f, Settings.MAX_LINEAR_CORRECTION);
linearError = MathUtils.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 = Pool.PopMat33();
K.Ex.Set(k11, k12, k13);
K.Ey.Set(k12, k22, k23);
K.Ez.Set(k13, k23, k33);
Vec3 C = Pool.PopVec3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
K.Solve33ToOut(C.NegateLocal(), impulse);
Pool.PushVec3(1);
Pool.PushMat33(1);
}
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 = Pool.PopMat22();
K.Ex.Set(k11, k12);
K.Ey.Set(k12, k22);
// temp is impulse1
K.SolveToOut(C1.NegateLocal(), temp);
C1.NegateLocal();
impulse.X = temp.X;
impulse.Y = temp.Y;
impulse.Z = 0.0f;
Pool.PushMat22(1);
}
float Px = impulse.X * perp.X + impulse.Z * axis.X;
float Py = impulse.X * perp.Y + impulse.Z * axis.Y;
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA.X -= mA * Px;
cA.Y -= mA * Py;
aA -= iA * LA;
cB.X += mB * Px;
cB.Y += mB * Py;
aB += iB * LB;
data.Positions[IndexA].C.Set(cA);
data.Positions[IndexA].A = aA;
data.Positions[IndexB].C.Set(cB);
data.Positions[IndexB].A = aB;
Pool.PushVec2(7);
Pool.PushVec3(1);
Pool.PushRot(2);
return(linearError <= Settings.LINEAR_SLOP && angularError <= Settings.ANGULAR_SLOP);
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
var cA = data.Positions[_indexA].C;
float aA = data.Positions[_indexA].A;
var cB = data.Positions[_indexB].C;
float aB = data.Positions[_indexB].A;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
var rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
var rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
float positionError, angularError;
var K = new Mat33();
K.Ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
K.Ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
K.Ez.X = -rA.Y * iA - rB.Y * iB;
K.Ex.Y = K.Ey.X;
K.Ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
K.Ez.Y = rA.X * iA + rB.X * iB;
K.Ex.Z = K.Ez.X;
K.Ey.Z = K.Ez.Y;
K.Ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
Vector2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
Vector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
Vector2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - ReferenceAngle;
positionError = C1.Length();
angularError = Math.Abs(C2);
Vector3 C = new Vector3(C1.X, C1.Y, C2);
Vector3 impulse = -K.Solve33(C);
Vector2 P = new Vector2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
}
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);
}
/// Multiply a matrix times a vector.
public static Vector3 Mul(Mat33 A, Vector3 v)
{
return(v.X * A.ex + v.Y * A.ey + v.Z * A.ez);
}
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 mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(rB, axis);
Vector2 perp = MathUtils.Mul(qA, _localYAxisA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(rB, 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;
}
/// Multiply a matrix times a vector.
public static Vector2 Mul22(Mat33 A, Vector2 v)
{
return(new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y));
}
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 mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
float positionError, angularError;
Mat33 K = new Mat33();
K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
K.ez.X = -rA.Y * iA - rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
K.ez.Y = rA.X * iA + rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
Vector2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
Vector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
Vector2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - ReferenceAngle;
positionError = C1.Length();
angularError = Math.Abs(C2);
Vector3 C = new Vector3(C1.X, C1.Y, C2);
Vector3 impulse = -K.Solve33(C);
Vector2 P = new Vector2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
}
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;
}
public override void initVelocityConstraints(SolverData data)
{
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA.set(m_bodyA.m_sweep.localCenter);
m_localCenterB.set(m_bodyB.m_sweep.localCenter);
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.m_invI;
m_invIB = m_bodyB.m_invI;
// Vec2 cA = data.positions[m_indexA].c;
double aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
double wA = data.velocities[m_indexA].w;
// Vec2 cB = data.positions[m_indexB].c;
double aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
double wB = data.velocities[m_indexB].w;
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 temp = pool.popVec2();
qA.set(aA);
qB.set(aB);
// Compute the effective masses.
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_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]
double mA = m_invMassA, mB = m_invMassB;
double iA = m_invIA, iB = m_invIB;
Mat33 K = pool.popMat33();
K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
K.ez.x = -m_rA.y * iA - m_rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
K.ez.y = m_rA.x * iA + m_rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (m_frequencyHz > 0.0d)
{
K.getInverse22(m_mass);
double invM = iA + iB;
double m = invM > 0.0d ? 1.0d / invM : 0.0d;
double C = aB - aA - m_referenceAngle;
// Frequency
double omega = 2.0d * MathUtils.PI * m_frequencyHz;
// Damping coefficient
double d = 2.0d * m * m_dampingRatio * omega;
// Spring stiffness
double k = m * omega * omega;
// magic formulas
double h = data.step.dt;
m_gamma = h * (d + h * k);
m_gamma = m_gamma != 0.0d ? 1.0d / m_gamma : 0.0d;
m_bias = C * h * k * m_gamma;
invM += m_gamma;
m_mass.ez.z = invM != 0.0d ? 1.0d / invM : 0.0d;
}
else
{
K.getSymInverse33(m_mass);
m_gamma = 0.0d;
m_bias = 0.0d;
}
if (data.step.warmStarting)
{
Vec2 P = pool.popVec2();
// Scale impulses to support a variable time step.
m_impulse.mulLocal(data.step.dtRatio);
P.set(m_impulse.x, m_impulse.y);
vA.x -= mA * P.x;
vA.y -= mA * P.y;
wA -= iA * (Vec2.cross(m_rA, P) + m_impulse.z);
vB.x += mB * P.x;
vB.y += mB * P.y;
wB += iB * (Vec2.cross(m_rB, P) + m_impulse.z);
pool.pushVec2(1);
}
else
{
m_impulse.setZero();
}
// 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(1);
pool.pushRot(2);
pool.pushMat33(1);
}
