/// <summary>
/// Compute the inverse of this matrix, such that inv(A) * A = identity.
/// </summary>
public Mat22 Invert()
{
float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y;
Mat22 B = new Mat22();
float det = a * d - b * c;
Box2DXDebug.Assert(det != 0.0f);
det = 1.0f / det;
B.Col1.X = det * d; B.Col2.X = -det * b;
B.Col1.Y = -det * c; B.Col2.Y = det * a;
return B;
}
internal PolygonShape(ShapeDef def)
: base(def)
{
Box2DXDebug.Assert(def.Type == ShapeType.PolygonShape);
_type = ShapeType.PolygonShape;
PolygonDef poly = (PolygonDef)def;
// Get the vertices transformed into the body frame.
_vertexCount = poly.VertexCount;
Box2DXDebug.Assert(3 <= _vertexCount && _vertexCount <= Settings.MaxPolygonVertices);
// Copy vertices.
for (int i = 0; i < _vertexCount; ++i)
{
_vertices[i] = poly.Vertices[i];
}
// Compute normals. Ensure the edges have non-zero length.
for (int i = 0; i < _vertexCount; ++i)
{
int i1 = i;
int i2 = i + 1 < _vertexCount ? i + 1 : 0;
Vec2 edge = _vertices[i2] - _vertices[i1];
Box2DXDebug.Assert(edge.LengthSquared() > Common.Settings.FLT_EPSILON * Common.Settings.FLT_EPSILON);
_normals[i] = Vec2.Cross(edge, 1.0f);
_normals[i].Normalize();
}
#if DEBUG
// Ensure the polygon is convex.
for (int i = 0; i < _vertexCount; ++i)
{
for (int j = 0; j < _vertexCount; ++j)
{
// Don't check vertices on the current edge.
if (j == i || j == (i + 1) % _vertexCount)
{
continue;
}
// Your polygon is non-convex (it has an indentation).
// Or your polygon is too skinny.
float s = Vec2.Dot(_normals[i], _vertices[j] - _vertices[i]);
Box2DXDebug.Assert(s < -Settings.LinearSlop);
}
}
// Ensure the polygon is counter-clockwise.
for (int i = 1; i < _vertexCount; ++i)
{
float cross = Vec2.Cross(_normals[i - 1], _normals[i]);
// Keep asinf happy.
cross = Common.Math.Clamp(cross, -1.0f, 1.0f);
// You have consecutive edges that are almost parallel on your polygon.
float angle = (float)System.Math.Asin(cross);
Box2DXDebug.Assert(angle > Settings.AngularSlop);
}
#endif
// Compute the polygon centroid.
_centroid = ComputeCentroid(poly.Vertices, poly.VertexCount);
// Compute the oriented bounding box.
ComputeOBB(out _obb, _vertices, _vertexCount);
// Create core polygon shape by shifting edges inward.
// Also compute the min/max radius for CCD.
for (int i = 0; i < _vertexCount; ++i)
{
int i1 = i - 1 >= 0 ? i - 1 : _vertexCount - 1;
int i2 = i;
Vec2 n1 = _normals[i1];
Vec2 n2 = _normals[i2];
Vec2 v = _vertices[i] - _centroid; ;
Vec2 d = new Vec2();
d.X = Vec2.Dot(n1, v) - Settings.ToiSlop;
d.Y = Vec2.Dot(n2, v) - Settings.ToiSlop;
// Shifting the edge inward by b2_toiSlop should
// not cause the plane to pass the centroid.
// Your shape has a radius/extent less than b2_toiSlop.
Box2DXDebug.Assert(d.X >= 0.0f);
Box2DXDebug.Assert(d.Y >= 0.0f);
Mat22 A = new Mat22();
A.Col1.X = n1.X; A.Col2.X = n1.Y;
A.Col1.Y = n2.X; A.Col2.Y = n2.Y;
_coreVertices[i] = A.Solve(d) + _centroid;
}
}
/// <summary>
/// Initialize using a position vector and a rotation matrix.
/// </summary>
/// <param name="position"></param>
/// <param name="R"></param>
public Transform(Vector2 position, Mat22 rotation)
{
this.position = position;
this.rotation = rotation;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit.
Body b1 = _body1;
Body b2 = _body2;
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float C = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Box2DXMath.Abs(C);
}
else if (_limitState == LimitState.AtLowerLimit)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = Box2DXMath.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = Box2DXMath.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1._sweep.A -= b1._invI * limitImpulse;
b2._sweep.A += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 C = b2._sweep.C + r2 - b1._sweep.C - r1;
positionError = C.Length();
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Handle large detachment.
float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vec2 u = C; u.Normalize();
float k = invMass1 + invMass2;
Box2DXDebug.Assert(k > Settings.FLT_EPSILON);
float m = 1.0f / k;
Vec2 impulse = m * (-C);
float k_beta = 0.5f;
b1._sweep.C -= k_beta * invMass1 * impulse;
b2._sweep.C += k_beta * invMass2 * impulse;
C = b2._sweep.C + r2 - b1._sweep.C - r1;
}
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
Vec2 impulse_ = K.Solve(-C);
b1._sweep.C -= b1._invMass * impulse_;
b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse_);
b2._sweep.C += b2._invMass * impulse_;
b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse_);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
/// <summary>
/// Multiply a matrix times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another.
/// </summary>
public static Vec2 Mul(Mat22 A, Vec2 v)
{
return(new Vec2(A.Col1.X * v.X + A.Col2.X * v.Y, A.Col1.Y * v.X + A.Col2.Y * v.Y));
}
/// <summary>
/// A * B
/// </summary>
public static Mat22 Mul(Mat22 A, Mat22 B)
{
return(new Mat22(Mul(A, B.Col1), Mul(A, B.Col2)));
}
/// Set this based on the position and angle.
public void Set(Vector2 p, float angle)
{
position = p;
R = new Mat22(angle);
}
/// <summary>
/// Initialize using a position vector and a rotation matrix.
/// </summary>
/// <param name="position"></param>
/// <param name="R"></param>
public XForm(Vec2 position, Mat22 rotation)
{
Position = position;
R = rotation;
}
/// <summary>
/// Initialize using a position vector and a rotation matrix.
/// </summary>
/// <param name="position"></param>
/// <param name="R"></param>
public XForm(Vector2 p, Mat22 rotation)
{
position = p;
R = rotation;
}
/// <summary>
/// Set this to the identity transform.
/// </summary>
public void SetIdentity()
{
position = Vector2.Zero;
R = Mat22.Identity;
}
/// <summary> /// Set the position of the body's origin and rotation (radians). /// This breaks any contacts and wakes the other bodies. /// </summary> /// <param name="position">The new world position of the body's origin (not necessarily /// the center of mass).</param> /// <param name="angle">The new world rotation angle of the body in radians.</param> /// <returns>Return false if the movement put a shape outside the world. In this case the /// body is automatically frozen.</returns> public bool SetTransform(Vector2 position, Mat22 rotation)
/// <summary>
/// Set this to the identity transform.
/// </summary>
public void SetIdentity()
{
position = Vector2.zero;
R = Mat22.Identity;
}
/// Set this based on the position and angle.
public void Set(Vector2 p, float angle)
{
position = p;
R = new Mat22(angle);
}
/// <summary>
/// Initialize using a position vector and a rotation matrix.
/// </summary>
/// <param name="position"></param>
/// <param name="R"></param>
public XForm(Vector2 p, Mat22 rotation)
{
position = p;
R = rotation;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body body = this._body2;
float mass = body.GetMass();
float num = 2f * Settings.Pi * this._frequencyHz;
float num2 = 2f * mass * this._dampingRatio * num;
float num3 = mass * (num * num);
Box2DXDebug.Assert(num2 + step.Dt * num3 > Settings.FLT_EPSILON);
this._gamma = 1f / (step.Dt * (num2 + step.Dt * num3));
this._beta = step.Dt * num3 * this._gamma;
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor - body.GetLocalCenter());
float invMass = body._invMass;
float invI = body._invI;
Mat22 a = default(Mat22);
a.Col1.X = invMass;
a.Col2.X = 0f;
a.Col1.Y = 0f;
a.Col2.Y = invMass;
Mat22 b = default(Mat22);
b.Col1.X = invI * vec.Y * vec.Y;
b.Col2.X = -invI * vec.X * vec.Y;
b.Col1.Y = -invI * vec.X * vec.Y;
b.Col2.Y = invI * vec.X * vec.X;
Mat22 mat = a + b;
mat.Col1.X = mat.Col1.X + this._gamma;
mat.Col2.Y = mat.Col2.Y + this._gamma;
this._mass = mat.Invert();
this._C = body._sweep.C + vec - this._target;
body._angularVelocity *= 0.98f;
this._impulse *= step.DtRatio;
Body expr_21D = body;
expr_21D._linearVelocity += invMass * this._impulse;
body._angularVelocity += invI * Vec2.Cross(vec, this._impulse);
}
/// <summary>
/// Multiply a matrix times a vector. If a rotation matrix is provided,
/// then this Transforms the vector from one frame to another.
/// </summary>
public static Vector2 Mul(Mat22 A, Vector2 v)
{
return(new Vector2(A.Col1.x * v.x + A.Col2.x * v.y, A.Col1.y * v.x + A.Col2.y * v.y));
}
public XForm(Vec2 position, Mat22 rotation)
{
this.Position = position;
this.R = rotation;
}
public static Mat22 Abs(Mat22 A)
{
return(new Mat22(Abs(A.Col1), Abs(A.Col2)));
}
public XForm(Vec2 position, Mat22 rotation)
{
this.Position = position;
this.R = rotation;
}
/// <summary>
/// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another (inverse transform).
/// </summary>
public static Vec2 MulT(Mat22 A, Vec2 v)
{
return(new Vec2(Vec2.Dot(v, A.Col1), Vec2.Dot(v, A.Col2)));
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// Compute the effective mass matrix.
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
// 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]
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
_pivotMass = K.Invert();
_motorMass = 1.0f / (invI1 + invI2);
if (_enableMotor == false)
{
_motorForce = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitForce = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitForce = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_limitForce = 0.0f;
}
}
else
{
_limitForce = 0.0f;
}
if (step.WarmStarting)
{
b1._linearVelocity -= Settings.FORCE_SCALE(step.Dt) * invMass1 * _pivotForce;
b1._angularVelocity -= Settings.FORCE_SCALE(step.Dt) * invI1 * (Vec2.Cross(r1, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce));
b2._linearVelocity += Settings.FORCE_SCALE(step.Dt) * invMass2 * _pivotForce;
b2._angularVelocity += Settings.FORCE_SCALE(step.Dt) * invI2 * (Vec2.Cross(r2, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce));
}
else
{
_pivotForce.SetZero();
_motorForce = 0.0f;
_limitForce = 0.0f;
}
_limitPositionImpulse = 0.0f;
}
/// <summary>
/// Initialize using a position vector and a rotation matrix.
/// </summary>
/// <param name="position"></param>
/// <param name="R"></param>
public XForm(Vec2 position, Mat22 rotation)
{
Position = position;
R = rotation;
}
internal override bool SolvePositionConstraints()
{
Body b1 = _body1;
Body b2 = _body2;
float positionError = 0.0f;
// Solve point-to-point position error.
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 ptpC = p2 - p1;
positionError = ptpC.Length();
// Prevent overly large corrections.
//b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
//ptpC = b2Clamp(ptpC, -dpMax, dpMax);
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
Vec2 impulse = K.Solve(-ptpC);
b1._sweep.C -= b1._invMass * impulse;
b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse);
b2._sweep.C += b2._invMass * impulse;
b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
// Handle limits.
float angularError = 0.0f;
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * limitC;
angularError = Box2DXMath.Abs(limitC);
}
else if (_limitState == LimitState.AtLowerLimit)
{
float limitC = angle - _lowerAngle;
angularError = Box2DXMath.Max(0.0f, -limitC);
// Prevent large angular corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float limitC = angle - _upperAngle;
angularError = Box2DXMath.Max(0.0f, limitC);
// Prevent large angular corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
b1._sweep.A -= b1._invI * limitImpulse;
b2._sweep.A += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
/// <summary>
/// Initialize using a position vector and a rotation matrix.
/// </summary>
/// <param name="position"></param>
/// <param name="R"></param>
public Transform(Vector2 position, Quaternion rotation)
{
this.position = position;
this.rotation = rotation;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = body._sweep.C;
float num = body._sweep.A;
Vec2 vec2 = body2._sweep.C;
float num2 = body2._sweep.A;
float num3 = 0f;
bool flag = false;
float y = 0f;
Mat22 a = new Mat22(num);
Mat22 a2 = new Mat22(num2);
Vec2 v = Box2DX.Common.Math.Mul(a, this._localAnchor1 - this._localCenter1);
Vec2 vec3 = Box2DX.Common.Math.Mul(a2, this._localAnchor2 - this._localCenter2);
Vec2 vec4 = vec2 + vec3 - vec - v;
if (this._enableLimit)
{
this._axis = Box2DX.Common.Math.Mul(a, this._localXAxis1);
this._a1 = Vec2.Cross(vec4 + v, this._axis);
this._a2 = Vec2.Cross(vec3, this._axis);
float num4 = Vec2.Dot(this._axis, vec4);
if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
{
y = Box2DX.Common.Math.Clamp(num4, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
num3 = Box2DX.Common.Math.Abs(num4);
flag = true;
}
else
{
if (num4 <= this._lowerTranslation)
{
y = Box2DX.Common.Math.Clamp(num4 - this._lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
num3 = this._lowerTranslation - num4;
flag = true;
}
else
{
if (num4 >= this._upperTranslation)
{
y = Box2DX.Common.Math.Clamp(num4 - this._upperTranslation - Settings.LinearSlop, 0f, Settings.MaxLinearCorrection);
num3 = num4 - this._upperTranslation;
flag = true;
}
}
}
}
this._perp = Box2DX.Common.Math.Mul(a, this._localYAxis1);
this._s1 = Vec2.Cross(vec4 + v, this._perp);
this._s2 = Vec2.Cross(vec3, this._perp);
float num5 = Vec2.Dot(this._perp, vec4);
num3 = Box2DX.Common.Math.Max(num3, Box2DX.Common.Math.Abs(num5));
float num6 = 0f;
Vec2 vec5;
if (flag)
{
float invMass = this._invMass1;
float invMass2 = this._invMass2;
float invI = this._invI1;
float invI2 = this._invI2;
float num7 = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
float num8 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2;
float y2 = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;
this._K.Col1.Set(num7, num8);
this._K.Col2.Set(num8, y2);
vec5 = this._K.Solve(-new Vec2
{
X = num5,
Y = y
});
}
else
{
float invMass = this._invMass1;
float invMass2 = this._invMass2;
float invI = this._invI1;
float invI2 = this._invI2;
float num7 = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
float x = -num5 / num7;
vec5.X = x;
vec5.Y = 0f;
}
Vec2 v2 = vec5.X * this._perp + vec5.Y * this._axis;
float num9 = vec5.X * this._s1 + vec5.Y * this._a1;
float num10 = vec5.X * this._s2 + vec5.Y * this._a2;
vec -= this._invMass1 * v2;
num -= this._invI1 * num9;
vec2 += this._invMass2 * v2;
num2 += this._invI2 * num10;
body._sweep.C = vec;
body._sweep.A = num;
body2._sweep.C = vec2;
body2._sweep.A = num2;
body.SynchronizeTransform();
body2.SynchronizeTransform();
return num3 <= Settings.LinearSlop && num6 <= Settings.AngularSlop;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 c1 = b1._sweep.C;
float a1 = b1._sweep.A;
Vector2 c2 = b2._sweep.C;
float a2 = b2._sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);
Vector2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1);
Vector2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = Box2DX.Common.Math.Mul(R1, _localXAxis1);
_a1 = (d + r1).Cross(_axis);
_a2 = r2.Cross(_axis);
float translation = Vector2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = Mathf.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Box2DX.Common.Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Mathf.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Mathf.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = Box2DX.Common.Math.Mul(R1, _localYAxis1);
_s1 = (d + r1).Cross(_perp);
_s2 = r2.Cross(_perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.x = Vector2.Dot(_perp, d);
C1.y = a2 - a1 - _refAngle;
linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1.x));
angularError = Box2DX.Common.Math.Abs(C1.y);
if (active)
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.x = C1.x;
C.y = C1.y;
C.z = C2;
impulse = _K.Solve33(-C);
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k22 = i1 + i2;
_K.Col1 = new Vector3(k11, k12, 0.0f);
_K.Col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0.0f;
}
Vector2 P = impulse.x * _perp + impulse.z * _axis;
float L1 = impulse.x * _s1 + impulse.y + impulse.z * _a1;
float L2 = impulse.x * _s2 + impulse.y + impulse.z * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.C = c1;
b1._sweep.A = a1;
b2._sweep.C = c2;
b2._sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vec2 c1 = b1._sweep.C;
float a1 = b1._sweep.A;
Vec2 c2 = b2._sweep.C;
float a2 = b2._sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);
Vec2 r1 = Math.Mul(R1, _localAnchor1 - _localCenter1);
Vec2 r2 = Math.Mul(R2, _localAnchor2 - _localCenter2);
Vec2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = Math.Mul(R1, _localXAxis1);
_a1 = Vec2.Cross(d + r1, _axis);
_a2 = Vec2.Cross(r2, _axis);
float translation = Vec2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = Math.Mul(R1, _localYAxis1);
_s1 = Vec2.Cross(d + r1, _perp);
_s2 = Vec2.Cross(r2, _perp);
Vec2 impulse;
float C1;
C1 = Vec2.Dot(_perp, d);
linearError = Math.Max(linearError, Math.Abs(C1));
angularError = 0.0f;
if (active)
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1.Set(k11, k12);
_K.Col2.Set(k12, k22);
Vec2 C = new Vec2();
C.X = C1;
C.Y = C2;
impulse = _K.Solve(-C);
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float impulse1;
if (k11 != 0.0f)
{
impulse1 = -C1 / k11;
}
else
{
impulse1 = 0.0f;
}
impulse.X = impulse1;
impulse.Y = 0.0f;
}
Vec2 P = impulse.X * _perp + impulse.Y * _axis;
float L1 = impulse.X * _s1 + impulse.Y * _a1;
float L2 = impulse.X * _s2 + impulse.Y * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.C = c1;
b1._sweep.A = a1;
b2._sweep.C = c2;
b2._sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
public static Mat22 operator +(Mat22 A, Mat22 B)
{
Mat22 C = new Mat22();
C.Set(A.Col1 + B.Col1, A.Col2 + B.Col2);
return C;
}
/// <summary>
/// Compute the inverse of this matrix, such that inv(A) * A = identity.
/// </summary>
public Mat22 GetInverse()
{
float a = Col1.x, b = Col2.x, c = Col1.y, d = Col2.y;
Mat22 B = new Mat22();
float det = a * d - b * c;
Box2DXDebug.Assert(det != 0.0f);
det = 1.0f / det;
B.Col1.x = det * d; B.Col2.x = -det * b;
B.Col1.y = -det * c; B.Col2.y = det * a;
return B;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b = _body2;
float mass = b.GetMass();
// Frequency
float omega = 2.0f * Settings.Pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * mass * _dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Box2DXDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON);
_gamma = 1.0f / (step.Dt * (d + step.Dt * k));
_beta = step.Dt * k * _gamma;
// Compute the effective mass matrix.
Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter());
// 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]
float invMass = b._invMass;
float invI = b._invI;
Mat22 K1 = new Mat22();
K1.Col1.x = invMass; K1.Col2.x = 0.0f;
K1.Col1.y = 0.0f; K1.Col2.y = invMass;
Mat22 K2 = new Mat22();
K2.Col1.x = invI * r.y * r.y; K2.Col2.x = -invI * r.x * r.y;
K2.Col1.y = -invI * r.x * r.y; K2.Col2.y = invI * r.x * r.x;
Mat22 K = K1 + K2;
K.Col1.x += _gamma;
K.Col2.y += _gamma;
_mass = K.GetInverse();
_C = b._sweep.C + r - _target;
// Cheat with some damping
b._angularVelocity *= 0.98f;
// Warm starting.
_impulse *= step.DtRatio;
b._linearVelocity += invMass * _impulse;
b._angularVelocity += invI * r.Cross(_impulse);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b = _body2;
// Compute the effective mass matrix.
Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter());
// 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]
float invMass = b._invMass;
float invI = b._invI;
Mat22 K1 = new Mat22();
K1.Col1.X = invMass; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass;
Mat22 K2 = new Mat22();
K2.Col1.X = invI * r.Y * r.Y; K2.Col2.X = -invI * r.X * r.Y;
K2.Col1.Y = -invI * r.X * r.Y; K2.Col2.Y = invI * r.X * r.X;
Mat22 K = K1 + K2;
K.Col1.X += _gamma;
K.Col2.Y += _gamma;
_mass = K.Invert();
_C = b._sweep.C + r - _target;
// Cheat with some damping
b._angularVelocity *= 0.98f;
// Warm starting.
Vec2 P = Settings.FORCE_SCALE(step.Dt) * _impulse;
b._linearVelocity += invMass * P;
b._angularVelocity += invI * Vec2.Cross(r, P);
}
