internal override void SolveVelocityConstraints(TimeStep step)
{
Body b = _body2;
Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter());
// Cdot = v + cross(w, r)
Vec2 Cdot = b._linearVelocity + Vec2.Cross(b._angularVelocity, r);
Vec2 force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * Common.Math.Mul(_mass, Cdot +
(_beta * step.Inv_Dt) * _C + Settings.FORCE_SCALE(step.Dt) * (_gamma * _impulse));
Vec2 oldForce = _impulse;
_impulse += force;
float forceMagnitude = _impulse.Length();
if (forceMagnitude > _maxForce)
{
_impulse *= _maxForce / forceMagnitude;
}
force = _impulse - oldForce;
Vec2 P = Settings.FORCE_SCALE(step.Dt) * force;
b._linearVelocity += b._invMass * P;
b._angularVelocity += b._invI * Vec2.Cross(r, P);
}
internal override void SolveVelocityConstraints(TimeStep step)
{
//B2_NOT_USED(step);
Body b1 = _body1;
Body b2 = _body2;
Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
// Cdot = dot(u, v + cross(w, r))
Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1);
Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);
float Cdot = Vec2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vec2 P = impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
bool result;
if (this._frequencyHz > 0f)
{
result = true;
}
else
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 u = body2._sweep.C + vec2 - body._sweep.C - vec;
float num = u.Normalize();
float num2 = num - this._length;
num2 = Box2DX.Common.Math.Clamp(num2, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float a = -this._mass * num2;
this._u = u;
Vec2 vec3 = a * this._u;
Body expr_EC_cp_0 = body;
expr_EC_cp_0._sweep.C = expr_EC_cp_0._sweep.C - body._invMass * vec3;
Body expr_10F_cp_0 = body;
expr_10F_cp_0._sweep.A = expr_10F_cp_0._sweep.A - body._invI * Vec2.Cross(vec, vec3);
Body expr_130_cp_0 = body2;
expr_130_cp_0._sweep.C = expr_130_cp_0._sweep.C + body2._invMass * vec3;
Body expr_153_cp_0 = body2;
expr_153_cp_0._sweep.A = expr_153_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec3);
body.SynchronizeTransform();
body2.SynchronizeTransform();
result = (System.Math.Abs(num2) < Settings.LinearSlop);
}
return(result);
}
internal override bool SolvePositionConstraints()
{
if (_frequencyHz > 0.0f)
{
return(true);
}
Body b1 = _body1;
Body b2 = _body2;
Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
float length = d.Normalize();
float C = length - _length;
C = Common.Math.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float impulse = -_mass * C;
_u = d;
Vec2 P = impulse * _u;
b1._sweep.C -= b1._invMass * P;
b1._sweep.A -= b1._invI * Vec2.Cross(r1, P);
b2._sweep.C += b2._invMass * P;
b2._sweep.A += b2._invI * Vec2.Cross(r2, P);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(System.Math.Abs(C) < Settings.LinearSlop);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body g1 = _ground1;
Body g2 = _ground2;
Body b1 = _body1;
Body b2 = _body2;
float K = 0.0f;
_J.SetZero();
if (_revolute1 != null)
{
_J.Angular1 = -1.0f;
K += b1._invI;
}
else
{
Vec2 ug = Common.Math.Mul(g1.GetXForm().R, _prismatic1._localXAxis1);
Vec2 r = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
float crug = Vec2.Cross(r, ug);
_J.Linear1 = -ug;
_J.Angular1 = -crug;
K += b1._invMass + b1._invI * crug * crug;
}
if (_revolute2 != null)
{
_J.Angular2 = -_ratio;
K += _ratio * _ratio * b2._invI;
}
else
{
Vec2 ug = Common.Math.Mul(g2.GetXForm().R, _prismatic2._localXAxis1);
Vec2 r = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
float crug = Vec2.Cross(r, ug);
_J.Linear2 = -_ratio * ug;
_J.Angular2 = -_ratio * crug;
K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
}
// Compute effective mass.
Box2DXDebug.Assert(K > 0.0f);
_mass = 1.0f / K;
if (step.WarmStarting)
{
// Warm starting.
float P = Settings.FORCE_SCALE(step.Dt) * _force;
b1._linearVelocity += b1._invMass * P * _J.Linear1;
b1._angularVelocity += b1._invI * P * _J.Angular1;
b2._linearVelocity += b2._invMass * P * _J.Linear2;
b2._angularVelocity += b2._invI * P * _J.Angular2;
}
else
{
_force = 0.0f;
}
}
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.
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.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 * Vec2.Cross(r, _impulse);
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
if (_state == LimitState.AtUpperLimit)
{
Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1);
Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);
float Cdot = -Vec2.Dot(_u1, v1) - _ratio * Vec2.Dot(_u2, v2);
float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _pulleyMass * Cdot;
float oldForce = _force;
_force = Box2DXMath.Max(0.0f, _force + force);
force = _force - oldForce;
Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _u1;
Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * _ratio * force * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
}
if (_limitState1 == LimitState.AtUpperLimit)
{
Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1);
float Cdot = -Vec2.Dot(_u1, v1);
float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass1 * Cdot;
float oldForce = _limitForce1;
_limitForce1 = Box2DXMath.Max(0.0f, _limitForce1 + force);
force = _limitForce1 - oldForce;
Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _u1;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
}
if (_limitState2 == LimitState.AtUpperLimit)
{
Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);
float Cdot = -Vec2.Dot(_u2, v2);
float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass2 * Cdot;
float oldForce = _limitForce2;
_limitForce2 = Box2DXMath.Max(0.0f, _limitForce2 + force);
force = _limitForce2 - oldForce;
Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * force * _u2;
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
if (_state == LimitState.AtUpperLimit)
{
Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1);
Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);
float Cdot = -Vec2.Dot(_u1, v1) - _ratio * Vec2.Dot(_u2, v2);
float impulse = _pulleyMass * (-Cdot);
float oldImpulse = _impulse;
_impulse = Box2DX.Common.Math.Max(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
Vec2 P1 = -impulse * _u1;
Vec2 P2 = -_ratio * impulse * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
}
if (_limitState1 == LimitState.AtUpperLimit)
{
Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1);
float Cdot = -Vec2.Dot(_u1, v1);
float impulse = -_limitMass1 * Cdot;
float oldImpulse = _limitImpulse1;
_limitImpulse1 = Box2DX.Common.Math.Max(0.0f, _limitImpulse1 + impulse);
impulse = _limitImpulse1 - oldImpulse;
Vec2 P1 = -impulse * _u1;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
}
if (_limitState2 == LimitState.AtUpperLimit)
{
Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);
float Cdot = -Vec2.Dot(_u2, v2);
float impulse = -_limitMass2 * Cdot;
float oldImpulse = _limitImpulse2;
_limitImpulse2 = Box2DX.Common.Math.Max(0.0f, _limitImpulse2 + impulse);
impulse = _limitImpulse2 - oldImpulse;
Vec2 P2 = -impulse * _u2;
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
}
}
public void Evaluate()
{
Body bodyA = _fixtureA.Body;
Body bodyB = _fixtureB.Body;
Box2DXDebug.Assert(CollideShapeFunction != null);
CollideShapeFunction(ref _manifold, _fixtureA.Shape, bodyA.GetXForm(), _fixtureB.Shape, bodyB.GetXForm());
}
public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body b1 = c.Body1;
Body b2 = c.Body2;
float invMass1 = b1._mass * b1._invMass;
float invI1 = b1._mass * b1._invI;
float invMass2 = b2._mass * b2._invMass;
float invI2 = b2._mass * b2._invI;
Vec2 normal = c.Normal;
// Solver normal constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, ccp.LocalAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, ccp.LocalAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 dp = p2 - p1;
// Approximate the current separation.
float separation = Vec2.Dot(dp, normal) + ccp.Separation;
// Track max constraint error.
minSeparation = Common.Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = baumgarte * Common.Math.Clamp(separation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
// Compute normal impulse
float impulse = -ccp.EqualizedMass * C;
Vec2 P = impulse * normal;
b1._sweep.C -= invMass1 * P;
b1._sweep.A -= invI1 * Vec2.Cross(r1, P);
b1.SynchronizeTransform();
b2._sweep.C += invMass2 * P;
b2._sweep.A += invI2 * Vec2.Cross(r2, P);
b2.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.LinearSlop because we don't
// push the separation above -Settings.LinearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
/// <summary>
/// Get the world manifold.
/// </summary>
public void GetWorldManifold(out WorldManifold worldManifold)
{
worldManifold = new WorldManifold();
Body bodyA = _fixtureA.Body;
Body bodyB = _fixtureB.Body;
Shape shapeA = _fixtureA.Shape;
Shape shapeB = _fixtureB.Shape;
worldManifold.Initialize(_manifold, bodyA.GetXForm(), shapeA._radius, bodyB.GetXForm(), shapeB._radius);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body ground = this._ground1;
Body ground2 = this._ground2;
Body body = this._body1;
Body body2 = this._body2;
float num = 0f;
this._J.SetZero();
if (this._revolute1 != null)
{
this._J.Angular1 = -1f;
num += body._invI;
}
else
{
Vec2 vec = Box2DX.Common.Math.Mul(ground.GetXForm().R, this._prismatic1._localXAxis1);
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
float num2 = Vec2.Cross(a, vec);
this._J.Linear1 = -vec;
this._J.Angular1 = -num2;
num += body._invMass + body._invI * num2 * num2;
}
if (this._revolute2 != null)
{
this._J.Angular2 = -this._ratio;
num += this._ratio * this._ratio * body2._invI;
}
else
{
Vec2 vec = Box2DX.Common.Math.Mul(ground2.GetXForm().R, this._prismatic2._localXAxis1);
Vec2 a = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
float num2 = Vec2.Cross(a, vec);
this._J.Linear2 = -this._ratio * vec;
this._J.Angular2 = -this._ratio * num2;
num += this._ratio * this._ratio * (body2._invMass + body2._invI * num2 * num2);
}
Box2DXDebug.Assert(num > 0f);
this._mass = 1f / num;
if (step.WarmStarting)
{
Body expr_1FA = body;
expr_1FA._linearVelocity += body._invMass * this._impulse * this._J.Linear1;
body._angularVelocity += body._invI * this._impulse * this._J.Angular1;
Body expr_24E = body2;
expr_24E._linearVelocity += body2._invMass * this._impulse * this._J.Linear2;
body2._angularVelocity += body2._invI * this._impulse * this._J.Angular2;
}
else
{
this._impulse = 0f;
}
}
public PulleyJoint(PulleyJointDef def)
: base(def)
{
_ground = _body1.GetWorld().GetGroundBody();
_groundAnchor1 = def.GroundAnchor1 - _ground.GetXForm().Position;
_groundAnchor2 = def.GroundAnchor2 - _ground.GetXForm().Position;
_localAnchor1 = def.LocalAnchor1;
_localAnchor2 = def.LocalAnchor2;
Box2DXDebug.Assert(def.Ratio != 0.0f);
_ratio = def.Ratio;
_constant = def.Length1 + _ratio * def.Length2;
_maxLength1 = Common.Math.Min(def.MaxLength1, _constant - _ratio * PulleyJoint.MinPulleyLength);
_maxLength2 = Common.Math.Min(def.MaxLength2, (_constant - PulleyJoint.MinPulleyLength) / _ratio);
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
if (this._state == LimitState.AtUpperLimit)
{
Vec2 b = body._linearVelocity + Vec2.Cross(body._angularVelocity, a);
Vec2 b2 = body2._linearVelocity + Vec2.Cross(body2._angularVelocity, a2);
float num = -Vec2.Dot(this._u1, b) - this._ratio * Vec2.Dot(this._u2, b2);
float num2 = this._pulleyMass * -num;
float num3 = this._impulse;
this._impulse = Box2DX.Common.Math.Max(0f, this._impulse + num2);
num2 = this._impulse - num3;
Vec2 vec = -num2 * this._u1;
Vec2 vec2 = -this._ratio * num2 * this._u2;
Body expr_120 = body;
expr_120._linearVelocity += body._invMass * vec;
body._angularVelocity += body._invI * Vec2.Cross(a, vec);
Body expr_15A = body2;
expr_15A._linearVelocity += body2._invMass * vec2;
body2._angularVelocity += body2._invI * Vec2.Cross(a2, vec2);
}
if (this._limitState1 == LimitState.AtUpperLimit)
{
Vec2 b = body._linearVelocity + Vec2.Cross(body._angularVelocity, a);
float num = -Vec2.Dot(this._u1, b);
float num2 = -this._limitMass1 * num;
float num3 = this._limitImpulse1;
this._limitImpulse1 = Box2DX.Common.Math.Max(0f, this._limitImpulse1 + num2);
num2 = this._limitImpulse1 - num3;
Vec2 vec = -num2 * this._u1;
Body expr_21C = body;
expr_21C._linearVelocity += body._invMass * vec;
body._angularVelocity += body._invI * Vec2.Cross(a, vec);
}
if (this._limitState2 == LimitState.AtUpperLimit)
{
Vec2 b2 = body2._linearVelocity + Vec2.Cross(body2._angularVelocity, a2);
float num = -Vec2.Dot(this._u2, b2);
float num2 = -this._limitMass2 * num;
float num3 = this._limitImpulse2;
this._limitImpulse2 = Box2DX.Common.Math.Max(0f, this._limitImpulse2 + num2);
num2 = this._limitImpulse2 - num3;
Vec2 vec2 = -num2 * this._u2;
Body expr_2DE = body2;
expr_2DE._linearVelocity += body2._invMass * vec2;
body2._angularVelocity += body2._invI * Vec2.Cross(a2, vec2);
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
this._u = body2._sweep.C + vec2 - body._sweep.C - vec;
float num = this._u.Length();
if (num > Settings.LinearSlop)
{
this._u *= 1f / num;
}
else
{
this._u.Set(0f, 0f);
}
float num2 = Vec2.Cross(vec, this._u);
float num3 = Vec2.Cross(vec2, this._u);
float num4 = body._invMass + body._invI * num2 * num2 + body2._invMass + body2._invI * num3 * num3;
Box2DXDebug.Assert(num4 > Settings.FLT_EPSILON);
this._mass = 1f / num4;
if (this._frequencyHz > 0f)
{
float num5 = num - this._length;
float num6 = 2f * Settings.Pi * this._frequencyHz;
float num7 = 2f * this._mass * this._dampingRatio * num6;
float num8 = this._mass * num6 * num6;
this._gamma = 1f / (step.Dt * (num7 + step.Dt * num8));
this._bias = num5 * step.Dt * num8 * this._gamma;
this._mass = 1f / (num4 + this._gamma);
}
if (step.WarmStarting)
{
this._impulse *= step.DtRatio;
Vec2 vec3 = this._impulse * this._u;
Body expr_222 = body;
expr_222._linearVelocity -= body._invMass * vec3;
body._angularVelocity -= body._invI * Vec2.Cross(vec, vec3);
Body expr_25C = body2;
expr_25C._linearVelocity += body2._invMass * vec3;
body2._angularVelocity += body2._invI * Vec2.Cross(vec2, vec3);
}
else
{
this._impulse = 0f;
}
}
public float GetJointSpeed()
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v = body._sweep.C + vec;
Vec2 v2 = body2._sweep.C + vec2;
Vec2 a = v2 - v;
Vec2 worldVector = body.GetWorldVector(this._localXAxis1);
Vec2 linearVelocity = body._linearVelocity;
Vec2 linearVelocity2 = body2._linearVelocity;
float angularVelocity = body._angularVelocity;
float angularVelocity2 = body2._angularVelocity;
return(Vec2.Dot(a, Vec2.Cross(angularVelocity, worldVector)) + Vec2.Dot(worldVector, linearVelocity2 + Vec2.Cross(angularVelocity2, vec2) - linearVelocity - Vec2.Cross(angularVelocity, vec)));
}
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);
}
public bool SolvePositionConstraints(float baumgarte)
{
float num = 0f;
for (int i = 0; i < this._constraintCount; i++)
{
ContactConstraint contactConstraint = this._constraints[i];
Body body = contactConstraint.Body1;
Body body2 = contactConstraint.Body2;
float a = body._mass * body._invMass;
float num2 = body._mass * body._invI;
float a2 = body2._mass * body2._invMass;
float num3 = body2._mass * body2._invI;
Vec2 normal = contactConstraint.Normal;
for (int j = 0; j < contactConstraint.PointCount; j++)
{
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, contactConstraintPoint.LocalAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, contactConstraintPoint.LocalAnchor2 - body2.GetLocalCenter());
Vec2 v = body._sweep.C + vec;
Vec2 v2 = body2._sweep.C + vec2;
Vec2 a3 = v2 - v;
float num4 = Vec2.Dot(a3, normal) + contactConstraintPoint.Separation;
num = Box2DX.Common.Math.Min(num, num4);
float num5 = baumgarte * Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
float a4 = -contactConstraintPoint.EqualizedMass * num5;
Vec2 vec3 = a4 * normal;
Body expr_157_cp_0 = body;
expr_157_cp_0._sweep.C = expr_157_cp_0._sweep.C - a * vec3;
Body expr_176_cp_0 = body;
expr_176_cp_0._sweep.A = expr_176_cp_0._sweep.A - num2 * Vec2.Cross(vec, vec3);
body.SynchronizeTransform();
Body expr_19C_cp_0 = body2;
expr_19C_cp_0._sweep.C = expr_19C_cp_0._sweep.C + a2 * vec3;
Body expr_1BC_cp_0 = body2;
expr_1BC_cp_0._sweep.A = expr_1BC_cp_0._sweep.A + num3 * Vec2.Cross(vec2, vec3);
body2.SynchronizeTransform();
}
}
return(num >= -1.5f * Settings.LinearSlop);
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body body = this._body2;
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor - body.GetLocalCenter());
Vec2 v = body._linearVelocity + Vec2.Cross(body._angularVelocity, a);
Vec2 vec = Box2DX.Common.Math.Mul(this._mass, -(v + this._beta * this._C + this._gamma * this._impulse));
Vec2 impulse = this._impulse;
this._impulse += vec;
float num = step.Dt * this._maxForce;
if (this._impulse.LengthSquared() > num * num)
{
this._impulse *= num / this._impulse.Length();
}
vec = this._impulse - impulse;
Body expr_F5 = body;
expr_F5._linearVelocity += body._invMass * vec;
body._angularVelocity += body._invI * Vec2.Cross(a, vec);
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
public float GetJointSpeed()
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 r1 = Box2DX.Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DX.Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 d = p2 - p1;
Vec2 axis = b1.GetWorldVector(_localXAxis1);
Vec2 v1 = b1._linearVelocity;
Vec2 v2 = b2._linearVelocity;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
float speed = Vec2.Dot(d, Vec2.Cross(w1, axis)) + Vec2.Dot(axis, v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1));
return(speed);
}
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);
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v = body._linearVelocity + Vec2.Cross(body._angularVelocity, a);
Vec2 v2 = body2._linearVelocity + Vec2.Cross(body2._angularVelocity, a2);
float num = Vec2.Dot(this._u, v2 - v);
float num2 = -this._mass * (num + this._bias + this._gamma * this._impulse);
this._impulse += num2;
Vec2 vec = num2 * this._u;
Body expr_DB = body;
expr_DB._linearVelocity -= body._invMass * vec;
body._angularVelocity -= body._invI * Vec2.Cross(a, vec);
Body expr_115 = body2;
expr_115._linearVelocity += body2._invMass * vec;
body2._angularVelocity += body2._invI * Vec2.Cross(a2, vec);
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b = _body2;
Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter());
// Cdot = v + cross(w, r)
Vec2 Cdot = b._linearVelocity + Vec2.Cross(b._angularVelocity, r);
Vec2 impulse = Box2DX.Common.Math.Mul(_mass, -(Cdot + _beta * _C + _gamma * _impulse));
Vec2 oldImpulse = _impulse;
_impulse += impulse;
float maxImpulse = step.Dt * _maxForce;
if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
_impulse *= maxImpulse / _impulse.Length();
}
impulse = _impulse - oldImpulse;
b._linearVelocity += b._invMass * impulse;
b._angularVelocity += b._invI * Vec2.Cross(r, impulse);
}
public PulleyJoint(PulleyJointDef def)
: base(def)
{
_ground = _body1.GetWorld().GetGroundBody();
_groundAnchor1 = def.GroundAnchor1 - _ground.GetXForm().Position;
_groundAnchor2 = def.GroundAnchor2 - _ground.GetXForm().Position;
_localAnchor1 = def.LocalAnchor1;
_localAnchor2 = def.LocalAnchor2;
Box2DXDebug.Assert(def.Ratio != 0.0f);
_ratio = def.Ratio;
_constant = def.Length1 + _ratio * def.Length2;
_maxLength1 = Common.Math.Min(def.MaxLength1, _constant - _ratio * PulleyJoint.MinPulleyLength);
_maxLength2 = Common.Math.Min(def.MaxLength2, (_constant - PulleyJoint.MinPulleyLength) / _ratio);
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
public override void Evaluate(ContactListener listener)
{
Body body = this._shape1.GetBody();
Body body2 = this._shape2.GetBody();
Manifold manifold = this._manifold.Clone();
Collision.Collision.CollideCircles(ref this._manifold, (CircleShape)this._shape1, body.GetXForm(), (CircleShape)this._shape2, body2.GetXForm());
ContactPoint contactPoint = new ContactPoint();
contactPoint.Shape1 = this._shape1;
contactPoint.Shape2 = this._shape2;
contactPoint.Friction = Settings.MixFriction(this._shape1.Friction, this._shape2.Friction);
contactPoint.Restitution = Settings.MixRestitution(this._shape1.Restitution, this._shape2.Restitution);
if (this._manifold.PointCount > 0)
{
this._manifoldCount = 1;
ManifoldPoint manifoldPoint = this._manifold.Points[0];
if (manifold.PointCount == 0)
{
manifoldPoint.NormalImpulse = 0f;
manifoldPoint.TangentImpulse = 0f;
if (listener != null)
{
contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1);
Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1);
Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2);
contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint;
contactPoint.Normal = this._manifold.Normal;
contactPoint.Separation = manifoldPoint.Separation;
contactPoint.ID = manifoldPoint.ID;
listener.Add(contactPoint);
}
}
else
{
ManifoldPoint manifoldPoint2 = manifold.Points[0];
manifoldPoint.NormalImpulse = manifoldPoint2.NormalImpulse;
manifoldPoint.TangentImpulse = manifoldPoint2.TangentImpulse;
if (listener != null)
{
contactPoint.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1);
Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint1);
Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint.LocalPoint2);
contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint;
contactPoint.Normal = this._manifold.Normal;
contactPoint.Separation = manifoldPoint.Separation;
contactPoint.ID = manifoldPoint.ID;
listener.Persist(contactPoint);
}
}
}
else
{
this._manifoldCount = 0;
if (manifold.PointCount > 0 && listener != null)
{
ManifoldPoint manifoldPoint2 = manifold.Points[0];
contactPoint.Position = body.GetWorldPoint(manifoldPoint2.LocalPoint1);
Vec2 linearVelocityFromLocalPoint = body.GetLinearVelocityFromLocalPoint(manifoldPoint2.LocalPoint1);
Vec2 linearVelocityFromLocalPoint2 = body2.GetLinearVelocityFromLocalPoint(manifoldPoint2.LocalPoint2);
contactPoint.Velocity = linearVelocityFromLocalPoint2 - linearVelocityFromLocalPoint;
contactPoint.Normal = manifold.Normal;
contactPoint.Separation = manifoldPoint2.Separation;
contactPoint.ID = manifoldPoint2.ID;
listener.Remove(contactPoint);
}
}
}
/// <summary>
/// Compute the volume and centroid of this fixture intersected with a half plane.
/// </summary>
/// <param name="normal">Normal the surface normal.</param>
/// <param name="offset">Offset the surface offset along normal.</param>
/// <param name="c">Returns the centroid.</param>
/// <returns>The total volume less than offset along normal.</returns>
public float ComputeSubmergedArea(Vec2 normal, float offset, out Vec2 c)
{
return(_shape.ComputeSubmergedArea(normal, offset, _body.GetXForm(), out c));
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1;
Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2;
float linearError = 0.0f;
if (_state == LimitState.AtUpperLimit)
{
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;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1.SetZero();
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2.SetZero();
}
float C = _constant - length1 - _ratio * length2;
linearError = Box2DXMath.Max(linearError, -C);
C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_pulleyMass * C;
Vec2 P1 = -impulse * _u1;
Vec2 P2 = -_ratio * impulse * _u2;
b1._sweep.C += b1._invMass * P1;
b1._sweep.A += b1._invI * Vec2.Cross(r1, P1);
b2._sweep.C += b2._invMass * P2;
b2._sweep.A += b2._invI * Vec2.Cross(r2, P2);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
if (_limitState1 == LimitState.AtUpperLimit)
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
_u1 = p1 - s1;
float length1 = _u1.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1.SetZero();
}
float C = _maxLength1 - length1;
linearError = Box2DXMath.Max(linearError, -C);
C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass1 * C;
Vec2 P1 = -impulse * _u1;
b1._sweep.C += b1._invMass * P1;
b1._sweep.A += b1._invI * Vec2.Cross(r1, P1);
b1.SynchronizeTransform();
}
if (_limitState2 == LimitState.AtUpperLimit)
{
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p2 = b2._sweep.C + r2;
_u2 = p2 - s2;
float length2 = _u2.Length();
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2.SetZero();
}
float C = _maxLength2 - length2;
linearError = Box2DXMath.Max(linearError, -C);
C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass2 * C;
Vec2 P2 = -impulse * _u2;
b2._sweep.C += b2._invMass * P2;
b2._sweep.A += b2._invI * Vec2.Cross(r2, P2);
b2.SynchronizeTransform();
}
return(linearError < Settings.LinearSlop);
}
void killWheelOrthogonalVelocity(Body hull, Body wheel)
{
var p_hull = hull.GetPosition();
var p_wheel = wheel.GetPosition();
var velocity = wheel.GetLinearVelocityFromLocalPoint(Vec2.Zero);
var vec1 = wheel.GetXForm().R.Col1;
var projection = Vec2.Dot(velocity, vec1);
vec1 *= projection;
var k = vec1.Length() / 0.001f; // превышение боковой скорости 1 мм / с
if (k < 0.1f) k = 0.1f; if (k > 10f) k = 10f;
float force = -k * 3000; //warning - mn
vec1.Normalize();
hull.ApplyForce(force * vec1, p_wheel);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
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 s1 = _ground.GetXForm().Position + _groundAnchor1;
Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1.SetZero();
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2.SetZero();
}
float C = _constant - length1 - _ratio * length2;
if (C > 0.0f)
{
_state = LimitState.InactiveLimit;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpperLimit;
}
if (length1 < _maxLength1)
{
_limitState1 = LimitState.InactiveLimit;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpperLimit;
}
if (length2 < _maxLength2)
{
_limitState2 = LimitState.InactiveLimit;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpperLimit;
}
// Compute effective mass.
float cr1u1 = Vec2.Cross(r1, _u1);
float cr2u2 = Vec2.Cross(r2, _u2);
_limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
_limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
Box2DXDebug.Assert(_limitMass1 > Settings.FLT_EPSILON);
Box2DXDebug.Assert(_limitMass2 > Settings.FLT_EPSILON);
Box2DXDebug.Assert(_pulleyMass > Settings.FLT_EPSILON);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (step.WarmStarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.DtRatio;
_limitImpulse1 *= step.DtRatio;
_limitImpulse2 *= step.DtRatio;
// Warm starting.
Vec2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vec2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
float limitVelocity(Body hull, float max)
{
Vec2 velocity = hull.GetLinearVelocityFromLocalPoint(Box2DX.Common.Vec2.Zero);
var vec1 = hull.GetXForm().R.Col2;
var projection = Vec2.Dot(velocity, vec1);
vec1 *= projection;
var A = vec1.Length();
if (A > max)
{
var k = max / A;
hull.SetLinearVelocity(vec1 * k);
}
return A;
}
internal override bool SolvePositionConstraints()
{
Body b1 = _body1;
Body b2 = _body2;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
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 d = p2 - p1;
Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
// Solve linear (point-to-line) constraint.
float linearC = Vec2.Dot(ay1, d);
// Prevent overly large corrections.
linearC = Box2DXMath.Clamp(linearC, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float linearImpulse = -_linearMass * linearC;
b1._sweep.C += (invMass1 * linearImpulse) * _linearJacobian.Linear1;
b1._sweep.A += invI1 * linearImpulse * _linearJacobian.Angular1;
//b1->SynchronizeTransform(); // updated by angular constraint
b2._sweep.C += (invMass2 * linearImpulse) * _linearJacobian.Linear2;
b2._sweep.A += invI2 * linearImpulse * _linearJacobian.Angular2;
//b2->SynchronizeTransform(); // updated by angular constraint
float positionError = Box2DXMath.Abs(linearC);
// Solve angular constraint.
float angularC = b2._sweep.A - b1._sweep.A - _refAngle;
// Prevent overly large corrections.
angularC = Box2DXMath.Clamp(angularC, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
float angularImpulse = -_angularMass * angularC;
b1._sweep.A -= b1._invI * angularImpulse;
b2._sweep.A += b2._invI * angularImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
float angularError = Box2DXMath.Abs(angularC);
// Solve linear limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
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 d_ = p2_ - p1_;
Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
float translation = Vec2.Dot(ax1, d_);
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float limitC = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
limitImpulse = -_motorMass * limitC;
positionError = Box2DXMath.Max(positionError, Box2DXMath.Abs(angularC));
}
else if (_limitState == LimitState.AtLowerLimit)
{
float limitC = translation - _lowerTranslation;
positionError = Box2DXMath.Max(positionError, -limitC);
// Prevent large linear corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC + Settings.LinearSlop, -Settings.MaxLinearCorrection, 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 = translation - _upperTranslation;
positionError = Box2DXMath.Max(positionError, limitC);
// Prevent large linear corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
b1._sweep.C += (invMass1 * limitImpulse) * _motorJacobian.Linear1;
b1._sweep.A += invI1 * limitImpulse * _motorJacobian.Angular1;
b2._sweep.C += (invMass2 * limitImpulse) * _motorJacobian.Linear2;
b2._sweep.A += invI2 * limitImpulse * _motorJacobian.Angular2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// Compute the effective masses.
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Compute point to line constraint effective mass.
// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
Vec2 e = b2._sweep.C + r2 - b1._sweep.C; // e = d + r1
_linearJacobian.Set(-ay1, -Vec2.Cross(e, ay1), ay1, Vec2.Cross(r2, ay1));
_linearMass = invMass1 + invI1 * _linearJacobian.Angular1 * _linearJacobian.Angular1 +
invMass2 + invI2 * _linearJacobian.Angular2 * _linearJacobian.Angular2;
Box2DXDebug.Assert(_linearMass > Settings.FLT_EPSILON);
_linearMass = 1.0f / _linearMass;
// Compute angular constraint effective mass.
_angularMass = invI1 + invI2;
if (_angularMass > Settings.FLT_EPSILON)
{
_angularMass = 1.0f / _angularMass;
}
// Compute motor and limit terms.
if (_enableLimit || _enableMotor)
{
// The motor and limit share a Jacobian and effective mass.
Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
_motorJacobian.Set(-ax1, -Vec2.Cross(e, ax1), ax1, Vec2.Cross(r2, ax1));
_motorMass = invMass1 + invI1 * _motorJacobian.Angular1 * _motorJacobian.Angular1 +
invMass2 + invI2 * _motorJacobian.Angular2 * _motorJacobian.Angular2;
Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
if (_enableLimit)
{
Vec2 d = e - r1; // p2 - p1
float jointTranslation = Vec2.Dot(ax1, d);
if (Box2DXMath.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitForce = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitForce = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_limitForce = 0.0f;
}
}
}
if (_enableMotor == false)
{
_motorForce = 0.0f;
}
if (_enableLimit == false)
{
_limitForce = 0.0f;
}
if (step.WarmStarting)
{
Vec2 P1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear1 + (_motorForce + _limitForce) * _motorJacobian.Linear1);
Vec2 P2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear2 + (_motorForce + _limitForce) * _motorJacobian.Linear2);
float L1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular1 - _torque + (_motorForce + _limitForce) * _motorJacobian.Angular1);
float L2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular2 + _torque + (_motorForce + _limitForce) * _motorJacobian.Angular2);
b1._linearVelocity += invMass1 * P1;
b1._angularVelocity += invI1 * L1;
b2._linearVelocity += invMass2 * P2;
b2._angularVelocity += invI2 * L2;
}
else
{
_force = 0.0f;
_torque = 0.0f;
_limitForce = 0.0f;
_motorForce = 0.0f;
}
_limitPositionImpulse = 0.0f;
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
_step = step;
_constraintCount = 0;
for (int i = 0; i < contactCount; ++i)
{
Box2DXDebug.Assert(contacts[i].IsSolid());
_constraintCount += contacts[i].GetManifoldCount();
}
_constraints = new ContactConstraint[_constraintCount];
for (int i = 0; i < _constraintCount; i++)
{
_constraints[i] = new ContactConstraint();
}
int count = 0;
for (int i = 0; i < contactCount; ++i)
{
Contact contact = contacts[i];
Shape shape1 = contact._shape1;
Shape shape2 = contact._shape2;
Body b1 = shape1.GetBody();
Body b2 = shape2.GetBody();
int manifoldCount = contact.GetManifoldCount();
Manifold[] manifolds = contact.GetManifolds();
float friction = Settings.MixFriction(shape1.Friction, shape2.Friction);
float restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution);
Vec2 v1 = b1._linearVelocity;
Vec2 v2 = b2._linearVelocity;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
for (int j = 0; j < manifoldCount; ++j)
{
Manifold manifold = manifolds[j];
Box2DXDebug.Assert(manifold.PointCount > 0);
Vec2 normal = manifold.Normal;
Box2DXDebug.Assert(count < _constraintCount);
ContactConstraint cc = _constraints[count];
cc.Body1 = b1;
cc.Body2 = b2;
cc.Manifold = manifold;
cc.Normal = normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.Restitution = restitution;
for (int k = 0; k < cc.PointCount; ++k)
{
ManifoldPoint cp = manifold.Points[k];
ContactConstraintPoint ccp = cc.Points[k];
ccp.NormalImpulse = cp.NormalImpulse;
ccp.TangentImpulse = cp.TangentImpulse;
ccp.Separation = cp.Separation;
ccp.LocalAnchor1 = cp.LocalPoint1;
ccp.LocalAnchor2 = cp.LocalPoint2;
ccp.R1 = Common.Math.Mul(b1.GetXForm().R, cp.LocalPoint1 - b1.GetLocalCenter());
ccp.R2 = Common.Math.Mul(b2.GetXForm().R, cp.LocalPoint2 - b2.GetLocalCenter());
float rn1 = Vec2.Cross(ccp.R1, normal);
float rn2 = Vec2.Cross(ccp.R2, normal);
rn1 *= rn1;
rn2 *= rn2;
float kNormal = b1._invMass + b2._invMass + b1._invI * rn1 + b2._invI * rn2;
Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
ccp.NormalMass = 1.0f / kNormal;
float kEqualized = b1._mass * b1._invMass + b2._mass * b2._invMass;
kEqualized += b1._mass * b1._invI * rn1 + b2._mass * b2._invI * rn2;
Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
ccp.EqualizedMass = 1.0f / kEqualized;
Vec2 tangent = Vec2.Cross(normal, 1.0f);
float rt1 = Vec2.Cross(ccp.R1, tangent);
float rt2 = Vec2.Cross(ccp.R2, tangent);
rt1 *= rt1;
rt2 *= rt2;
float kTangent = b1._invMass + b2._invMass + b1._invI * rt1 + b2._invI * rt2;
Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
if (ccp.Separation > 0.0f)
{
ccp.VelocityBias = -step.Inv_Dt * ccp.Separation; // TODO_ERIN b2TimeStep
}
else
{
float vRel = Vec2.Dot(cc.Normal, v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1));
if (vRel < -Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.Restitution * vRel;
}
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = cc.Points[0];
ContactConstraintPoint ccp2 = cc.Points[1];
float invMass1 = b1._invMass;
float invI1 = b1._invI;
float invMass2 = b2._invMass;
float invI2 = b2._invI;
float rn11 = Vec2.Cross(ccp1.R1, normal);
float rn12 = Vec2.Cross(ccp1.R2, normal);
float rn21 = Vec2.Cross(ccp2.R1, normal);
float rn22 = Vec2.Cross(ccp2.R2, normal);
float k11 = invMass1 + invMass2 + invI1 * rn11 * rn11 + invI2 * rn12 * rn12;
float k22 = invMass1 + invMass2 + invI1 * rn21 * rn21 + invI2 * rn22 * rn22;
float k12 = invMass1 + invMass2 + invI1 * rn11 * rn21 + invI2 * rn12 * rn22;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K.Col1.Set(k11, k12);
cc.K.Col2.Set(k12, k22);
cc.NormalMass = cc.K.Invert();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
++count;
}
}
Box2DXDebug.Assert(count == _constraintCount);
}
