/// Get the current length of the segment attached to bodyB.
public float GetCurrentLengthB()
{
Vec2 p = m_bodyB.GetWorldPoint(m_localAnchorB);
Vec2 s = m_groundAnchorB;
Vec2 d = p - s;
return(d.Length());
}
/// Initialize the bodies, anchors, and length using the world
/// anchors.
// 1-D constrained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
public void Initialize(Body b1, Body b2,
Vec2 anchor1, Vec2 anchor2)
{
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA.GetLocalPoint(anchor1);
localAnchorB = bodyB.GetLocalPoint(anchor2);
Vec2 d = anchor2 - anchor1;
length = d.Length();
}
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
// Pulley:
// length1 = norm(p1 - s1)
// length2 = norm(p2 - s2)
// C0 = (length1 + ratio * length2)_initial
// C = C0 - (length1 + ratio * length2)
// u1 = (p1 - s1) / norm(p1 - s1)
// u2 = (p2 - s2) / norm(p2 - s2)
// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
public void Initialize(Body bA, Body bB,
Vec2 groundA, Vec2 groundB,
Vec2 anchorA, Vec2 anchorB,
float r)
{
bodyA = bA;
bodyB = bB;
groundAnchorA = groundA;
groundAnchorB = groundB;
localAnchorA = bodyA.GetLocalPoint(anchorA);
localAnchorB = bodyB.GetLocalPoint(anchorB);
Vec2 dA = anchorA - groundA;
lengthA = dA.Length();
Vec2 dB = anchorB - groundB;
lengthB = dB.Length();
ratio = r;
Utilities.Assert(ratio > Single.Epsilon);
}
public static float Distance(Vec2 a, Vec2 b)
{
Vec2 c = a - b;
return(c.Length());
}
internal override bool SolvePositionConstraints(SolverData data)
{
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA);
Rot qB = new Rot(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
float positionError, angularError;
Mat33 K;
K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
K.ez.X = -rA.Y * iA - rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
K.ez.Y = rA.X * iA + rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (m_frequencyHz > 0.0f)
{
Vec2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
Vec2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * Utilities.Cross(rA, P);
cB += mB * P;
aB += iB * Utilities.Cross(rB, P);
}
else
{
Vec2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - m_referenceAngle;
positionError = C1.Length();
angularError = Math.Abs(C2);
Vec3 C = new Vec3(C1.X, C1.Y, C2);
Vec3 impulse = -K.Solve33(C);
Vec2 P = new Vec2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (Utilities.Cross(rA, P) + impulse.Z);
cB += mB * P;
aB += iB * (Utilities.Cross(rB, P) + impulse.Z);
}
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(positionError <= Settings._linearSlop && angularError <= Settings._angularSlop);
}
internal override bool SolvePositionConstraints(SolverData data)
{
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA); Rot qB = new Rot(aB);
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
// Get the pulley axes.
Vec2 uA = cA + rA - m_groundAnchorA;
Vec2 uB = cB + rB - m_groundAnchorB;
float lengthA = uA.Length();
float lengthB = uB.Length();
if (lengthA > 10.0f * Settings._linearSlop)
{
uA *= 1.0f / lengthA;
}
else
{
uA.SetZero();
}
if (lengthB > 10.0f * Settings._linearSlop)
{
uB *= 1.0f / lengthB;
}
else
{
uB.SetZero();
}
// Compute effective mass.
float ruA = Utilities.Cross(rA, uA);
float ruB = Utilities.Cross(rB, uB);
float mA = m_invMassA + m_invIA * ruA * ruA;
float mB = m_invMassB + m_invIB * ruB * ruB;
float mass = mA + m_ratio * m_ratio * mB;
if (mass > 0.0f)
{
mass = 1.0f / mass;
}
float C = m_constant - lengthA - m_ratio * lengthB;
float linearError = Math.Abs(C);
float impulse = -mass * C;
Vec2 PA = -impulse * uA;
Vec2 PB = -m_ratio * impulse * uB;
cA += m_invMassA * PA;
aA += m_invIA * Utilities.Cross(rA, PA);
cB += m_invMassB * PB;
aB += m_invIB * Utilities.Cross(rB, PB);
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(linearError < Settings._linearSlop);
}
public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep)
{
Timer timer = new Timer();
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < m_bodies.Count(); i++)
{
Body b = m_bodies[i];
Vec2 c = b.m_sweep.c;
float a = b.m_sweep.a;
Vec2 v = b.m_linearVelocity;
float w = b.m_angularVelocity;
// Store positions for continuous collision.
b.m_sweep.c0 = b.m_sweep.c;
b.m_sweep.a0 = b.m_sweep.a;
if (b.m_type == BodyType._dynamicBody)
{
// Integrate velocities.
v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
w += h * b.m_invI * b.m_torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
v *= Utilities.Clamp(1.0f - h * b.m_linearDamping, 0.0f, 1.0f);
w *= Utilities.Clamp(1.0f - h * b.m_angularDamping, 0.0f, 1.0f);
}
Position pos = new Position();
pos.c = c;
pos.a = a;
m_positions.Add(pos);
Velocity vel = new Velocity();
vel.v = v;
vel.w = w;
m_velocities.Add(vel);
}
timer.Reset();
// Solver data
SolverData solverData;
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
ContactSolverDef contactSolverDef;
contactSolverDef.step = step;
contactSolverDef.contacts = m_contacts;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
ContactSolver contactSolver = new ContactSolver(contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int i = 0; i < m_joints.Count(); ++i)
{
m_joints[i].InitVelocityConstraints(solverData);
}
profile.solveInit = timer.GetMilliseconds();
// Solve velocity constraints
timer.Reset();
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < m_joints.Count(); ++j)
{
m_joints[j].SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
profile.solveVelocity = timer.GetMilliseconds();
// Integrate positions
for (int i = 0; i < m_bodies.Count(); ++i)
{
Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
Vec2 translation = h * v;
if (Utilities.Dot(translation, translation) > Settings._maxTranslationSquared)
{
float ratio = Settings._maxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > Settings._maxRotationSquared)
{
float ratio = Settings._maxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
// Solve position constraints
timer.Reset();
bool positionSolved = false;
for (int i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int j = 0; j < m_joints.Count; ++j)
{
bool jointOkay = m_joints[j].SolvePositionConstraints(solverData);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
// Copy state buffers back to the bodies
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body body = m_bodies[i];
body.m_sweep.c = m_positions[i].c;
body.m_sweep.a = m_positions[i].a;
body.m_linearVelocity = m_velocities[i].v;
body.m_angularVelocity = m_velocities[i].w;
body.SynchronizeTransform();
}
profile.solvePosition = timer.GetMilliseconds();
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float minSleepTime = Single.MaxValue;
const float linTolSqr = Settings._linearSleepTolerance * Settings._linearSleepTolerance;
const float angTolSqr = Settings._angularSleepTolerance * Settings._angularSleepTolerance;
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body b = m_bodies[i];
if (b.GetBodyType() == BodyType._staticBody)
{
continue;
}
if ((b.m_flags & Body.BodyFlags.e_autoSleepFlag) == 0 ||
b.m_angularVelocity * b.m_angularVelocity > angTolSqr ||
Utilities.Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
{
b.m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.m_sleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.m_sleepTime);
}
}
if (minSleepTime >= Settings._timeToSleep && positionSolved)
{
for (int i = 0; i < m_bodies.Count(); ++i)
{
Body b = m_bodies[i];
b.SetAwake(false);
}
}
}
}
internal override bool SolvePositionConstraints(SolverData data)
{
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA);
Rot qB = new Rot(aB);
float angularError = 0.0f;
float positionError = 0.0f;
bool fixedRotation = (m_invIA + m_invIB == 0.0f);
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != LimitState.e_inactiveLimit && fixedRotation == false)
{
float angle = aB - aA - m_referenceAngle;
float limitImpulse = 0.0f;
if (m_limitState == LimitState.e_equalLimits)
{
// Prevent large angular corrections
float C = Utilities.Clamp(angle - m_lowerAngle, -Settings._maxAngularCorrection, Settings._maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = Math.Abs(C);
}
else if (m_limitState == LimitState.e_atLowerLimit)
{
float C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = Utilities.Clamp(C + Settings._angularSlop, -Settings._maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == LimitState.e_atUpperLimit)
{
float C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = Utilities.Clamp(C - Settings._angularSlop, 0.0f, Settings._maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
aA -= m_invIA * limitImpulse;
aB += m_invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
Vec2 C = cB + rB - cA - rA;
positionError = C.Length();
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Mat22 K;
K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;
Vec2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * Utilities.Cross(rA, impulse);
cB += mB * impulse;
aB += iB * Utilities.Cross(rB, impulse);
}
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(positionError <= Settings._linearSlop && angularError <= Settings._angularSlop);
}
