| public GetTransform ( ) : Box2DX.Common.Transform | ||
| return | Box2DX.Common.Transform | |
internal override void SolveVelocityConstraints(TimeStep step)
{
//B2_NOT_USED(step);
Body b1 = _body1;
Body b2 = _body2;
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// Cdot = dot(u, v + cross(w, r))
Vector2 v1 = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity);
Vector2 v2 = b2._linearVelocity + r2.CrossScalarPreMultiply(b2._angularVelocity);
float Cdot = Vector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vector2 P = impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * r1.Cross(P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * r2.Cross(P);
}
internal override void SolveVelocityConstraints(TimeStep step)
{
//B2_NOT_USED(step);
Body b1 = _bodyA;
Body b2 = _bodyB;
Vec2 r1 = Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Math.Mul(b2.GetTransform().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 void InitVelocityConstraints(TimeStep step)
{
Body b = _body2;
float mass = b.GetMass();
// Frequency
float omega = 2.0f * Settings.Pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * mass * _dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Box2DXDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON);
_gamma = 1.0f / (step.Dt * (d + step.Dt * k));
_beta = step.Dt * k * _gamma;
// Compute the effective mass matrix.
Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter());
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
float invMass = b._invMass;
float invI = b._invI;
Mat22 K1 = new Mat22();
K1.Col1.X = invMass; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass;
Mat22 K2 = new Mat22();
K2.Col1.X = invI * r.Y * r.Y; K2.Col2.X = -invI * r.X * r.Y;
K2.Col1.Y = -invI * r.X * r.Y; K2.Col2.Y = invI * r.X * r.X;
Mat22 K = K1 + K2;
K.Col1.X += _gamma;
K.Col2.Y += _gamma;
_mass = K.GetInverse();
_C = b._sweep.C + r - _target;
// Cheat with some damping
b._angularVelocity *= 0.98f;
// Warm starting.
_impulse *= step.DtRatio;
b._linearVelocity += invMass * _impulse;
b._angularVelocity += invI * r.Cross(_impulse);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body 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
{
Vector2 ug = g1.GetTransform().TransformDirection(_prismatic1._localXAxis1);
Vector2 r = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
float crug = r.Cross(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
{
Vector2 ug = g2.GetTransform().TransformDirection(_prismatic2._localXAxis1);
Vector2 r = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
float crug = r.Cross(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.
b1._linearVelocity += b1._invMass * _impulse * _J.Linear1;
b1._angularVelocity += b1._invI * _impulse * _J.Angular1;
b2._linearVelocity += b2._invMass * _impulse * _J.Linear2;
b2._angularVelocity += b2._invI * _impulse * _J.Angular2;
}
else
{
_impulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().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.GetTransform(), _fixtureB.Shape, bodyB.GetTransform());
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body g1 = _ground1;
Body g2 = _ground2;
Body b1 = _bodyA;
Body b2 = _bodyB;
float K = 0.0f;
_J.SetZero();
if (_revolute1 != null)
{
_J.Angular1 = -1.0f;
K += b1._invI;
}
else
{
Vec2 ug = Math.Mul(g1.GetTransform().R, _prismatic1._localXAxis1);
Vec2 r = Math.Mul(b1.GetTransform().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 = Math.Mul(g2.GetTransform().R, _prismatic2._localXAxis1);
Vec2 r = Math.Mul(b2.GetTransform().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.
_mass = K > 0.0f ? 1.0f / K : 0.0f;
if (step.WarmStarting)
{
// Warm starting.
b1._linearVelocity += b1._invMass * _impulse * _J.Linear1;
b1._angularVelocity += b1._invI * _impulse * _J.Angular1;
b2._linearVelocity += b2._invMass * _impulse * _J.Linear2;
b2._angularVelocity += b2._invI * _impulse * _J.Angular2;
}
else
{
_impulse = 0.0f;
}
}
public override void Evaluate()
{
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
Collision.Collision.CollideCircles(out Manifold,
(CircleShape)_fixtureA.GetShape(), bodyA.GetTransform(),
(CircleShape)_fixtureB.GetShape(), bodyB.GetTransform());
}
/// <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.GetTransform(), shapeA._radius, bodyB.GetTransform(), shapeB._radius);
}
public PulleyJoint(PulleyJointDef def)
: base(def)
{
_ground = _body1.GetWorld().GetGroundBody();
_groundAnchor1 = def.GroundAnchor1 - _ground.GetTransform().position;
_groundAnchor2 = def.GroundAnchor2 - _ground.GetTransform().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;
}
private void DrawJoint(Joint joint)
{
Body b1 = joint.GetBody1();
Body b2 = joint.GetBody2();
Transform xf1 = b1.GetTransform();
Transform xf2 = b2.GetTransform();
Vec2 x1 = xf1.Position;
Vec2 x2 = xf2.Position;
Vec2 p1 = joint.Anchor1;
Vec2 p2 = joint.Anchor2;
Color color = new Color(0.5f, 0.8f, 0.8f);
switch (joint.GetType())
{
case JointType.DistanceJoint:
_debugDraw.DrawSegment(p1, p2, color);
break;
case JointType.PulleyJoint:
{
PulleyJoint pulley = (PulleyJoint)joint;
Vec2 s1 = pulley.GroundAnchor1;
Vec2 s2 = pulley.GroundAnchor2;
_debugDraw.DrawSegment(s1, p1, color);
_debugDraw.DrawSegment(s2, p2, color);
_debugDraw.DrawSegment(s1, s2, color);
}
break;
case JointType.MouseJoint:
// don't draw this
break;
default:
_debugDraw.DrawSegment(x1, p1, color);
_debugDraw.DrawSegment(p1, p2, color);
_debugDraw.DrawSegment(x2, p2, color);
break;
}
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
public float GetJointSpeed()
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.C + r1;
Vector2 p2 = b2._sweep.C + r2;
Vector2 d = p2 - p1;
Vector2 axis = b1.GetWorldVector(_localXAxis1);
Vector2 v1 = b1._linearVelocity;
Vector2 v2 = b2._linearVelocity;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
float speed = Vector2.Dot(d, axis.CrossScalarPreMultiply(w1)) + Vector2.Dot(axis, v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1));
return(speed);
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
public float GetJointSpeed()
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vec2 r1 = Box2DX.Common.Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DX.Common.Math.Mul(b2.GetTransform().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 bool SolvePositionConstraints(float baumgarte)
{
if (_frequencyHz > 0.0f)
{
//There is no possition correction for soft distace constraint.
return(true);
}
Body b1 = _body1;
Body b2 = _body2;
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
float length = d.magnitude;
d.Normalize();
float C = length - _length;
C = Mathf.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float impulse = -_mass * C;
_u = d;
Vector2 P = impulse * _u;
b1._sweep.C -= b1._invMass * P;
b1._sweep.A -= b1._invI * r1.Cross(P);
b2._sweep.C += b2._invMass * P;
b2._sweep.A += b2._invI * r2.Cross(P);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(System.Math.Abs(C) < Settings.LinearSlop);
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b = _bodyB;
Vec2 r = Common.Math.Mul(b.GetTransform().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);
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b = _body2;
Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter());
// Cdot = v + cross(w, r)
Vector2 Cdot = b._linearVelocity + r.CrossScalarPreMultiply(b._angularVelocity);
Vector2 impulse = _mass.Multiply(-(Cdot + _beta * _C + _gamma * _impulse));
Vector2 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 * r.Cross(impulse);
}
/// <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(Vector2 normal, float offset, out Vector2 c)
{
return(_shape.ComputeSubmergedArea(normal, offset, _body.GetTransform(), out c));
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
// Compute the effective mass matrix.
Vec2 r1 = Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Math.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
_u = b2._sweep.C + r2 - b1._sweep.C - r1;
// Handle singularity.
float length = _u.Length();
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u.Set(0.0f, 0.0f);
}
float cr1u = Vec2.Cross(r1, _u);
float cr2u = Vec2.Cross(r2, _u);
float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (_frequencyHz > 0.0f)
{
float C = length - _length;
// 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 = step.Dt * (d + step.Dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.Dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (step.WarmStarting)
{
//Scale the inpulse to support a variable timestep.
_impulse *= step.DtRatio;
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);
}
else
{
_impulse = 0.0f;
}
}
public void DrawDebugData()
{
if (_debugDraw == null)
{
return;
}
DebugDraw.DrawFlags flags = _debugDraw.Flags;
if ((flags & DebugDraw.DrawFlags.Shape) != 0)
{
for (Body b = _bodyList; b != null; b = b.GetNext())
{
Transform xf = b.GetTransform();
for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
{
if (b.IsStatic())
{
DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f));
}
else if (b.IsSleeping())
{
DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f));
}
else
{
DrawShape(f, xf, new Color(0.9f, 0.9f, 0.9f));
}
}
}
}
if ((flags & DebugDraw.DrawFlags.Joint) != 0)
{
for (Joint j = _jointList; j != null; j = j.GetNext())
{
if (j.GetType() != JointType.MouseJoint)
{
DrawJoint(j);
}
}
}
if ((flags & DebugDraw.DrawFlags.Pair) != 0)
{
// TODO_ERIN
}
if ((flags & DebugDraw.DrawFlags.Aabb) != 0)
{
Color color = new Color(0.9f, 0.3f, 0.9f);
BroadPhase bp = _contactManager._broadPhase;
for (Body b = _bodyList; b != null; b = b.GetNext())
{
for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
{
AABB aabb = bp.GetFatAABB(f.ProxyId);
Vec2[] vs = new Vec2[4];
vs[0].Set(aabb.LowerBound.X, aabb.LowerBound.Y);
vs[1].Set(aabb.UpperBound.X, aabb.LowerBound.Y);
vs[2].Set(aabb.UpperBound.X, aabb.UpperBound.Y);
vs[3].Set(aabb.LowerBound.X, aabb.UpperBound.Y);
_debugDraw.DrawPolygon(vs, 4, color);
}
}
}
if ((flags & DebugDraw.DrawFlags.CenterOfMass) != 0)
{
for (Body b = _bodyList; b != null; b = b.GetNext())
{
Transform xf = b.GetTransform();
xf.Position = b.GetWorldCenter();
_debugDraw.DrawXForm(xf);
}
}
}
public PulleyJoint(PulleyJointDef def)
: base(def)
{
_ground = _body1.GetWorld().GetGroundBody();
_groundAnchor1 = def.GroundAnchor1 - _ground.GetTransform().position;
_groundAnchor2 = def.GroundAnchor2 - _ground.GetTransform().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;
}
/// Test a point for containment in this fixture. This only works for convex shapes.
/// @param xf the shape world transform.
/// @param p a point in world coordinates.
public bool TestPoint(Vec2 p)
{
return(Shape.TestPoint(Body.GetTransform(), p));
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// Compute the effective mass matrix.
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
_u = b2._sweep.C + r2 - b1._sweep.C - r1;
// Handle singularity.
float length = _u.magnitude;
if (length > Settings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.zero;
}
float cr1u = r1.Cross(_u);
float cr2u = r2.Cross(_u);
float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
Box2DXDebug.Assert(invMass > Settings.FLT_EPSILON);
_mass = 1.0f / invMass;
if (_frequencyHz > 0.0f)
{
float C = length - _length;
// 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 = 1.0f / (step.Dt * (d + step.Dt * k));
_bias = C * step.Dt * k * _gamma;
_mass = 1.0f / (invMass + _gamma);
}
if (step.WarmStarting)
{
//Scale the inpulse to support a variable timestep.
_impulse *= step.DtRatio;
Vector2 P = _impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * r1.Cross(P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * r2.Cross(P);
}
else
{
_impulse = 0.0f;
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
Transform xf1 = b1.GetTransform();
Transform xf2 = b2.GetTransform();
// Compute the effective masses.
Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1);
Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2);
Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
_invMass1 = b1._invMass;
_invI1 = b1._invI;
_invMass2 = b2._invMass;
_invI2 = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = xf1.TransformDirection(_localXAxis1);
_a1 = (d + r1).Cross(_axis);
_a2 = r2.Cross(_axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic constraint.
{
_perp = xf1.TransformDirection(_localYAxis1);
_s1 = (d + r1).Cross(_perp);
_s2 = r2.Cross(_perp);
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector2(k11, k12);
_K.Col2 = new Vector2(k12, k22);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitState = LimitState.AtLowerLimit;
_impulse.Y = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitState = LimitState.AtUpperLimit;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.WarmStarting)
{
// Account for variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;
b1._linearVelocity -= _invMass1 * P;
b1._angularVelocity -= _invI1 * L1;
b2._linearVelocity += _invMass2 * P;
b2._angularVelocity += _invI2 * L2;
}
else
{
_impulse = Vector2.Zero;
_motorImpulse = 0.0f;
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
// You cannot create a prismatic joint between bodies that
// both have fixed rotation.
Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
Transform xf1 = b1.GetTransform();
Transform xf2 = b2.GetTransform();
// Compute the effective masses.
Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2);
Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
_invMass1 = b1._invMass;
_invI1 = b1._invI;
_invMass2 = b2._invMass;
_invI2 = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = Math.Mul(xf1.R, _localXAxis1);
_a1 = Vec2.Cross(d + r1, _axis);
_a2 = Vec2.Cross(r2, _axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
if (_motorMass > Settings.FLT_EPSILON)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = Math.Mul(xf1.R, _localYAxis1);
_s1 = Vec2.Cross(d + r1, _perp);
_s2 = Vec2.Cross(r2, _perp);
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1.Set(k11, k12, k13);
_K.Col2.Set(k12, k22, k23);
_K.Col3.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vec2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitState = LimitState.AtLowerLimit;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitState = LimitState.AtUpperLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.WarmStarting)
{
// Account for variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;
b1._linearVelocity -= _invMass1 * P;
b1._angularVelocity -= _invI1 * L1;
b2._linearVelocity += _invMass2 * P;
b2._angularVelocity += _invI2 * L2;
}
else
{
_impulse.SetZero();
_motorImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vector2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
//Solve motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorTorque;
_motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
//Solve limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot1 = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.EqualLimits)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLowerLimit)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpperLimit)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
Vector2 P = impulse.ToVector2();
v1 -= m1 * P;
w1 -= i1 * (r1.Cross(P) + impulse.Z);
v2 += m2 * P;
w2 += i2 * (r2.Cross(P) + impulse.Z);
}
else
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
w1 -= i1 * r1.Cross(impulse);
v2 += m2 * impulse;
w2 += i2 * r2.Cross(impulse);
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 s1 = _groundAnchor1;
Vec2 s2 = _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;
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
}
// Compute the effective mass matrix.
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
_mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
_mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
_mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
_mass.Col1.Y = _mass.Col2.X;
_mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
_mass.Col3.Y = r1.X * i1 + r2.Y * i2;
_mass.Col1.Z = _mass.Col3.X;
_mass.Col2.Z = _mass.Col3.Y;
_mass.Col3.Z = i1 + i2;
_motorMass = 1.0f / (i1 + i2);
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLowerLimit)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpperLimit)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (step.WarmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vector2 P = _impulse.ToVector2();
b1._linearVelocity -= m1 * P;
b1._angularVelocity -= i1 * (r1.Cross(P) + _motorImpulse + _impulse.Z);
b2._linearVelocity += m2 * P;
b2._angularVelocity += i2 * (r2.Cross(P) + _motorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vec2 s1 = _groundAnchor1;
Vec2 s2 = _groundAnchor2;
float linearError = 0.0f;
if (_state == LimitState.AtUpperLimit)
{
Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().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.GetTransform().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.GetTransform().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);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit.
Body b1 = _body1;
Body b2 = _body2;
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float C = Box2DX.Common.Math.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Box2DXMath.Abs(C);
}
else if (_limitState == LimitState.AtLowerLimit)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = Box2DX.Common.Math.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = Box2DX.Common.Math.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1._sweep.A -= b1._invI * limitImpulse;
b2._sweep.A += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 C = b2._sweep.C + r2 - b1._sweep.C - r1;
positionError = C.Length;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Handle large detachment.
float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.LengthSquared > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C; u.Normalize();
float k = invMass1 + invMass2;
Box2DXDebug.Assert(k > Settings.FLT_EPSILON);
float m = 1.0f / k;
Vector2 impulse = m * (-C);
float k_beta = 0.5f;
b1._sweep.C -= k_beta * invMass1 * impulse;
b2._sweep.C += k_beta * invMass2 * impulse;
C = b2._sweep.C + r2 - b1._sweep.C - r1;
}
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
Vector2 impulse_ = K.Solve(-C);
b1._sweep.C -= b1._invMass * impulse_;
b1._sweep.A -= b1._invI * r1.Cross(impulse_);
b2._sweep.C += b2._invMass * impulse_;
b2._sweep.A += b2._invI * r2.Cross(impulse_);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
{
Step = step;
ConstraintCount = contactCount;
Constraints = new ContactConstraint[ConstraintCount];
for (int i = 0; i < ConstraintCount; ++i)
{
Contact contact = contacts[i];
Fixture fixtureA = contact.GetFixtureA();
Fixture fixtureB = contact.GetFixtureB();
Shape shapeA = fixtureA.GetShape();
Shape shapeB = fixtureB.GetShape();
float radiusA = shapeA._radius;
float radiusB = shapeB._radius;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
Manifold manifold = contact.GetManifold();
float friction = Settings.MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
float restitution = Settings.MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());
Vec2 vA = bodyA._linearVelocity;
Vec2 vB = bodyB._linearVelocity;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
Box2DXDebug.Assert(manifold.PointCount > 0);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, bodyA.GetTransform(), radiusA, bodyB.GetTransform(), radiusB);
ContactConstraint cc = new ContactConstraint();
Constraints[i] = cc;
cc.BodyA = bodyA;
cc.BodyB = bodyB;
cc.Manifold = manifold;
cc.Normal = worldManifold.Normal;
cc.PointCount = manifold.PointCount;
cc.Friction = friction;
cc.Restitution = restitution;
cc.LocalPlaneNormal = manifold.LocalPlaneNormal;
cc.LocalPoint = manifold.LocalPoint;
cc.Radius = radiusA + radiusB;
cc.Type = manifold.Type;
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint ccp = cc.Points[j];
ccp.NormalImpulse = cp.NormalImpulse;
ccp.TangentImpulse = cp.TangentImpulse;
ccp.LocalPoint = cp.LocalPoint;
ccp.RA = worldManifold.Points[j] - bodyA._sweep.C;
ccp.RB = worldManifold.Points[j] - bodyB._sweep.C;
float rnA = Vec2.Cross(ccp.RA, cc.Normal);
float rnB = Vec2.Cross(ccp.RB, cc.Normal);
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
Box2DXDebug.Assert(kNormal > Settings.FLT_EPSILON);
ccp.NormalMass = 1.0f / kNormal;
float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;
Box2DXDebug.Assert(kEqualized > Settings.FLT_EPSILON);
ccp.EqualizedMass = 1.0f / kEqualized;
Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f);
float rtA = Vec2.Cross(ccp.RA, tangent);
float rtB = Vec2.Cross(ccp.RB, tangent);
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
Box2DXDebug.Assert(kTangent > Settings.FLT_EPSILON);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA));
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 invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
float rn1A = Vec2.Cross(ccp1.RA, cc.Normal);
float rn1B = Vec2.Cross(ccp1.RB, cc.Normal);
float rn2A = Vec2.Cross(ccp2.RA, cc.Normal);
float rn2B = Vec2.Cross(ccp2.RB, cc.Normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// 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;
}
}
}
}