/**
* @inheritDoc
*/
public override bool TestPoint(b2Transform xf, b2Vec2 p)
{
b2Vec2 tVec;
//b2Vec2 pLocal = b2MulT(xf.R, p - xf.position);
b2Mat22 tMat = xf.R;
float tX = p.x - xf.position.x;
float tY = p.y - xf.position.y;
float pLocalX = (tX * tMat.col1.x + tY * tMat.col1.y);
float pLocalY = (tX * tMat.col2.x + tY * tMat.col2.y);
for (int i = 0; i < m_vertexCount; ++i)
{
//float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
tVec = m_vertices[i];
tX = pLocalX - tVec.x;
tY = pLocalY - tVec.y;
tVec = m_normals[i];
float dot = (tVec.x * tX + tVec.y * tY);
if (dot > 0.0f)
{
return(false);
}
}
return(true);
}
/**
* @inheritDoc
*/
public override void ComputeAABB(b2AABB aabb, b2Transform transform)
{
b2Mat22 tMat = transform.R;
//b2Vec2 v1 = b2Mul(transform, m_v1);
float v1X = transform.position.x + (tMat.col1.x * m_v1.x + tMat.col2.x * m_v1.y);
float v1Y = transform.position.y + (tMat.col1.y * m_v1.x + tMat.col2.y * m_v1.y);
//b2Vec2 v2 = b2Mul(transform, m_v2);
float v2X = transform.position.x + (tMat.col1.x * m_v2.x + tMat.col2.x * m_v2.y);
float v2Y = transform.position.y + (tMat.col1.y * m_v2.x + tMat.col2.y * m_v2.y);
if (v1X < v2X)
{
aabb.lowerBound.x = v1X;
aabb.upperBound.x = v2X;
}
else
{
aabb.lowerBound.x = v2X;
aabb.upperBound.x = v1X;
}
if (v1Y < v2Y)
{
aabb.lowerBound.y = v1Y;
aabb.upperBound.y = v2Y;
}
else
{
aabb.lowerBound.y = v2Y;
aabb.upperBound.y = v1Y;
}
}
//--------------- Internals Below -------------------
/** @private */
public b2MouseJoint(b2MouseJointDef def) : base(def)
{
//b2Settings.b2Assert(def.target.IsValid());
//b2Settings.b2Assert(b2Math.b2IsValid(def.maxForce) && def.maxForce > 0.0);
//b2Settings.b2Assert(b2Math.b2IsValid(def.frequencyHz) && def.frequencyHz > 0.0);
//b2Settings.b2Assert(b2Math.b2IsValid(def.dampingRatio) && def.dampingRatio > 0.0);
m_target.SetV(def.target);
//m_localAnchor = b2MulT(m_bodyB.m_xf, m_target);
float tX = m_target.x - m_bodyB.m_xf.position.x;
float tY = m_target.y - m_bodyB.m_xf.position.y;
b2Mat22 tMat = m_bodyB.m_xf.R;
m_localAnchor.x = (tX * tMat.col1.x + tY * tMat.col1.y);
m_localAnchor.y = (tX * tMat.col2.x + tY * tMat.col2.y);
m_maxForce = def.maxForce;
m_impulse.SetZero();
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
m_beta = 0.0f;
m_gamma = 0.0f;
}
/**
* @inheritDoc
*/
public override void ComputeAABB(b2AABB aabb, b2Transform xf)
{
//var lower:b2Vec2 = b2Math.MulX(xf, m_vertices[0]);
b2Mat22 tMat = xf.R;
b2Vec2 tVec = m_vertices[0];
float lowerX = xf.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
float lowerY = xf.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
float upperX = lowerX;
float upperY = lowerY;
for (int i = 1; i < m_vertexCount; ++i)
{
tVec = m_vertices[i];
float vX = xf.position.x + (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
float vY = xf.position.y + (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
lowerX = lowerX < vX ? lowerX : vX;
lowerY = lowerY < vY ? lowerY : vY;
upperX = upperX > vX ? upperX : vX;
upperY = upperY > vY ? upperY : vY;
}
aabb.lowerBound.x = lowerX - m_radius;
aabb.lowerBound.y = lowerY - m_radius;
aabb.upperBound.x = upperX + m_radius;
aabb.upperBound.y = upperY + m_radius;
}
/**
* Get the first vertex and apply the supplied transform.
*/
public b2Vec2 GetFirstVertex(b2Transform xf)
{
//return b2Mul(xf, m_coreV1);
b2Mat22 tMat = xf.R;
return(new b2Vec2(xf.position.x + (tMat.col1.x * m_coreV1.x + tMat.col2.x * m_coreV1.y),
xf.position.y + (tMat.col1.y * m_coreV1.x + tMat.col2.y * m_coreV1.y)));
}
public static b2Vec2 b2Mul(b2Mat22 A, b2Vec2 v)
{
b2Vec2 ret = new b2Vec2(Box2DPINVOKE.b2Mul__SWIG_0(b2Mat22.getCPtr(A), b2Vec2.getCPtr(v)), true);
if (Box2DPINVOKE.SWIGPendingException.Pending)
{
throw Box2DPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
/**
* @inheritDoc
*/
public override void ComputeAABB(b2AABB aabb, b2Transform transform)
{
//b2Vec2 p = transform.position + b2Mul(transform.R, m_p);
b2Mat22 tMat = transform.R;
float pX = transform.position.x + (tMat.col1.x * m_p.x + tMat.col2.x * m_p.y);
float pY = transform.position.y + (tMat.col1.y * m_p.x + tMat.col2.y * m_p.y);
aabb.lowerBound.Set(pX - m_radius, pY - m_radius);
aabb.upperBound.Set(pX + m_radius, pY + m_radius);
}
public static b2Mat22 b2MulT(b2Mat22 A, b2Mat22 B)
{
b2Mat22 ret = new b2Mat22(Box2DPINVOKE.b2MulT__SWIG_1(b2Mat22.getCPtr(A), b2Mat22.getCPtr(B)), true);
if (Box2DPINVOKE.SWIGPendingException.Pending)
{
throw Box2DPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static b2Mat22 b2Abs(b2Mat22 A)
{
b2Mat22 ret = new b2Mat22(Box2DPINVOKE.b2Abs__SWIG_2(b2Mat22.getCPtr(A)), true);
if (Box2DPINVOKE.SWIGPendingException.Pending)
{
throw Box2DPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
/**
* @inheritDoc
*/
public override bool RayCast(b2RayCastOutput output, b2RayCastInput input, b2Transform transform)
{
//b2Vec2 position = transform.position + b2Mul(transform.R, m_p);
b2Mat22 tMat = transform.R;
float positionX = transform.position.x + (tMat.col1.x * m_p.x + tMat.col2.x * m_p.y);
float positionY = transform.position.y + (tMat.col1.y * m_p.x + tMat.col2.y * m_p.y);
//b2Vec2 s = input.p1 - position;
float sX = input.p1.x - positionX;
float sY = input.p1.y - positionY;
//float32 b = b2Dot(s, s) - m_radius * m_radius;
float b = (sX * sX + sY * sY) - m_radius * m_radius;
/*// Does the segment start inside the circle?
* if (b < 0.0)
* {
* output.fraction = 0;
* output.hit = e_startsInsideCollide;
* return;
* }*/
// Solve quadratic equation.
//b2Vec2 r = input.p2 - input.p1;
float rX = input.p2.x - input.p1.x;
float rY = input.p2.y - input.p1.y;
//float32 c = b2Dot(s, r);
float c = (sX * rX + sY * rY);
//float32 rr = b2Dot(r, r);
float rr = (rX * rX + rY * rY);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0 || rr < float.MinValue)
{
return(false);
}
// Find the point of intersection of the line with the circle.
float a = -(c + Mathf.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
// manual inline of: output.normal = s + a * r;
output.normal.x = sX + a * rX;
output.normal.y = sY + a * rY;
output.normal.Normalize();
return(true);
}
return(false);
}
/**
* @inheritDoc
*/
public override bool TestPoint(b2Transform transform, b2Vec2 p)
{
//b2Vec2 center = transform.position + b2Mul(transform.R, m_p);
b2Mat22 tMat = transform.R;
float dX = transform.position.x + (tMat.col1.x * m_p.x + tMat.col2.x * m_p.y);
float dY = transform.position.y + (tMat.col1.y * m_p.x + tMat.col2.y * m_p.y);
//b2Vec2 d = p - center;
dX = p.x - dX;
dY = p.y - dY;
//return b2Dot(d, d) <= m_radius * m_radius;
return((dX * dX + dY * dY) <= m_radius * m_radius);
}
/**
* Get the interpolated transform at a specific time.
* @param alpha is a factor in [0,1], where 0 indicates t0.
*/
public void GetTransform(b2Transform xf, float alpha)
{
xf.position.x = (1.0f - alpha) * c0.x + alpha * c.x;
xf.position.y = (1.0f - alpha) * c0.y + alpha * c.y;
float angle = (1.0f - alpha) * a0 + alpha * a;
xf.R.Set(angle);
// Shift to origin
//xf->position -= b2Mul(xf->R, localCenter);
b2Mat22 tMat = xf.R;
xf.position.x -= (tMat.col1.x * localCenter.x + tMat.col2.x * localCenter.y);
xf.position.y -= (tMat.col1.y * localCenter.x + tMat.col2.y * localCenter.y);
}
/** @inheritDoc */
public override float GetReactionTorque(float inv_dt)
{
// TODO_ERIN not tested
//b2Vec2 r = b2Mul(m_bodyB->m_xf.R, m_localAnchor2 - m_bodyB->GetLocalCenter());
b2Mat22 tMat = m_bodyB.m_xf.R;
float rX = m_localAnchor1.x - m_bodyB.m_sweep.localCenter.x;
float rY = m_localAnchor1.y - m_bodyB.m_sweep.localCenter.y;
float tX = tMat.col1.x * rX + tMat.col2.x * rY;
rY = tMat.col1.y * rX + tMat.col2.y * rY;
rX = tX;
//b2Vec2 P = m_impulse * m_J.linearB;
float PX = m_impulse * m_J.linearB.x;
float PY = m_impulse * m_J.linearB.y;
//float32 L = m_impulse * m_J.angularB - b2Cross(r, P);
//return inv_dt * L;
return(inv_dt * (m_impulse * m_J.angularB - rX * PY + rY * PX));
}
public b2Mat22 GetInverse()
{
float a = ex.x;
float b = ey.x;
float c = ex.y;
float d = ey.y;
b2Mat22 B = new b2Mat22();
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
B.ex.x = det * d;
B.ey.x = -det * b;
B.ex.y = -det * c;
B.ey.y = det * a;
return(B);
}
/**
* Get the support point in the given world direction.
* Use the supplied transform.
*/
public b2Vec2 Support(b2Transform xf, float dX, float dY)
{
b2Mat22 tMat = xf.R;
//b2Vec2 v1 = b2Mul(xf, m_coreV1);
float v1X = xf.position.x + (tMat.col1.x * m_coreV1.x + tMat.col2.x * m_coreV1.y);
float v1Y = xf.position.y + (tMat.col1.y * m_coreV1.x + tMat.col2.y * m_coreV1.y);
//b2Vec2 v2 = b2Mul(xf, m_coreV2);
float v2X = xf.position.x + (tMat.col1.x * m_coreV2.x + tMat.col2.x * m_coreV2.y);
float v2Y = xf.position.y + (tMat.col1.y * m_coreV2.x + tMat.col2.y * m_coreV2.y);
if ((v1X * dX + v1Y * dY) > (v2X * dX + v2Y * dY))
{
s_supportVec.x = v1X;
s_supportVec.y = v1Y;
}
else
{
s_supportVec.x = v2X;
s_supportVec.y = v2Y;
}
return(s_supportVec);
}
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);
}
/**
* Computes a polygon's OBB
* @see http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf
*/
static public void ComputeOBB(b2OBB obb, List <b2Vec2> vs, int count)
{
int i;
List <b2Vec2> p = new List <b2Vec2>(count + 1);
for (i = 0; i < count; ++i)
{
p[i] = vs[i];
}
p[count] = p[0];
float minArea = float.MaxValue;
for (i = 1; i <= count; ++i)
{
b2Vec2 root = p[(int)(i - 1)];
//b2Vec2 ux = p[i] - root;
float uxX = p[i].x - root.x;
float uxY = p[i].y - root.y;
//var length:Number = ux.Normalize();
float length = Mathf.Sqrt(uxX * uxX + uxY * uxY);
uxX /= length;
uxY /= length;
//b2Settings.b2Assert(length > Number.MIN_VALUE);
//b2Vec2 uy(-ux.y, ux.x);
float uyX = -uxY;
float uyY = uxX;
//b2Vec2 lower(FLT_MAX, FLT_MAX);
float lowerX = float.MaxValue;
float lowerY = float.MaxValue;
//b2Vec2 upper(-FLT_MAX, -FLT_MAX);
float upperX = -float.MaxValue;
float upperY = -float.MaxValue;
for (int j = 0; j < count; ++j)
{
//b2Vec2 d = p[j] - root;
float dX = p[j].x - root.x;
float dY = p[j].y - root.y;
//b2Vec2 r;
//var rX:Number = b2Dot(ux, d);
float rX = (uxX * dX + uxY * dY);
//var rY:Number = b2Dot(uy, d);
float rY = (uyX * dX + uyY * dY);
//lower = b2Min(lower, r);
if (rX < lowerX)
{
lowerX = rX;
}
if (rY < lowerY)
{
lowerY = rY;
}
//upper = b2Max(upper, r);
if (rX > upperX)
{
upperX = rX;
}
if (rY > upperY)
{
upperY = rY;
}
}
float area = (upperX - lowerX) * (upperY - lowerY);
if (area < 0.95f * minArea)
{
minArea = area;
//obb->R.col1 = ux;
obb.R.col1.x = uxX;
obb.R.col1.y = uxY;
//obb->R.col2 = uy;
obb.R.col2.x = uyX;
obb.R.col2.y = uyY;
//b2Vec2 center = 0.5f * (lower + upper);
float centerX = 0.5f * (lowerX + upperX);
float centerY = 0.5f * (lowerY + upperY);
//obb->center = root + b2Mul(obb->R, center);
b2Mat22 tMat = obb.R;
obb.center.x = root.x + (tMat.col1.x * centerX + tMat.col2.x * centerY);
obb.center.y = root.y + (tMat.col1.y * centerX + tMat.col2.y * centerY);
//obb->extents = 0.5f * (upper - lower);
obb.extents.x = 0.5f * (upperX - lowerX);
obb.extents.y = 0.5f * (upperY - lowerY);
}
}
//b2Settings.b2Assert(minArea < Number.MAX_VALUE);
}
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);
}
internal override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA = m_bodyA.m_sweep.localCenter;
m_localCenterB = 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;
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 mass matrix.
m_rA = Utils.b2Mul(qA, m_linearOffset - m_localCenterA);
m_rB = Utils.b2Mul(qB, -m_localCenterB);
// J = [-I -r1_skew I r2_skew]
// 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;
float mB = m_invMassB;
float iA = m_invIA;
float iB = m_invIB;
// Upper 2 by 2 of K for point to point
b2Mat22 K = new b2Mat22();
K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
m_linearMass = K.GetInverse();
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
m_linearError = cB + m_rB - cA - m_rA;
m_angularError = aB - aA - m_angularOffset;
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse *= data.step.dtRatio;
m_angularImpulse *= data.step.dtRatio;
b2Vec2 P = new b2Vec2(m_linearImpulse.x, m_linearImpulse.y);
vA -= mA * P;
wA -= iA * (Utils.b2Cross(m_rA, P) + m_angularImpulse);
vB += mB * P;
wB += iB * (Utils.b2Cross(m_rB, P) + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 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(b2TimeStep step)
{
b2Mat22 tMat;
float tX;
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
// Compute the effective mass matrix.
//b2Vec2 rA = b2Mul(bA->m_xf.R, m_localAnchorA - bA->GetLocalCenter());
tMat = bA.m_xf.R;
float rAX = m_localAnchorA.x - bA.m_sweep.localCenter.x;
float rAY = m_localAnchorA.y - bA.m_sweep.localCenter.y;
tX = (tMat.col1.x * rAX + tMat.col2.x * rAY);
rAY = (tMat.col1.y * rAX + tMat.col2.y * rAY);
rAX = tX;
//b2Vec2 rB = b2Mul(bB->m_xf.R, m_localAnchorB - bB->GetLocalCenter());
tMat = bB.m_xf.R;
float rBX = m_localAnchorB.x - bB.m_sweep.localCenter.x;
float rBY = m_localAnchorB.y - bB.m_sweep.localCenter.y;
tX = (tMat.col1.x * rBX + tMat.col2.x * rBY);
rBY = (tMat.col1.y * rBX + tMat.col2.y * rBY);
rBX = tX;
// 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 = bA.m_invMass;
float mB = bB.m_invMass;
float iA = bA.m_invI;
float iB = bB.m_invI;
b2Mat22 K = new b2Mat22();
K.col1.x = mA + mB; K.col2.x = 0.0f;
K.col1.y = 0.0f; K.col2.y = mA + mB;
K.col1.x += iA * rAY * rAY; K.col2.x += -iA * rAX * rAY;
K.col1.y += -iA * rAX * rAY; K.col2.y += iA * rAX * rAX;
K.col1.x += iB * rBY * rBY; K.col2.x += -iB * rBX * rBY;
K.col1.y += -iB * rBX * rBY; K.col2.y += iB * rBX * rBX;
K.GetInverse(m_linearMass);
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse.x *= step.dtRatio;
m_linearImpulse.y *= step.dtRatio;
m_angularImpulse *= step.dtRatio;
b2Vec2 P = m_linearImpulse;
bA.m_linearVelocity.x -= mA * P.x;
bA.m_linearVelocity.y -= mA * P.y;
bA.m_angularVelocity -= iA * (rAX * P.y - rAY * P.x + m_angularImpulse);
bB.m_linearVelocity.x += mB * P.x;
bB.m_linearVelocity.y += mB * P.y;
bB.m_angularVelocity += iB * (rBX * P.y - rBY * P.x + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 0.0f;
}
}
internal override void InitVelocityConstraints(b2SolverData data)
{
m_indexB = m_bodyB.m_islandIndex;
m_localCenterB = m_bodyB.m_sweep.localCenter;
m_invMassB = m_bodyB.m_invMass;
m_invIB = m_bodyB.m_invI;
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 qB = new b2Rot(aB);
float mass = m_bodyB.GetMass();
// Frequency
float omega = 2.0f * Settings.b2_pi * m_frequencyHz;
// Damping coefficient
float d = 2.0f * mass * m_dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float h = data.step.dt;
Debug.Assert(d + h * k > float.Epsilon);
m_gamma = h * (d + h * k);
if (m_gamma != 0.0f)
{
m_gamma = 1.0f / m_gamma;
}
m_beta = h * k * m_gamma;
// Compute the effective mass matrix.
m_rB = Utils.b2Mul(qB, m_localAnchorB - m_localCenterB);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
b2Mat22 K = new b2Mat22();
K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
K.ex.y = -m_invIB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
m_mass = K.GetInverse();
m_C = cB + m_rB - m_targetA;
m_C *= m_beta;
// Cheat with some damping
wB *= 0.98f;
if (data.step.warmStarting)
{
m_impulse *= data.step.dtRatio;
vB += m_invMassB * m_impulse;
wB += m_invIB * Utils.b2Cross(m_rB, m_impulse);
}
else
{
m_impulse.SetZero();
}
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
public override bool SolvePositionConstraints(b2SolverData data)
{
b2Vec2 cA = m_bodyA.InternalPosition.c;
float aA = m_bodyA.InternalPosition.a;
b2Vec2 cB = m_bodyB.InternalPosition.c;
float aB = m_bodyB.InternalPosition.a;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
float angularError = 0.0f;
float positionError = 0.0f;
bool fixedRotation = (m_invIA + m_invIB == 0.0f);
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != b2LimitState.e_inactiveLimit && fixedRotation == false)
{
float angle = aB - aA - m_referenceAngle;
float limitImpulse = 0.0f;
if (m_limitState == b2LimitState.e_equalLimits)
{
// Prevent large angular corrections
float C = b2Math.b2Clamp(angle - m_lowerAngle, -b2Settings.b2_maxAngularCorrection, b2Settings.b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = b2Math.b2Abs(C);
}
else if (m_limitState == b2LimitState.e_atLowerLimit)
{
float C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = b2Math.b2Clamp(C + b2Settings.b2_angularSlop, -b2Settings.b2_maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == b2LimitState.e_atUpperLimit)
{
float C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = b2Math.b2Clamp(C - b2Settings.b2_angularSlop, 0.0f, b2Settings.b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
aA -= m_invIA * limitImpulse;
aB += m_invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 C = cB + rB - cA - rA;
positionError = C.Length;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
b2Mat22 K = new b2Mat22();
K.exx = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
K.exy = -iA * rA.x * rA.y - iB * rB.x * rB.y;
K.eyx = K.ex.y;
K.eyy = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
b2Vec2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * b2Math.b2Cross(rA, impulse);
cB += mB * impulse;
aB += iB * b2Math.b2Cross(rB, impulse);
}
m_bodyA.InternalPosition.c = cA;
m_bodyA.InternalPosition.a = aA;
m_bodyB.InternalPosition.c = cB;
m_bodyB.InternalPosition.a = aB;
return(positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop);
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2Mat22 obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public b2Mat22 GetInverse()
{
b2Mat22 ret = new b2Mat22(Box2DPINVOKE.b2Mat22_GetInverse(swigCPtr), true);
return(ret);
}
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;
float aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
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 mass matrix.
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;
b2Mat22 K = new b2Mat22();
K.exx = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
K.exy = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
K.eyx = K.ex.y;
K.eyy = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
m_linearMass = K.GetInverse();
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse *= data.step.dtRatio;
m_angularImpulse *= data.step.dtRatio;
b2Vec2 P = new b2Vec2(m_linearImpulse.x, m_linearImpulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + m_angularImpulse);
vB += mB * P;
wB += iB * (b2Math.b2Cross(m_rB, P) + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 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;
}
