/**
* Build vertices to represent an oriented box.
*
* @param hx the half-width.
* @param hy the half-height.
* @param center the center of the box in local coordinates.
* @param angle the rotation of the box in local coordinates.
*/
public void setAsBox(double hx, double hy, Vec2 center, double angle)
{
m_count = 4;
m_vertices[0].set(-hx, -hy);
m_vertices[1].set(hx, -hy);
m_vertices[2].set(hx, hy);
m_vertices[3].set(-hx, hy);
m_normals[0].set(0.0d, -1.0d);
m_normals[1].set(1.0d, 0.0d);
m_normals[2].set(0.0d, 1.0d);
m_normals[3].set(-1.0d, 0.0d);
m_centroid.set(center);
Transform xf = poolt1;
xf.p.set(center);
xf.q.set(angle);
// Transform vertices and normals.
for (int i = 0; i < m_count; ++i)
{
Transform.mulToOut(xf, m_vertices[i], m_vertices[i]);
Rot.mulToOut(xf.q, m_normals[i], m_normals[i]);
}
}
public void getWorldVectorToOut(Vec2 localVector, Vec2 out_)
{
Rot.mulToOut(m_xf.q, localVector, out_);
}
public static void mulToOut(Transform A, Transform B, Transform out_)
{
Rot.mul(A.q, B.q, out_.q);
Rot.mulToOut(A.q, B.p, out_.p);
out_.p.addLocal(A.p);
}
public override bool solvePositionConstraints(SolverData data)
{
Vec2 cA = data.positions[m_indexA].c;
double aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
double aB = data.positions[m_indexB].a;
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 temp = pool.popVec2();
qA.set(aA);
qB.set(aB);
Rot.mulToOut(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOut(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
d2.set(cB).subLocal(cA).addLocal(rB).subLocal(rA);
Vec2 ay = pool.popVec2();
Rot.mulToOut(qA, m_localYAxisA, ay);
double sAy = Vec2.cross(temp.set(d2).addLocal(rA), ay);
double sBy = Vec2.cross(rB, ay);
double C = Vec2.dot(d2, ay);
double k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
double impulse;
if (k != 0.0d)
{
impulse = -C / k;
}
else
{
impulse = 0.0d;
}
Vec2 P = pool.popVec2();
P.x = impulse * ay.x;
P.y = impulse * ay.y;
double LA = impulse * sAy;
double LB = impulse * sBy;
cA.x -= m_invMassA * P.x;
cA.y -= m_invMassA * P.y;
aA -= m_invIA * LA;
cB.x += m_invMassB * P.x;
cB.y += m_invMassB * P.y;
aB += m_invIB * LB;
pool.pushVec2(3);
pool.pushRot(2);
// 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(MathUtils.abs(C) <= Settings.linearSlop);
}
// pooling
public override void initVelocityConstraints(SolverData data)
{
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA.set(m_bodyA.m_sweep.localCenter);
m_localCenterB.set(m_bodyB.m_sweep.localCenter);
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.m_invI;
m_invIB = m_bodyB.m_invI;
double mA = m_invMassA, mB = m_invMassB;
double iA = m_invIA, iB = m_invIB;
Vec2 cA = data.positions[m_indexA].c;
double aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
double wA = data.velocities[m_indexA].w;
Vec2 cB = data.positions[m_indexB].c;
double aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
double wB = data.velocities[m_indexB].w;
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 temp = pool.popVec2();
qA.set(aA);
qB.set(aB);
// Compute the effective masses.
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
d2.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
// Point to line constraint
{
Rot.mulToOut(qA, m_localYAxisA, m_ay);
m_sAy = Vec2.cross(temp.set(d2).addLocal(rA), m_ay);
m_sBy = Vec2.cross(rB, m_ay);
m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
if (m_mass > 0.0d)
{
m_mass = 1.0d / m_mass;
}
}
// Spring constraint
m_springMass = 0.0d;
m_bias = 0.0d;
m_gamma = 0.0d;
if (m_frequencyHz > 0.0d)
{
Rot.mulToOut(qA, m_localXAxisA, m_ax);
m_sAx = Vec2.cross(temp.set(d2).addLocal(rA), m_ax);
m_sBx = Vec2.cross(rB, m_ax);
double invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
if (invMass > 0.0d)
{
m_springMass = 1.0d / invMass;
double C = Vec2.dot(d2, m_ax);
// Frequency
double omega = 2.0d * MathUtils.PI * m_frequencyHz;
// Damping coefficient
double d = 2.0d * m_springMass * m_dampingRatio * omega;
// Spring stiffness
double k = m_springMass * omega * omega;
// magic formulas
double h = data.step.dt;
m_gamma = h * (d + h * k);
if (m_gamma > 0.0d)
{
m_gamma = 1.0d / m_gamma;
}
m_bias = C * h * k * m_gamma;
m_springMass = invMass + m_gamma;
if (m_springMass > 0.0d)
{
m_springMass = 1.0d / m_springMass;
}
}
}
else
{
m_springImpulse = 0.0d;
}
// Rotational motor
if (m_enableMotor)
{
m_motorMass = iA + iB;
if (m_motorMass > 0.0d)
{
m_motorMass = 1.0d / m_motorMass;
}
}
else
{
m_motorMass = 0.0d;
m_motorImpulse = 0.0d;
}
if (data.step.warmStarting)
{
Vec2 P = pool.popVec2();
// Account for variable time step.
m_impulse *= data.step.dtRatio;
m_springImpulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
P.x = m_impulse * m_ay.x + m_springImpulse * m_ax.x;
P.y = m_impulse * m_ay.y + m_springImpulse * m_ax.y;
double LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
double LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;
vA.x -= m_invMassA * P.x;
vA.y -= m_invMassA * P.y;
wA -= m_invIA * LA;
vB.x += m_invMassB * P.x;
vB.y += m_invMassB * P.y;
wB += m_invIB * LB;
pool.pushVec2(1);
}
else
{
m_impulse = 0.0d;
m_springImpulse = 0.0d;
m_motorImpulse = 0.0d;
}
pool.pushRot(2);
pool.pushVec2(1);
// data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
// data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
