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 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); }