/// <summary>
/// Solves the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void SolveVelocityConstraints(TimeStep step)
{
//B2_NOT_USED(step);
Body b1 = Body1;
Body b2 = Body2;
Vec2 r1 = Math.Mul(b1.GetXForm().R, LocalAnchor1 - b1.GetLocalCenter());
Vec2 r2 = 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);
}
public Vec2 GetSearchDirection()
{
switch (Count)
{
case 1:
return(-Vertices[0].W);
case 2:
{
Vec2 e12 = Vertices[1].W - Vertices[0].W;
float sgn = Vec2.Cross(e12, -Vertices[0].W);
if (sgn > 0.0f)
{
// Origin is left of e12.
return(Vec2.Cross(1.0f, e12));
}
else
{
// Origin is right of e12.
return(Vec2.Cross(e12, 1.0f));
}
}
default:
Box2DXDebug.Assert(false);
return(Vec2.Zero);
}
}
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);
}
/// <summary>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void InitVelocityConstraints(TimeStep step)
{
Body body2 = Body2;
float body2Mass = body2.GetMass();
// Frequency
float omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
float coefficient = 2.0f * body2Mass * DampingRatio * omega;
// Spring stiffness
float stiffness = body2Mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Box2DxDebug.Assert(coefficient + step.Dt * stiffness > Settings.FltEpsilon);
Gamma = 1.0f / (step.Dt * (coefficient + step.Dt * stiffness));
Beta = step.Dt * stiffness * Gamma;
// Compute the effective mass matrix.
Vec2 effectiveMass = Math.Mul(body2.GetXForm().R, LocalAnchor - body2.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 = body2.InvMass;
float invI = body2.InvI;
Mat22 k1 = new Mat22
{
Col1 = new Vec2(invMass, 0.0f),
Col2 = new Vec2(0.0f, invMass)
};
Mat22 k2 = new Mat22
{
Col1 = new Vec2(invI * effectiveMass.Y * effectiveMass.Y, -invI * effectiveMass.X * effectiveMass.Y),
Col2 = new Vec2(-invI * effectiveMass.X * effectiveMass.Y, invI * effectiveMass.X * effectiveMass.X)
};
Mat22 k = k1 + k2;
k.Col1.X += Gamma;
k.Col2.Y += Gamma;
Mass = k.GetInverse();
C = body2.Sweep.C + effectiveMass - Target;
// Cheat with some damping
body2.AngularVelocity *= 0.98f;
// Warm starting.
Impulse *= step.DtRatio;
body2.LinearVelocity += invMass * Impulse;
body2.AngularVelocity += invI * Vec2.Cross(effectiveMass, Impulse);
}
public void GetSearchDirection(Vec2 result)
{
switch (Count)
{
case 1:
result.Set(m_v1.W).NegateLocal();
return;
case 2:
e12.Set(m_v2.W).SubLocal(m_v1.W);
// use out for a temp variable real quick
result.Set(m_v1.W).NegateLocal();
float sgn = Vec2.Cross(e12, result);
if (sgn > 0f)
{
// Origin is left of e12.
Vec2.CrossToOutUnsafe(1f, e12, result);
return;
}
else
{
// Origin is right of e12.
Vec2.CrossToOutUnsafe(e12, 1f, result);
return;
}
default:
Debug.Assert(false);
result.SetZero();
return;
}
}
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;
}
}
public override float GetReactionTorque(float inv_dt)
{
Vec2 a = Box2DX.Common.Math.Mul(this._body2.GetXForm().R, this._localAnchor2 - this._body2.GetLocalCenter());
Vec2 b = this._impulse * this._J.Linear2;
float num = this._impulse * this._J.Angular2 - Vec2.Cross(a, b);
return(inv_dt * num);
}
public void ApplyImpulse(Vec2 impulse, Vec2 contactVector)
{
// velocity += im * impulse;
// angularVelocity += iI * Cross( contactVector, impulse );
velocity.Addsi(impulse, invMass);
angularVelocity += invInertia * Vec2.Cross(contactVector, impulse);
}
/// <summary>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void InitVelocityConstraints(TimeStep step)
{
Body g1 = Ground1;
Body g2 = Ground2;
Body b1 = Body1;
Body b2 = Body2;
float k = 0.0f;
jacobian.SetZero();
if (Revolute1 != null)
{
jacobian.Angular1 = -1.0f;
k += b1.InvI;
}
else
{
Vec2 ug = Math.Mul(g1.GetXForm().R, Prismatic1.LocalXAxis1);
Vec2 r = Math.Mul(b1.GetXForm().R, LocalAnchor1 - b1.GetLocalCenter());
float crug = Vec2.Cross(r, ug);
jacobian.Linear1 = -ug;
jacobian.Angular1 = -crug;
k += b1.InvMass + b1.InvI * crug * crug;
}
if (Revolute2 != null)
{
jacobian.Angular2 = -Ratio;
k += Ratio * Ratio * b2.InvI;
}
else
{
Vec2 ug = Math.Mul(g2.GetXForm().R, Prismatic2.LocalXAxis1);
Vec2 r = Math.Mul(b2.GetXForm().R, LocalAnchor2 - b2.GetLocalCenter());
float crug = Vec2.Cross(r, ug);
jacobian.Linear2 = -Ratio * ug;
jacobian.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 * jacobian.Linear1;
b1.AngularVelocity += b1.InvI * Impulse * jacobian.Angular1;
b2.LinearVelocity += b2.InvMass * Impulse * jacobian.Linear2;
b2.AngularVelocity += b2.InvI * Impulse * jacobian.Angular2;
}
else
{
Impulse = 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.
Box2DNetDebug.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.MathB2.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);
}
/// <summary>
/// Gets the reaction torque using the specified inv dt
/// </summary>
/// <param name="invDt">The inv dt</param>
/// <returns>The float</returns>
public override float GetReactionTorque(float invDt)
{
// TODO_ERIN not tested
Vec2 r = Math.Mul(Body2.GetXForm().R, LocalAnchor2 - Body2.GetLocalCenter());
Vec2 p = Impulse * jacobian.Linear2;
float l = Impulse * jacobian.Angular2 - Vec2.Cross(r, p);
return(invDt * l);
}
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);
}
}
/// <summary>
/// Apply an impulse at a point. This immediately modifies the velocity.
/// It also modifies the angular velocity if the point of application
/// is not at the center of mass. This wakes up the body.
/// </summary>
/// <param name="impulse">The world impulse vector, usually in N-seconds or kg-m/s.</param>
/// <param name="point">The world position of the point of application.</param>
public void ApplyImpulse(Vec2 impulse)
{
if (IsSleeping())
{
WakeUp();
}
_linearVelocity += _invMass * impulse;
_angularVelocity += _invI * Vec2.Cross(this.GetWorldCenter() - _sweep.C, impulse);
}
/// <summary>
/// Apply a force at a world point. If the force is not
/// applied at the center of mass, it will generate a torque and
/// affect the angular velocity. This wakes up the body.
/// </summary>
/// <param name="force">The world force vector, usually in Newtons (N).</param>
public void ApplyForce(Vec2 force)
{
if (IsSleeping())
{
WakeUp();
}
_force += force;
_torque += Vec2.Cross(this.GetWorldCenter() - _sweep.C, force);
}
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 void ApplyImpulse(Vec2 impulse, Vec2 point)
{
if (this.IsSleeping())
{
this.WakeUp();
}
this._linearVelocity += this._invMass * impulse;
this._angularVelocity += this._invI * Vec2.Cross(point - this._sweep.C, impulse);
}
public void ApplyForce(Vec2 force, Vec2 point)
{
if (this.IsSleeping())
{
this.WakeUp();
}
this._force += force;
this._torque += Vec2.Cross(point - this._sweep.C, force);
}
/// <summary>
/// Solves the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
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 = Box2DXMath.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 = Box2DXMath.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 = Box2DXMath.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);
}
}
/// <summary>
/// Apply an impulse at a point. This immediately modifies the velocity.
/// It also modifies the angular velocity if the point of application
/// is not at the center of mass. This wakes up the body.
/// </summary>
/// <param name="impulse">The world impulse vector, usually in N-seconds or kg-m/s.</param>
/// <param name="point">The world position of the point of application.</param>
public void ApplyImpulse(Vec2 impulse, Vec2 point)
{
if (IsSleeping())
{
WakeUp();
}
_linearVelocity += _invMass * impulse;
_angularVelocity += _invI * Vec2.Cross(point - _sweep.C, impulse);
}
public override float GetReactionTorque(float inv_dt)
{
// TODO_ERIN not tested
Vec2 r = Common.Math.Mul(_body2.GetXForm().R, _localAnchor2 - _body2.GetLocalCenter());
Vec2 P = _impulse * _J.Linear2;
float L = _impulse * _J.Angular2 - Vec2.Cross(r, P);
return(inv_dt * L);
}
/// <summary>
/// Apply a force at a world point. If the force is not
/// applied at the center of mass, it will generate a torque and
/// affect the angular velocity. This wakes up the body.
/// </summary>
/// <param name="force">The world force vector, usually in Newtons (N).</param>
/// <param name="point">The world position of the point of application.</param>
public void ApplyForce(Vec2 force, Vec2 point)
{
if (IsSleeping())
{
WakeUp();
}
_force += force;
_torque += Vec2.Cross(point - _sweep.C, force);
}
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 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);
}
public void SetAsEdge(Vec2 v1, Vec2 v2)
{
_vertexCount = 2;
_vertices[0] = v1;
_vertices[1] = v2;
_centroid = 0.5f * (v1 + v2);
_normals[0] = Vec2.Cross(v2 - v1, 1.0f);
_normals[0].Normalize();
_normals[1] = -_normals[0];
}
public override void Draw(DebugDraw debugDraw)
{
float r = 1000;
Vec2 p1 = Offset * Normal + Vec2.Cross(Normal, r);
Vec2 p2 = Offset * Normal - Vec2.Cross(Normal, r);
Color color = new Color(0, 0, 0.8f);
debugDraw.DrawSegment(p1, p2, color);
}
/// <summary>
/// Sets the as edge using the specified v 1
/// </summary>
/// <param name="v1">The </param>
/// <param name="v2">The </param>
public void SetAsEdge(Vec2 v1, Vec2 v2)
{
VertexCount = 2;
Vertices[0] = v1;
Vertices[1] = v2;
Centroid = 0.5f * (v1 + v2);
Normals[0] = Vec2.Cross(v2 - v1, 1.0f);
Normals[0].Normalize();
Normals[1] = -Normals[0];
}
// Sequential solver.
public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < ConstraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = bodyA._mass * bodyA._invMass;
float invIA = bodyA._mass * bodyA._invI;
float invMassB = bodyB._mass * bodyB._invMass;
float invIB = bodyB._mass * bodyB._invI;
PositionSolverManifold psm = new PositionSolverManifold();
psm.Initialize(c);
Vec2 normal = psm.Normal;
// Solve normal constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
Vec2 point = psm.Points[j];
float separation = psm.Separations[j];
Vec2 rA = point - bodyA._sweep.C;
Vec2 rB = point - bodyB._sweep.C;
// Track max constraint error.
minSeparation = Common.Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = Common.Math.Clamp(baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);
// Compute normal impulse
float impulse = -ccp.EqualizedMass * C;
Vec2 P = impulse * normal;
bodyA._sweep.C -= invMassA * P;
bodyA._sweep.A -= invIA * Vec2.Cross(rA, P);
bodyA.SynchronizeTransform();
bodyB._sweep.C += invMassB * P;
bodyB._sweep.A += invIB * Vec2.Cross(rB, P);
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
/// Set the mass properties to override the mass properties of the fixtures.
/// Note that this changes the center of mass position. You can make the body
/// static by using zero mass.
/// Note that creating or destroying fixtures can also alter the mass.
/// @warning The supplied rotational inertia is assumed to be relative to the center of mass.
/// @param massData the mass properties.
// TODO ERIN adjust linear velocity and torque to account for movement of center.
public void SetMassData(MassData massData)
{
Box2DXDebug.Assert(_world.IsLocked() == false);
if (_world.IsLocked() == true)
{
return;
}
_invMass = 0.0f;
_I = 0.0f;
_invI = 0.0f;
_mass = massData.Mass;
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
}
if (massData.I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
{
_I = massData.I - _mass * Vec2.Dot(massData.Center, massData.Center);
_invI = 1.0f / _I;
}
// Move center of mass.
Vec2 oldCenter = _sweep.C;
_sweep.LocalCenter = massData.Center;
_sweep.C0 = _sweep.C = Math.Mul(_xf, _sweep.LocalCenter);
// Update center of mass velocity.
_linearVelocity += Vec2.Cross(_angularVelocity, _sweep.C - oldCenter);
BodyType oldType = _type;
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
}
else
{
_type = BodyType.Dynamic;
}
// If the body type changed, we need to flag contacts for filtering.
if (oldType != _type)
{
for (ContactEdge ce = _contactList; ce != null; ce = ce.Next)
{
ce.Contact.FlagForFiltering();
}
}
}
/// <summary>
/// Copy vertices. This assumes the vertices define a convex polygon.
/// It is assumed that the exterior is the the right of each edge.
/// </summary>
public void Set(Vec2[] vertices, int count)
{
Box2DNetDebug.Assert(3 <= count && count <= Settings.MaxPolygonVertices);
_vertexCount = count;
int i;
// Copy vertices.
for (i = 0; i < _vertexCount; ++i)
{
_vertices[i] = vertices[i];
}
// Compute normals. Ensure the edges have non-zero length.
for (i = 0; i < _vertexCount; ++i)
{
int i1 = i;
int i2 = i + 1 < count ? i + 1 : 0;
Vec2 edge = _vertices[i2] - _vertices[i1];
Box2DNetDebug.Assert(edge.LengthSquared() > Settings.FLT_EPSILON_SQUARED);
_normals[i] = Vec2.Cross(edge, 1.0f);
_normals[i].Normalize();
}
#if DEBUG
// Ensure the polygon is convex and the interior
// is to the left of each edge.
for (i = 0; i < _vertexCount; ++i)
{
int i1 = i;
int i2 = i + 1 < count ? i + 1 : 0;
Vec2 edge = _vertices[i2] - _vertices[i1];
for (int j = 0; j < _vertexCount; ++j)
{
// Don't check vertices on the current edge.
if (j == i1 || j == i2)
{
continue;
}
Vec2 r = _vertices[j] - _vertices[i1];
// Your polygon is non-convex (it has an indentation) or
// has colinear edges.
float s = Vec2.Cross(edge, r);
Box2DNetDebug.Assert(s > 0.0f);
}
}
#endif
// Compute the polygon centroid.
_centroid = ComputeCentroid(_vertices, _vertexCount);
}
/// <summary>
/// Describes whether this instance solve position constraints
/// </summary>
/// <param name="baumgarte">The baumgarte</param>
/// <returns>The bool</returns>
public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < ConstraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float invMassA = bodyA.Mass * bodyA.InvMass;
float invIa = bodyA.Mass * bodyA.InvI;
float invMassB = bodyB.Mass * bodyB.InvMass;
float invIb = bodyB.Mass * bodyB.InvI;
SPositionSolverManifold.Initialize(c);
Vec2 normal = SPositionSolverManifold.Normal;
// Solver normal constraints
for (int j = 0; j < c.PointCount; ++j)
{
Vec2 point = SPositionSolverManifold.Points[j];
float separation = SPositionSolverManifold.Separations[j];
Vec2 rA = point - bodyA.Sweep.C;
Vec2 rB = point - bodyB.Sweep.C;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float clamp = baumgarte * Math.Clamp(separation + Settings.LinearSlop,
-Settings.MaxLinearCorrection,
0.0f);
// Compute normal impulse
float impulse = -c.Points[j].EqualizedMass * clamp;
Vec2 p = impulse * normal;
bodyA.Sweep.C -= invMassA * p;
bodyA.Sweep.A -= invIa * Vec2.Cross(rA, p);
bodyA.SynchronizeTransform();
bodyB.Sweep.C += invMassB * p;
bodyB.Sweep.A += invIb * Vec2.Cross(rB, p);
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.LinearSlop because we don't
// push the separation above -Settings.LinearSlop.
return(minSeparation >= -1.5f * Settings.LinearSlop);
}
/**
* Triangulates a polygon using simple ear-clipping algorithm. Returns
* size of Triangle array unless the polygon can't be triangulated.
* This should only happen if the polygon self-intersects,
* though it will not _always_ return null for a bad polygon - it is the
* caller's responsibility to check for self-intersection, and if it
* doesn't, it should at least check that the return value is non-null
* before using. You're warned!
*
* Triangles may be degenerate, especially if you have identical points
* in the input to the algorithm. Check this before you use them.
*
* This is totally unoptimized, so for large polygons it should not be part
* of the simulation loop.
*
* Returns:
* -1 if algorithm fails (self-intersection most likely)
* 0 if there are not enough vertices to triangulate anything.
* Number of triangles if triangulation was successful.
*
* results will be filled with results - ear clipping always creates vNum - 2
* or fewer (due to pinch point polygon snipping), so allocate an array of
* this size.
*/
public static int TriangulatePolygon(List<float> xv, List<float> yv, List<Triangle> results)
{
if (xv.Count < 3)
return 0;
//Recurse and split on pinch points
Polygon pA = new Polygon(), pB = new Polygon();
Polygon pin = new Polygon(xv, yv);
if (ResolvePinchPoint(pin, ref pA, ref pB))
{
List<Triangle> mergeA = new List<Triangle>();
List<Triangle> mergeB = new List<Triangle>();
int nA = TriangulatePolygon(pA.x, pA.y, mergeA);
int nB = TriangulatePolygon(pB.x, pB.y, mergeB);
if (nA==-1 || nB==-1)
return -1;
for (int i=0; i<nA; ++i)
results.Add(mergeA[i]);
for (int i=0; i<nB; ++i)
results.Add(mergeB[i]);
return (nA+nB);
}
int vNum = xv.Count;
List<Triangle> buffer = new List<Triangle>();
float[] xrem = new float[vNum];
float[] yrem = new float[vNum];
for (int i = 0; i < vNum; ++i)
{
xrem[i] = xv[i];
yrem[i] = yv[i];
}
int xremLength = vNum;
while (vNum > 3)
{
// Find an ear
int earIndex = -1;
//float earVolume = -1.0f;
float earMaxMinCross = -10.0f;
for (int i = 0; i < vNum; ++i)
{
if (IsEar(i, xrem, yrem))
{
int lower = remainder(i-1, vNum);
int upper = remainder(i+1, vNum);
Vec2 d1 = new Vec2(xrem[upper]-xrem[i], yrem[upper]-yrem[i]);
Vec2 d2 = new Vec2(xrem[i]-xrem[lower], yrem[i]-yrem[lower]);
Vec2 d3 = new Vec2(xrem[lower]-xrem[upper], yrem[lower]-yrem[upper]);
d1.Normalize();
d2.Normalize();
d3.Normalize();
float cross12 = Math.Abs(d1.Cross(d2));
float cross23 = Math.Abs(d2.Cross(d3));
float cross31 = Math.Abs(d3.Cross(d1));
//Find the maximum minimum angle
float minCross = Math.Min(cross12, Math.Min(cross23, cross31));
if (minCross > earMaxMinCross)
{
earIndex = i;
earMaxMinCross = minCross;
}
/*//This bit chooses the ear with greatest volume first
float testVol = b2Abs( d1.X*d2.Y-d2.X*d1.Y );
if (testVol > earVolume){
earIndex = i;
earVolume = testVol;
}*/
}
}
// If we still haven't found an ear, we're screwed.
// Note: sometimes this is happening because the
// remaining points are collinear. Really these
// should just be thrown out without halting triangulation.
if (earIndex == -1)
{
if (Polygon.ReportErrors)
{
Polygon dump = new Polygon(xrem.ToList<float>(), yrem.ToList<float>());
//printf("Couldn't find an ear, dumping remaining poly:\n");
//printf("Please submit this dump to ewjordan at Box2d forums\n");
throw new Exception("Couldn't find ear");
}
/*for (int i = 0; i < buffer.Count; i++)
results.Add(buffer[i]);
if (buffer.Count > 0)
return buffer.Count;
else
return -1;*/
}
// Clip off the ear:
// - remove the ear tip from the list
--vNum;
float[] newx = new float[vNum];
float[] newy = new float[vNum];
int currDest = 0;
for (int i = 0; i < vNum; ++i)
{
if (currDest == earIndex) ++currDest;
newx[i] = xrem[currDest];
newy[i] = yrem[currDest];
++currDest;
}
// - add the clipped triangle to the triangle list
int under = (earIndex == 0) ? (vNum) : (earIndex - 1);
int over = (earIndex == vNum) ? 0 : (earIndex + 1);
Triangle toAdd = new Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under], yrem[under]);
buffer.Add(toAdd);
// - replace the old list with the new one
xrem = newx;
yrem = newy;
}
Triangle ntoAdd = new Triangle(xrem[1], yrem[1], xrem[2], yrem[2],
xrem[0], yrem[0]);
buffer.Add(ntoAdd);
if (!(buffer.Count == xremLength-2))
throw new Exception("wat");
for (int i = 0; i < buffer.Count; i++)
results.Add(buffer[i]);
return buffer.Count;
}
public bool IsUsable(bool printError = ReportErrors)
{
int error = -1;
bool noError = true;
if (nVertices < 3 || nVertices > maxVerticesPerPolygon) { noError = false; error = 0; }
if (!IsConvex()) { noError = false; error = 1; }
if (!IsSimple()) { noError = false; error = 2; }
if (GetArea() < Statics.FLT_EPSILON) { noError = false; error = 3; }
//Compute normals
List<Vec2> normals = new List<Vec2>();
List<Vec2> vertices = new List<Vec2>();
for (int i = 0; i < nVertices; ++i)
{
vertices[i] = new Vec2(x[i], y[i]);
int i1 = i;
int i2 = i + 1 < nVertices ? i + 1 : 0;
Vec2 edge = new Vec2(x[i2]-x[i1], y[i2]-y[i1]);
normals[i] = edge.Cross(1.0f);
normals[i].Normalize();
}
//Required side checks
for (int i=0; i<nVertices; ++i)
{
int iminus = (i==0)?nVertices-1:i-1;
//int iplus = (i==nVertices-1)?0:i+1;
//Parallel sides check
float cross = normals[iminus].Cross(normals[i]);
cross = b2Math.b2Clamp(cross, -1.0f, 1.0f);
float angle = (float)Math.Asin(cross);
if (angle <= Box2DSettings.b2_angularSlop)
{
noError = false;
error = 4;
break;
}
//Too skinny check
for (int j=0; j<nVertices; ++j)
{
if (j == i || j == (i + 1) % nVertices)
{
continue;
}
float s = normals[i].Dot(vertices[j] - vertices[i]);
if (s >= -Box2DSettings.b2_angularSlop)
{
noError = false;
error = 5;
}
}
Vec2 centroid = Statics.PolyCentroid(vertices);
Vec2 n1 = normals[iminus];
Vec2 n2 = normals[i];
Vec2 v = vertices[i] - centroid; ;
Vec2 d = new Vec2(
n1.Dot(v) - 0.04f,
n2.Dot(v) - 0.04f);
// Shifting the edge inward by b2_toiSlop should
// not cause the plane to pass the centroid.
if ((d.X < 0.0f)||(d.Y < 0.0f))
{
noError = false;
error = 6;
}
}
if (!noError && printError)
{
string exceptionStr = "Found invalid polygon, ";
switch (error)
{
case 0:
exceptionStr += "must have between 3 and "+Polygon.maxVerticesPerPolygon.ToString()+" vertices.\n";
break;
case 1:
exceptionStr += "must be convex.\n";
break;
case 2:
exceptionStr += "must be simple (cannot intersect itself).\n";
break;
case 3:
exceptionStr += "area is too small.\n";
break;
case 4:
exceptionStr += "sides are too close to parallel.\n";
break;
case 5:
exceptionStr += "polygon is too thin.\n";
break;
case 6:
exceptionStr += "core shape generation would move edge past centroid (too thin).\n";
break;
default:
exceptionStr +="don't know why.\n";
break;
}
throw new Exception(exceptionStr);
}
return noError;
}
