// Solve A * x = b
public b2Vec3 Solve33(b2Vec3 outV, float bX, float bY, float bZ)
{
float a11 = col1.x;
float a21 = col1.y;
float a31 = col1.z;
float a12 = col2.x;
float a22 = col2.y;
float a32 = col2.z;
float a13 = col3.x;
float a23 = col3.y;
float a33 = col3.z;
//float32 det = b2Dot(col1, b2Cross(col2, col3));
float det = a11 * (a22 * a33 - a32 * a23) +
a21 * (a32 * a13 - a12 * a33) +
a31 * (a12 * a23 - a22 * a13);
if (det != 0.0f)
{
det = 1.0f / det;
}
//out.x = det * b2Dot(b, b2Cross(col2, col3));
outV.x = det * (bX * (a22 * a33 - a32 * a23) +
bY * (a32 * a13 - a12 * a33) +
bZ * (a12 * a23 - a22 * a13));
//out.y = det * b2Dot(col1, b2Cross(b, col3));
outV.y = det * (a11 * (bY * a33 - bZ * a23) +
a21 * (bZ * a13 - bX * a33) +
a31 * (bX * a23 - bY * a13));
//out.z = det * b2Dot(col1, b2Cross(col2, b));
outV.z = det * (a11 * (a22 * bZ - a32 * bY) +
a21 * (a32 * bX - a12 * bZ) +
a31 * (a12 * bY - a22 * bX));
return(outV);
}
public b2Mat33(b2Vec3 c1, b2Vec3 c2, b2Vec3 c3) : this(Box2DPINVOKE.new_b2Mat33__SWIG_1(b2Vec3.getCPtr(c1), b2Vec3.getCPtr(c2), b2Vec3.getCPtr(c3)), true)
{
if (Box2DPINVOKE.SWIGPendingException.Pending)
{
throw Box2DPINVOKE.SWIGPendingException.Retrieve();
}
}
public void MinusEqual(b2Vec3 v)
{
Box2dPINVOKE.b2Vec3_MinusEqual(swigCPtr, b2Vec3.getCPtr(v));
if (Box2dPINVOKE.SWIGPendingException.Pending)
{
throw Box2dPINVOKE.SWIGPendingException.Retrieve();
}
}
public override void SolveVelocityConstraints(b2SolverData data)
{
b2Vec2 vA = m_bodyA.InternalVelocity.v;
float wA = m_bodyA.InternalVelocity.w;
b2Vec2 vB = m_bodyB.InternalVelocity.v;
float wB = m_bodyB.InternalVelocity.w;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
if (m_frequencyHz > 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;
b2Vec2 Cdot1 = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA);
b2Vec2 impulse1 = -b2Math.b2Mul22(m_mass, Cdot1);
m_impulse.x += impulse1.x;
m_impulse.y += impulse1.y;
b2Vec2 P = impulse1;
vA -= mA * P;
wA -= iA * b2Math.b2Cross(ref m_rA, ref P);
vB += mB * P;
wB += iB * b2Math.b2Cross(ref m_rB, ref P);
}
else
{
b2Vec2 Cdot1 = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA);
float Cdot2 = wB - wA;
b2Vec3 Cdot = new b2Vec3(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 impulse = -b2Math.b2Mul(m_mass, Cdot);
m_impulse += impulse;
b2Vec2 P = b2Vec2.Zero;
P.Set(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(ref m_rA, ref P) + impulse.z);
vB += mB * P;
wB += iB * (b2Math.b2Cross(ref m_rB, ref P) + impulse.z);
}
m_bodyA.InternalVelocity.v = vA;
m_bodyA.InternalVelocity.w = wA;
m_bodyB.InternalVelocity.v = vB;
m_bodyB.InternalVelocity.w = wB;
}
public b2Vec3 Solve33(b2Vec3 b)
{
b2Vec3 ret = new b2Vec3(Box2DPINVOKE.b2Mat33_Solve33(swigCPtr, b2Vec3.getCPtr(b)), true);
if (Box2DPINVOKE.SWIGPendingException.Pending)
{
throw Box2DPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static b2Vec3 Multiply(float s, b2Vec3 a)
{
b2Vec3 ret = new b2Vec3(Box2dPINVOKE.Multiply__SWIG_1(s, b2Vec3.getCPtr(a)), true);
if (Box2dPINVOKE.SWIGPendingException.Pending)
{
throw Box2dPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static float b2Dot(b2Vec3 a, b2Vec3 b)
{
float ret = Box2dPINVOKE.b2Dot__SWIG_1(b2Vec3.getCPtr(a), b2Vec3.getCPtr(b));
if (Box2dPINVOKE.SWIGPendingException.Pending)
{
throw Box2dPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static b2Vec3 b2Cross(b2Vec3 a, b2Vec3 b)
{
b2Vec3 ret = new b2Vec3(Box2dPINVOKE.b2Cross__SWIG_3(b2Vec3.getCPtr(a), b2Vec3.getCPtr(b)), true);
if (Box2dPINVOKE.SWIGPendingException.Pending)
{
throw Box2dPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static b2Vec3 b2Mul(b2Mat33 A, b2Vec3 v)
{
b2Vec3 ret = new b2Vec3(Box2DPINVOKE.b2Mul__SWIG_2(b2Mat33.getCPtr(A), b2Vec3.getCPtr(v)), true);
if (Box2DPINVOKE.SWIGPendingException.Pending)
{
throw Box2DPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public b2Mat33(b2Vec3 c1 = null, b2Vec3 c2 = null, b2Vec3 c3 = null)
{
if (c1 == null && c2 == null && c3 == null)
{
col1.SetZero();
col2.SetZero();
col3.SetZero();
}
else
{
col1.SetV(c1);
col2.SetV(c2);
col3.SetV(c3);
}
}
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
public b2Vec3 Solve33(b2Vec3 b)
{
float det = Utils.b2Dot(ex, Utils.b2Cross(ey, ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
b2Vec3 x = new b2Vec3();
x.x = det * Utils.b2Dot(b, Utils.b2Cross(ey, ez));
x.y = det * Utils.b2Dot(ex, Utils.b2Cross(b, ez));
x.z = det * Utils.b2Dot(ex, Utils.b2Cross(ey, b));
return(x);
}
public void Add(b2Vec3 v)
{
x += v.x; y += v.y; z += v.z;
}
public void SetV(b2Vec3 v)
{
x = v.x;
y = v.y;
z = v.z;
}
public override bool SolvePositionConstraints(float baumgarte)
{
//B2_NOT_USED(baumgarte);
float limitC;
float oldLimitImpulse;
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
b2Vec2 c1 = bA.m_sweep.c;
float a1 = bA.m_sweep.a;
b2Vec2 c2 = bB.m_sweep.c;
float a2 = bB.m_sweep.a;
b2Mat22 tMat;
float tX;
float m1;
float m2;
float i1;
float i2;
// Solve linear limit constraint
float linearError = 0.0f;
float angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
b2Mat22 R1 = b2Mat22.FromAngle(a1);
b2Mat22 R2 = b2Mat22.FromAngle(a2);
//b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);
tMat = R1;
float r1X = m_localAnchor1.x - m_localCenterA.x;
float r1Y = m_localAnchor1.y - m_localCenterA.y;
tX = (tMat.col1.x * r1X + tMat.col2.x * r1Y);
r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
r1X = tX;
//b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);
tMat = R2;
float r2X = m_localAnchor2.x - m_localCenterB.x;
float r2Y = m_localAnchor2.y - m_localCenterB.y;
tX = (tMat.col1.x * r2X + tMat.col2.x * r2Y);
r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
r2X = tX;
float dX = c2.x + r2X - c1.x - r1X;
float dY = c2.y + r2Y - c1.y - r1Y;
if (m_enableLimit)
{
m_axis = b2Math.MulMV(R1, m_localXAxis1);
//m_a1 = b2Math.b2Cross(d + r1, m_axis);
m_a1 = (dX + r1X) * m_axis.y - (dY + r1Y) * m_axis.x;
//m_a2 = b2Math.b2Cross(r2, m_axis);
m_a2 = r2X * m_axis.y - r2Y * m_axis.x;
float translation = m_axis.x * dX + m_axis.y * dY;
if (b2Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop)
{
// Prevent large angular corrections.
C2 = b2Math.Clamp(translation, -b2Settings.b2_maxLinearCorrection, b2Settings.b2_maxLinearCorrection);
linearError = b2Math.Abs(translation);
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large angular corrections and allow some slop.
C2 = b2Math.Clamp(translation - m_lowerTranslation + b2Settings.b2_linearSlop, -b2Settings.b2_maxLinearCorrection, 0.0f);
linearError = m_lowerTranslation - translation;
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large angular corrections and allow some slop.
C2 = b2Math.Clamp(translation - m_upperTranslation + b2Settings.b2_linearSlop, 0.0f, b2Settings.b2_maxLinearCorrection);
linearError = translation - m_upperTranslation;
active = true;
}
}
m_perp = b2Math.MulMV(R1, m_localYAxis1);
//m_s1 = b2Cross(d + r1, m_perp);
m_s1 = (dX + r1X) * m_perp.y - (dY + r1Y) * m_perp.x;
//m_s2 = b2Cross(r2, m_perp);
m_s2 = r2X * m_perp.y - r2Y * m_perp.x;
b2Vec3 impulse = new b2Vec3();
float C1X = m_perp.x * dX + m_perp.y * dY;
float C1Y = a2 - a1 - m_refAngle;
linearError = b2Math.Max(linearError, b2Math.Abs(C1X));
angularError = b2Math.Abs(C1Y);
if (active)
{
m1 = m_invMassA;
m2 = m_invMassB;
i1 = m_invIA;
i2 = m_invIB;
m_K.col1.x = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
m_K.col1.y = i1 * m_s1 + i2 * m_s2;
m_K.col1.z = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
m_K.col2.x = m_K.col1.y;
m_K.col2.y = i1 + i2;
m_K.col2.z = i1 * m_a1 + i2 * m_a2;
m_K.col3.x = m_K.col1.z;
m_K.col3.y = m_K.col2.z;
m_K.col3.z = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
m_K.Solve33(impulse, -C1X, -C1Y, -C2);
}
else
{
m1 = m_invMassA;
m2 = m_invMassB;
i1 = m_invIA;
i2 = m_invIB;
float k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
float k12 = i1 * m_s1 + i2 * m_s2;
float k22 = i1 + i2;
m_K.col1.Set(k11, k12, 0.0f);
m_K.col2.Set(k12, k22, 0.0f);
b2Vec2 impulse1 = m_K.Solve22(new b2Vec2(), -C1X, -C1Y);
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0.0f;
}
float PX = impulse.x * m_perp.x + impulse.z * m_axis.x;
float PY = impulse.x * m_perp.y + impulse.z * m_axis.y;
float L1 = impulse.x * m_s1 + impulse.y + impulse.z * m_a1;
float L2 = impulse.x * m_s2 + impulse.y + impulse.z * m_a2;
c1.x -= m_invMassA * PX;
c1.y -= m_invMassA * PY;
a1 -= m_invIA * L1;
c2.x += m_invMassB * PX;
c2.y += m_invMassB * PY;
a2 += m_invIB * L2;
// TODO_ERIN remove need for this
//bA.m_sweep.c = c1; //Already done by reference
bA.m_sweep.a = a1;
//bB.m_sweep.c = c2; //Already done by reference
bB.m_sweep.a = a2;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
return(linearError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop);
}
public override void SolveVelocityConstraints(b2TimeStep step)
{
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
b2Vec2 v1 = bA.m_linearVelocity;
float w1 = bA.m_angularVelocity;
b2Vec2 v2 = bB.m_linearVelocity;
float w2 = bB.m_angularVelocity;
float PX;
float PY;
float L1;
float L2;
// Solve linear motor constraint
if (m_enableMotor && m_limitState != e_equalLimits)
{
//float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
float Cdot = m_axis.x * (v2.x - v1.x) + m_axis.y * (v2.y - v1.y) + m_a2 * w2 - m_a1 * w1;
float impulse = m_motorMass * (m_motorSpeed - Cdot);
float oldImpulse = m_motorImpulse;
float maxImpulse = step.dt * m_maxMotorForce;
m_motorImpulse = b2Math.Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
PX = impulse * m_axis.x;
PY = impulse * m_axis.y;
L1 = impulse * m_a1;
L2 = impulse * m_a2;
v1.x -= m_invMassA * PX;
v1.y -= m_invMassA * PY;
w1 -= m_invIA * L1;
v2.x += m_invMassB * PX;
v2.y += m_invMassB * PY;
w2 += m_invIB * L2;
}
//Cdot1.x = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
float Cdot1X = m_perp.x * (v2.x - v1.x) + m_perp.y * (v2.y - v1.y) + m_s2 * w2 - m_s1 * w1;
float Cdot1Y = w2 - w1;
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
// Solve prismatic and limit constraint in block form
//Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
float Cdot2 = m_axis.x * (v2.x - v1.x) + m_axis.y * (v2.y - v1.y) + m_a2 * w2 - m_a1 * w1;
b2Vec3 f1 = m_impulse.Copy();
b2Vec3 df = m_K.Solve33(new b2Vec3(), -Cdot1X, -Cdot1Y, -Cdot2);
m_impulse.Add(df);
if (m_limitState == e_atLowerLimit)
{
m_impulse.z = b2Math.Max(m_impulse.z, 0.0f);
}
else if (m_limitState == e_atUpperLimit)
{
m_impulse.z = b2Math.Min(m_impulse.z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot3\(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
//b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.col3.x, m_K.col3.y);
float bX = -Cdot1X - (m_impulse.z - f1.z) * m_K.col3.x;
float bY = -Cdot1Y - (m_impulse.z - f1.z) * m_K.col3.y;
b2Vec2 f2r = m_K.Solve22(new b2Vec2(), bX, bY);
f2r.x += f1.x;
f2r.y += f1.y;
m_impulse.x = f2r.x;
m_impulse.y = f2r.y;
df.x = m_impulse.x - f1.x;
df.y = m_impulse.y - f1.y;
df.z = m_impulse.z - f1.z;
PX = df.x * m_perp.x + df.z * m_axis.x;
PY = df.x * m_perp.y + df.z * m_axis.y;
L1 = df.x * m_s1 + df.y + df.z * m_a1;
L2 = df.x * m_s2 + df.y + df.z * m_a2;
v1.x -= m_invMassA * PX;
v1.y -= m_invMassA * PY;
w1 -= m_invIA * L1;
v2.x += m_invMassB * PX;
v2.y += m_invMassB * PY;
w2 += m_invIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
b2Vec2 df2 = m_K.Solve22(new b2Vec2(), -Cdot1X, -Cdot1Y);
m_impulse.x += df2.x;
m_impulse.y += df2.y;
PX = df2.x * m_perp.x;
PY = df2.x * m_perp.y;
L1 = df2.x * m_s1 + df2.y;
L2 = df2.x * m_s2 + df2.y;
v1.x -= m_invMassA * PX;
v1.y -= m_invMassA * PY;
w1 -= m_invIA * L1;
v2.x += m_invMassB * PX;
v2.y += m_invMassB * PY;
w2 += m_invIB * L2;
}
bA.m_linearVelocity.SetV(v1);
bA.m_angularVelocity = w1;
bB.m_linearVelocity.SetV(v2);
bB.m_angularVelocity = w2;
}
private void GenerateTerrain(bool hardcore)
{
const int GRASS = 0, STUMP = 1, STAIRS = 2, PIT = 3, _STATES_ = 4;
int state = GRASS;
float velocity = 0.0f;
float y = TERRAIN_HEIGHT;
int counter = TERRAIN_STARTPAD;
bool oneshot = false;
terrain_poly.Clear();
terrain.Clear();
terrain_x.Clear();
terrain_y.Clear();
float original_y = 0;
float stair_height = 0;
int stair_width = 0;
int stair_steps = 0;
for (int i = 0; i < TERRAIN_LENGTH; ++i)
{
float x = i * TERRAIN_STEP;
terrain_x.Add(x);
if (state == GRASS && !oneshot)
{
velocity = 0.8f * velocity + 0.01f * Neuro.Tools.Sign(TERRAIN_HEIGHT - y);
if (i > TERRAIN_STARTPAD)
{
velocity += (float)Rng.NextFloat(-1, 1) / SCALE; //1
}
y += velocity;
}
else if (state == PIT && oneshot)
{
counter = Rng.Next(3, 5);
var poly = new []
{
new b2Vec2(x, y),
new b2Vec2(x + TERRAIN_STEP, y),
new b2Vec2(x + TERRAIN_STEP, y - 4 * TERRAIN_STEP),
new b2Vec2(x, y - 4 * TERRAIN_STEP)
};
(fd_polygon.shape as b2PolygonShape).Set(poly);
var t = World.CreateStaticBody(fd_polygon);
t.SetUserData(new CustomBodyData()
{
Color1 = new b2Vec3(1, 1, 1), Color2 = new b2Vec3(0.6f, 0.6f, 0.6f)
});
terrain.Add(t);
(fd_polygon.shape as b2PolygonShape).Set(poly.Select(p => new b2Vec2(p.x + TERRAIN_STEP * counter, p.y)).ToArray());
t = World.CreateStaticBody(fd_polygon);
t.SetUserData(new CustomBodyData()
{
Color1 = new b2Vec3(1, 1, 1), Color2 = new b2Vec3(0.6f, 0.6f, 0.6f)
});
terrain.Add(t);
counter += 2;
original_y = y;
}
else if (state == PIT && !oneshot)
{
y = original_y;
if (counter > 1)
{
y -= 4 * TERRAIN_STEP;
}
}
else if (state == STUMP && oneshot)
{
counter = Rng.Next(1, 3);
var poly = new[]
{
new b2Vec2(x, y),
new b2Vec2(x + counter * TERRAIN_STEP, y),
new b2Vec2(x + counter * TERRAIN_STEP, y + counter * TERRAIN_STEP),
new b2Vec2(x, y + counter * TERRAIN_STEP)
};
(fd_polygon.shape as b2PolygonShape).Set(poly);
var t = World.CreateStaticBody(fd_polygon);
t.SetUserData(new CustomBodyData()
{
Color1 = new b2Vec3(1, 1, 1), Color2 = new b2Vec3(0.6f, 0.6f, 0.6f)
});
terrain.Add(t);
}
else if (state == STAIRS && oneshot)
{
stair_height = Rng.NextDouble() > 0.5 ? 1 : -1;
stair_width = Rng.Next(4, 5);
stair_steps = Rng.Next(3, 5);
original_y = y;
for (int s = 0; s < stair_steps; ++s)
{
var poly = new[]
{
new b2Vec2(x + (s * stair_width) * TERRAIN_STEP, y + (s * stair_height) * TERRAIN_STEP),
new b2Vec2(x + ((1 + s) * stair_width) * TERRAIN_STEP, y + (s * stair_height) * TERRAIN_STEP),
new b2Vec2(x + ((1 + s) * stair_width) * TERRAIN_STEP, y + (-1 + s * stair_height) * TERRAIN_STEP),
new b2Vec2(x + (s * stair_width) * TERRAIN_STEP, y + (-1 + s * stair_height) * TERRAIN_STEP),
};
(fd_polygon.shape as b2PolygonShape).Set(poly);
var t = World.CreateStaticBody(fd_polygon);
t.SetUserData(new CustomBodyData()
{
Color1 = new b2Vec3(1, 1, 1), Color2 = new b2Vec3(0.6f, 0.6f, 0.6f)
});
terrain.Add(t);
}
counter = stair_steps * stair_width;
}
else if (state == STAIRS && !oneshot)
{
var s = stair_steps * stair_width - counter - stair_height;
var n = s / stair_width;
y = original_y + (n * stair_height) * TERRAIN_STEP;
}
oneshot = false;
terrain_y.Add(y);
counter -= 1;
if (counter == 0)
{
counter = Rng.Next(TERRAIN_GRASS / 2, TERRAIN_GRASS);
if (state == GRASS && hardcore)
{
state = Rng.Next(1, _STATES_);
oneshot = true;
}
else
{
state = GRASS;
oneshot = true;
}
}
}
for (int i = 0; i < TERRAIN_LENGTH - 1; ++i)
{
var poly = new[]
{
new b2Vec2(terrain_x[i], terrain_y[i]),
new b2Vec2(terrain_x[i + 1], terrain_y[i + 1])
};
(fd_edge.shape as b2EdgeShape).m_vertex1 = poly[0];
(fd_edge.shape as b2EdgeShape).m_vertex2 = poly[1];
var t = World.CreateStaticBody(fd_edge);
var color = new b2Vec3(0.3f, i % 2 == 0 ? 1.0f : 0.8f, 0.3f);
t.SetUserData(new CustomBodyData()
{
Color1 = color, Color2 = color
});
terrain.Add(t);
color = new b2Vec3(0.4f, 0.6f, 0.3f);
var poly_extended = new List <float[]>
{
new float[] { poly[0].x, poly[0].y },
new float[] { poly[1].x, poly[1].y },
new float[] { poly[1].x, 0 },
new float[] { poly[0].x, 0 }
};
terrain_poly.Add(new Tuple <List <float[]>, b2Vec3> (poly_extended, color));
}
terrain.Reverse();
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
m_invMassA = m_bodyA.InvertedMass;
m_invMassB = m_bodyB.InvertedMass;
m_invIA = m_bodyA.InvertedI;
m_invIB = m_bodyB.InvertedI;
b2Vec2 cA = m_bodyA.InternalPosition.c;
float aA = m_bodyA.InternalPosition.a;
b2Vec2 vA = m_bodyA.InternalVelocity.v;
float wA = m_bodyA.InternalVelocity.w;
b2Vec2 cB = m_bodyB.InternalPosition.c;
float aB = m_bodyB.InternalPosition.a;
b2Vec2 vB = m_bodyB.InternalVelocity.v;
float wB = m_bodyB.InternalVelocity.w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_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 = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
bool fixedRotation = (iA + iB == 0.0f);
b2Vec3 ex = new b2Vec3();
b2Vec3 ey = new b2Vec3();
b2Vec3 ez = new b2Vec3();
ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
ez.x = -m_rA.y * iA - m_rB.y * iB;
ex.y = ey.x;
ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
ez.y = m_rA.x * iA + m_rB.x * iB;
ex.z = ez.x;
ey.z = ez.y;
ez.z = iA + iB;
m_mass = new b2Mat33(ex, ey, ez);
m_motorMass = iA + iB;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
if (m_enableMotor == false || fixedRotation)
{
m_motorImpulse = 0.0f;
}
if (m_enableLimit && fixedRotation == false)
{
float jointAngle = aB - aA - m_referenceAngle;
if (b2Math.b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2Settings.b2_angularSlop)
{
m_limitState = b2LimitState.e_equalLimits;
}
else if (jointAngle <= m_lowerAngle)
{
if (m_limitState != b2LimitState.e_atLowerLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = b2LimitState.e_atLowerLimit;
}
else if (jointAngle >= m_upperAngle)
{
if (m_limitState != b2LimitState.e_atUpperLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = b2LimitState.e_atUpperLimit;
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_impulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
b2Vec2 P = new b2Vec2(m_impulse.x, m_impulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
vB += mB * P;
wB += iB * (b2Math.b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
}
else
{
m_impulse.SetZero();
m_motorImpulse = 0.0f;
}
m_bodyA.InternalVelocity.v = vA;
m_bodyA.InternalVelocity.w = wA;
m_bodyB.InternalVelocity.v = vB;
m_bodyB.InternalVelocity.w = wB;
}
public override void SolveVelocityConstraints(b2SolverData data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
float mA = InvertedMassA, mB = InvertedMassB;
float iA = InvertedIA, iB = InvertedIB;
// Solve linear motor constraint.
if (m_enableMotor && m_limitState != b2LimitState.e_equalLimits)
{
float Cdot = b2Math.b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
float impulse = m_motorMass * (m_motorSpeed - Cdot);
float oldImpulse = m_motorImpulse;
float maxImpulse = data.step.dt * m_maxMotorForce;
m_motorImpulse = b2Math.b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
b2Vec2 P = impulse * m_axis;
float LA = impulse * m_a1;
float LB = impulse * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
b2Vec2 Cdot1 = new b2Vec2();
Cdot1.x = b2Math.b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
Cdot1.y = wB - wA;
if (m_enableLimit && m_limitState != b2LimitState.e_inactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
Cdot2 = b2Math.b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
b2Vec3 Cdot = new b2Vec3(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 f1 = m_impulse;
b2Vec3 df = m_K.Solve33(-Cdot);
m_impulse += df;
if (m_limitState == b2LimitState.e_atLowerLimit)
{
m_impulse.z = Math.Max(m_impulse.z, 0.0f);
}
else if (m_limitState == b2LimitState.e_atUpperLimit)
{
m_impulse.z = Math.Min(m_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)
b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * (new b2Vec2(m_K.ez.x, m_K.ez.y));
b2Vec2 f2r = m_K.Solve22(b) + (new b2Vec2(f1.x, f1.y));
m_impulse.x = f2r.x;
m_impulse.y = f2r.y;
df = m_impulse - f1;
b2Vec2 P = df.x * m_perp + df.z * m_axis;
float LA = df.x * m_s1 + df.y + df.z * m_a1;
float LB = df.x * m_s2 + df.y + df.z * m_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.
b2Vec2 df = m_K.Solve22(-Cdot1);
m_impulse.x += df.x;
m_impulse.y += df.y;
b2Vec2 P = df.x * m_perp;
float LA = df.x * m_s1 + df.y;
float LB = df.x * m_s2 + df.y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
b2Vec2 Cdot10 = Cdot1;
Cdot1.x = b2Math.b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
Cdot1.y = wB - wA;
if (b2Math.b2Abs(Cdot1.x) > 0.01f || b2Math.b2Abs(Cdot1.y) > 0.01f)
{
b2Vec2 test = b2Math.b2Mul22(m_K, df);
Cdot1.x += 0.0f;
}
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
m_invMassA = m_bodyA.InvertedMass;
m_invMassB = m_bodyB.InvertedMass;
m_invIA = m_bodyA.InvertedI;
m_invIB = m_bodyB.InvertedI;
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_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 = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
b2Vec3 ex = new b2Vec3();
b2Vec3 ey = new b2Vec3();
b2Vec3 ez = new b2Vec3();
ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
ez.x = -m_rA.y * iA - m_rB.y * iB;
ex.y = ey.x;
ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
ez.y = m_rA.x * iA + m_rB.x * iB;
ex.z = ez.x;
ey.z = ez.y;
ez.z = iA + iB;
b2Mat33 K = new b2Mat33(ex, ey, ez);
if (m_frequencyHz > 0.0f)
{
m_mass = 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 * b2Settings.b2_pi * m_frequencyHz;
// Damping coefficient
float d = 2.0f * m * 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.ezz = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
m_mass = K.GetSymInverse33(m_mass);
m_gamma = 0.0f;
m_bias = 0.0f;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_impulse *= data.step.dtRatio;
b2Vec2 P = new b2Vec2(m_impulse.x, m_impulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + m_impulse.z);
vB += mB * P;
wB += iB * (b2Math.b2Cross(m_rB, P) + m_impulse.z);
}
else
{
m_impulse.SetZero();
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
public override bool SolvePositionConstraints(b2SolverData data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
float positionError, angularError;
b2Vec3 ex = new b2Vec3();
b2Vec3 ey = new b2Vec3();
b2Vec3 ez = new b2Vec3();
ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
ez.x = -rA.y * iA - rB.y * iB;
ex.y = ey.x;
ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
ez.y = rA.x * iA + rB.x * iB;
ex.z = ez.x;
ey.z = ez.y;
ez.z = iA + iB;
b2Mat33 K = new b2Mat33(ex, ey, ez);
if (m_frequencyHz > 0.0f)
{
b2Vec2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
b2Vec2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * b2Math.b2Cross(rA, P);
cB += mB * P;
aB += iB * b2Math.b2Cross(rB, P);
}
else
{
b2Vec2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - m_referenceAngle;
positionError = C1.Length();
angularError = b2Math.b2Abs(C2);
b2Vec3 C = new b2Vec3(C1.x, C1.y, C2);
b2Vec3 impulse = -K.Solve33(C);
b2Vec2 P = new b2Vec2(impulse.x, impulse.y);
cA -= mA * P;
aA -= iA * (b2Math.b2Cross(rA, P) + impulse.z);
cB += mB * P;
aB += iB * (b2Math.b2Cross(rB, P) + impulse.z);
}
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop);
}
public void SetVVV(b2Vec3 c1, b2Vec3 c2, b2Vec3 c3)
{
col1.SetV(c1);
col2.SetV(c2);
col3.SetV(c3);
}
/// Construct this matrix using columns.
public b2Mat33(b2Vec3 c1, b2Vec3 c2, b2Vec3 c3)
{
ex = c1;
ey = c2;
ez = c3;
}
public b2Vec3 Minus()
{
b2Vec3 ret = new b2Vec3(Box2dPINVOKE.b2Vec3_Minus(swigCPtr), true);
return(ret);
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2Vec3 obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public override void SolveVelocityConstraints(b2SolverData data)
{
b2Vec2 vA = m_bodyA.InternalVelocity.v;
float wA = m_bodyA.InternalVelocity.w;
b2Vec2 vB = m_bodyB.InternalVelocity.v;
float wB = m_bodyB.InternalVelocity.w;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (m_enableMotor && m_limitState != b2LimitState.e_equalLimits && fixedRotation == false)
{
float Cdot = wB - wA - m_motorSpeed;
float impulse = -m_motorMass * Cdot;
float oldImpulse = m_motorImpulse;
float maxImpulse = data.step.dt * m_maxMotorTorque;
m_motorImpulse = b2Math.b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve limit constraint.
if (m_enableLimit && m_limitState != b2LimitState.e_inactiveLimit && fixedRotation == false)
{
b2Vec2 Cdot1 = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA);
float Cdot2 = wB - wA;
b2Vec3 Cdot = new b2Vec3(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 impulse = -m_mass.Solve33(Cdot);
if (m_limitState == b2LimitState.e_equalLimits)
{
m_impulse += impulse;
}
else if (m_limitState == b2LimitState.e_atLowerLimit)
{
float newImpulse = m_impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
b2Vec2 rhs = -Cdot1 + m_impulse.z * (new b2Vec2(m_mass.ez.x, m_mass.ez.y));
b2Vec2 reduced = m_mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
else
{
m_impulse += impulse;
}
}
else if (m_limitState == b2LimitState.e_atUpperLimit)
{
float newImpulse = m_impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
b2Vec2 rhs = -Cdot1 + m_impulse.z * (new b2Vec2(m_mass.ez.x, m_mass.ez.y));
b2Vec2 reduced = m_mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
else
{
m_impulse += impulse;
}
}
b2Vec2 P = new b2Vec2(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + impulse.z);
vB += mB * P;
wB += iB * (b2Math.b2Cross(m_rB, P) + impulse.z);
}
else
{
// Solve point-to-point constraint
b2Vec2 Cdot = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA);
b2Vec2 impulse = m_mass.Solve22(-Cdot);
m_impulse.x += impulse.x;
m_impulse.y += impulse.y;
vA -= mA * impulse;
wA -= iA * b2Math.b2Cross(ref m_rA, ref impulse);
vB += mB * impulse;
wB += iB * b2Math.b2Cross(ref m_rB, ref impulse);
}
m_bodyA.InternalVelocity.v = vA;
m_bodyA.InternalVelocity.w = wA;
m_bodyB.InternalVelocity.v = vB;
m_bodyB.InternalVelocity.w = wB;
}
public void Subtract(b2Vec3 v)
{
x -= v.x; y -= v.y; z -= v.z;
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
InvertedMassA = m_bodyA.InvertedMass;
InvertedMassB = m_bodyB.InvertedMass;
InvertedIA = m_bodyA.InvertedI;
InvertedIB = m_bodyB.InvertedI;
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
// Compute the effective masses.
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = (cB - cA) + rB - rA;
float mA = InvertedMassA, mB = InvertedMassB;
float iA = InvertedIA, iB = InvertedIB;
// Compute motor Jacobian and effective mass.
{
m_axis = b2Math.b2Mul(qA, m_localXAxisA);
m_a1 = b2Math.b2Cross(d + rA, m_axis);
m_a2 = b2Math.b2Cross(rB, m_axis);
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
}
// Prismatic constraint.
{
m_perp = b2Math.b2Mul(qA, m_localYAxisA);
m_s1 = b2Math.b2Cross(d + rA, m_perp);
m_s2 = b2Math.b2Cross(rB, m_perp);
float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
float k12 = iA * m_s1 + iB * m_s2;
float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * m_a1 + iB * m_a2;
float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
m_K.ex.Set(k11, k12, k13);
m_K.ey.Set(k12, k22, k23);
m_K.ez.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
float jointTranslation = b2Math.b2Dot(m_axis, d);
if (b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop)
{
m_limitState = b2LimitState.e_equalLimits;
}
else if (jointTranslation <= m_lowerTranslation)
{
if (m_limitState != b2LimitState.e_atLowerLimit)
{
m_limitState = b2LimitState.e_atLowerLimit;
m_impulse.z = 0.0f;
}
}
else if (jointTranslation >= m_upperTranslation)
{
if (m_limitState != b2LimitState.e_atUpperLimit)
{
m_limitState = b2LimitState.e_atUpperLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
m_impulse.z = 0.0f;
}
if (m_enableMotor == false)
{
m_motorImpulse = 0.0f;
}
if (data.step.warmStarting)
{
// Account for variable time step.
m_impulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
float LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
float LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
m_impulse.SetZero();
m_motorImpulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
/// <summary>
/// Perform the cross product on two vectors.
/// </summary>
public static b2Vec3 Cross(b2Vec3 a, b2Vec3 b)
{
return(new b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x));
}
public override bool SolvePositionConstraints(b2SolverData data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
float mA = InvertedMassA, mB = InvertedMassB;
float iA = InvertedIA, iB = InvertedIB;
// Compute fresh Jacobians
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = cB + rB - cA - rA;
b2Vec2 axis = b2Math.b2Mul(qA, m_localXAxisA);
float a1 = b2Math.b2Cross(d + rA, axis);
float a2 = b2Math.b2Cross(rB, axis);
b2Vec2 perp = b2Math.b2Mul(qA, m_localYAxisA);
float s1 = b2Math.b2Cross(d + rA, perp);
float s2 = b2Math.b2Cross(rB, perp);
b2Vec3 impulse;
b2Vec2 C1 = new b2Vec2();
C1.x = b2Math.b2Dot(perp, d);
C1.y = aB - aA - m_referenceAngle;
float linearError = b2Math.b2Abs(C1.x);
float angularError = b2Math.b2Abs(C1.y);
bool active = false;
float C2 = 0.0f;
if (m_enableLimit)
{
float translation = b2Math.b2Dot(axis, d);
if (b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop)
{
// Prevent large angular corrections
C2 = b2Math.b2Clamp(translation, -b2Settings.b2_maxLinearCorrection, b2Settings.b2_maxLinearCorrection);
linearError = Math.Max(linearError, b2Math.b2Abs(translation));
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Math.b2Clamp(translation - m_lowerTranslation + b2Settings.b2_linearSlop, -b2Settings.b2_maxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, m_lowerTranslation - translation);
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Math.b2Clamp(translation - m_upperTranslation - b2Settings.b2_linearSlop, 0.0f, b2Settings.b2_maxLinearCorrection);
linearError = Math.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;
b2Mat33 K = new b2Mat33(
new b2Vec3(k11, k12, k13),
new b2Vec3(k12, k22, k23),
new b2Vec3(k13, k23, k33));
b2Vec3 C = new b2Vec3(C1.x, C1.y, 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;
}
b2Mat22 K = new b2Mat22();
K.ex.Set(k11, k12);
K.ey.Set(k12, k22);
b2Vec2 impulse1 = K.Solve(-C1);
impulse = new b2Vec3();
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0.0f;
}
b2Vec2 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[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(linearError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop);
}
/// <summary>
/// Perform the dot product on two vectors.
/// </summary>
public static float Dot(b2Vec3 a, b2Vec3 b)
{
return(a.x * b.x + a.y * b.y + a.z * b.z);
}
