use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.
the class DistanceJoint method solvePositionConstraints.
@Override
public boolean solvePositionConstraints(final SolverData data) {
if (m_frequencyHz > 0.0f) {
return true;
}
final Rot qA = pool.popRot();
final Rot qB = pool.popRot();
final Tuple2f rA = pool.popVec2();
final Tuple2f rB = pool.popVec2();
final Tuple2f u = pool.popVec2();
Tuple2f cA = data.positions[m_indexA];
float aA = data.positions[m_indexA].a;
Tuple2f cB = data.positions[m_indexB];
float aB = data.positions[m_indexB].a;
qA.set(aA);
qB.set(aB);
Rot.mulToOutUnsafe(qA, u.set(m_localAnchorA).subbed(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, u.set(m_localAnchorB).subbed(m_localCenterB), rB);
u.set(cB).added(rB).subbed(cA).subbed(rA);
float length = u.normalize();
float C = length - m_length;
C = MathUtils.clamp(C, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);
float impulse = -m_mass * C;
float Px = impulse * u.x;
float Py = impulse * u.y;
cA.x -= m_invMassA * Px;
cA.y -= m_invMassA * Py;
aA -= m_invIA * (rA.x * Py - rA.y * Px);
cB.x += m_invMassB * Px;
cB.y += m_invMassB * Py;
aB += m_invIB * (rB.x * Py - rB.y * Px);
// data.positions[m_indexA].c.set(cA);
data.positions[m_indexA].a = aA;
// data.positions[m_indexB].c.set(cB);
data.positions[m_indexB].a = aB;
pool.pushVec2(3);
pool.pushRot(2);
return Math.abs(C) < Settings.linearSlop;
}
use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.
the class DistanceJoint method initVelocityConstraints.
@Override
public void initVelocityConstraints(final SolverData data) {
m_indexA = A.island;
m_indexB = B.island;
m_localCenterA.set(A.sweep.localCenter);
m_localCenterB.set(B.sweep.localCenter);
m_invMassA = A.m_invMass;
m_invMassB = B.m_invMass;
m_invIA = A.m_invI;
m_invIB = B.m_invI;
Tuple2f cA = data.positions[m_indexA];
float aA = data.positions[m_indexA].a;
Tuple2f vA = data.velocities[m_indexA];
float wA = data.velocities[m_indexA].w;
Tuple2f cB = data.positions[m_indexB];
float aB = data.positions[m_indexB].a;
Tuple2f vB = data.velocities[m_indexB];
float wB = data.velocities[m_indexB].w;
final Rot qA = pool.popRot();
final Rot qB = pool.popRot();
qA.set(aA);
qB.set(aB);
// use m_u as temporary variable
Rot.mulToOutUnsafe(qA, m_u.set(m_localAnchorA).subbed(m_localCenterA), m_rA);
Rot.mulToOutUnsafe(qB, m_u.set(m_localAnchorB).subbed(m_localCenterB), m_rB);
m_u.set(cB).added(m_rB).subbed(cA).subbed(m_rA);
pool.pushRot(2);
// Handle singularity.
float length = m_u.length();
if (length > Settings.linearSlop) {
m_u.x *= 1.0f / length;
m_u.y *= 1.0f / length;
} else {
m_u.set(0.0f, 0.0f);
}
float crAu = Tuple2f.cross(m_rA, m_u);
float crBu = Tuple2f.cross(m_rB, m_u);
float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
// Compute the effective mass matrix.
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (m_frequencyHz > 0.0f) {
float C = length - m_length;
// Frequency
float omega = 2.0f * MathUtils.PI * m_frequencyHz;
// Damping coefficient
float d = 2.0f * m_mass * m_dampingRatio * omega;
// Spring stiffness
float k = m_mass * omega * omega;
// magic formulas
float h = data.step.dt;
m_gamma = h * (d + h * k);
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
m_bias = C * h * k * m_gamma;
invMass += m_gamma;
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
} else {
m_gamma = 0.0f;
m_bias = 0.0f;
}
if (data.step.warmStarting) {
// Scale the impulse to support a variable time step.
m_impulse *= data.step.dtRatio;
Tuple2f P = pool.popVec2();
P.set(m_u).scaled(m_impulse);
vA.x -= m_invMassA * P.x;
vA.y -= m_invMassA * P.y;
wA -= m_invIA * Tuple2f.cross(m_rA, P);
vB.x += m_invMassB * P.x;
vB.y += m_invMassB * P.y;
wB += m_invIB * Tuple2f.cross(m_rB, P);
pool.pushVec2(1);
} else {
m_impulse = 0.0f;
}
// data.velocities[m_indexA].v.set(vA);
data.velocities[m_indexA].w = wA;
// data.velocities[m_indexB].v.set(vB);
data.velocities[m_indexB].w = wB;
}
use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.
the class GearJoint method solvePositionConstraints.
@Override
public boolean solvePositionConstraints(SolverData data) {
Tuple2f cA = data.positions[m_indexA];
float aA = data.positions[m_indexA].a;
Tuple2f cB = data.positions[m_indexB];
float aB = data.positions[m_indexB].a;
Tuple2f cC = data.positions[m_indexC];
float aC = data.positions[m_indexC].a;
Tuple2f cD = data.positions[m_indexD];
float aD = data.positions[m_indexD].a;
Rot qA = pool.popRot(), qB = pool.popRot(), qC = pool.popRot(), qD = pool.popRot();
qA.set(aA);
qB.set(aB);
qC.set(aC);
qD.set(aD);
float linearError = 0.0f;
float coordinateA, coordinateB;
Tuple2f temp = pool.popVec2();
Tuple2f JvAC = pool.popVec2();
Tuple2f JvBD = pool.popVec2();
float JwA, JwB, JwC, JwD;
float mass = 0.0f;
if (m_typeA == JointType.REVOLUTE) {
JvAC.setZero();
JwA = 1.0f;
JwC = 1.0f;
mass += m_iA + m_iC;
coordinateA = aA - aC - m_referenceAngleA;
} else {
Tuple2f rC = pool.popVec2();
Tuple2f rA = pool.popVec2();
Tuple2f pC = pool.popVec2();
Tuple2f pA = pool.popVec2();
Rot.mulToOutUnsafe(qC, m_localAxisC, JvAC);
Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subbed(m_lcC), rC);
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subbed(m_lcA), rA);
JwC = Tuple2f.cross(rC, JvAC);
JwA = Tuple2f.cross(rA, JvAC);
mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
pC.set(m_localAnchorC).subbed(m_lcC);
Rot.mulTransUnsafe(qC, temp.set(rA).added(cA).subbed(cC), pA);
coordinateA = Tuple2f.dot(pA.subbed(pC), m_localAxisC);
pool.pushVec2(4);
}
if (m_typeB == JointType.REVOLUTE) {
JvBD.setZero();
JwB = m_ratio;
JwD = m_ratio;
mass += m_ratio * m_ratio * (m_iB + m_iD);
coordinateB = aB - aD - m_referenceAngleB;
} else {
Tuple2f u = pool.popVec2();
Tuple2f rD = pool.popVec2();
Tuple2f rB = pool.popVec2();
Tuple2f pD = pool.popVec2();
Tuple2f pB = pool.popVec2();
Rot.mulToOutUnsafe(qD, m_localAxisD, u);
Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subbed(m_lcD), rD);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subbed(m_lcB), rB);
JvBD.set(u).scaled(m_ratio);
JwD = Tuple2f.cross(rD, u);
JwB = Tuple2f.cross(rB, u);
mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
pD.set(m_localAnchorD).subbed(m_lcD);
Rot.mulTransUnsafe(qD, temp.set(rB).added(cB).subbed(cD), pB);
coordinateB = Tuple2f.dot(pB.subbed(pD), m_localAxisD);
pool.pushVec2(5);
}
float C = (coordinateA + m_ratio * coordinateB) - m_constant;
float impulse = 0.0f;
if (mass > 0.0f) {
impulse = -C / mass;
}
pool.pushVec2(3);
pool.pushRot(4);
cA.x += (m_mA * impulse) * JvAC.x;
cA.y += (m_mA * impulse) * JvAC.y;
aA += m_iA * impulse * JwA;
cB.x += (m_mB * impulse) * JvBD.x;
cB.y += (m_mB * impulse) * JvBD.y;
aB += m_iB * impulse * JwB;
cC.x -= (m_mC * impulse) * JvAC.x;
cC.y -= (m_mC * impulse) * JvAC.y;
aC -= m_iC * impulse * JwC;
cD.x -= (m_mD * impulse) * JvBD.x;
cD.y -= (m_mD * impulse) * JvBD.y;
aD -= m_iD * impulse * JwD;
// data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
// data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
// data.positions[m_indexC].c = cC;
data.positions[m_indexC].a = aC;
// data.positions[m_indexD].c = cD;
data.positions[m_indexD].a = aD;
// TODO_ERIN not implemented
return linearError < Settings.linearSlop;
}
use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.
the class RopeJoint method initVelocityConstraints.
@Override
public void initVelocityConstraints(final SolverData data) {
indexA = A.island;
indexB = B.island;
m_localCenterA.set(A.sweep.localCenter);
m_localCenterB.set(B.sweep.localCenter);
m_invMassA = A.m_invMass;
m_invMassB = B.m_invMass;
m_invIA = A.m_invI;
m_invIB = B.m_invI;
Tuple2f cA = data.positions[indexA];
float aA = data.positions[indexA].a;
Tuple2f vA = data.velocities[indexA];
float wA = data.velocities[indexA].w;
Tuple2f cB = data.positions[indexB];
float aB = data.positions[indexB].a;
Tuple2f vB = data.velocities[indexB];
float wB = data.velocities[indexB].w;
final Rot qA = new Rot();
final Rot qB = new Rot();
final Tuple2f temp = new v2();
qA.set(aA);
qB.set(aB);
// Compute the effective masses.
Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subbed(m_localCenterA), m_rA);
Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subbed(m_localCenterB), m_rB);
m_u.set(cB).added(m_rB).subbed(cA).subbed(m_rA);
length = m_u.length();
float C = length - targetLength();
float ca = Math.abs(C);
if (length > Settings.linearSlop) {
m_u.scaled(1.0f / length);
// if (C > Settings.linearSlop) {
state = LimitState.AT_UPPER;
// }
// /*else if (C < -Settings.linearSlop) {
// state = LimitState.AT_LOWER;
// } */ else {
// state = LimitState.INACTIVE;
// m_u.setZero();
// m_mass = 0.0f;
// m_impulse = 0.0f;
// length = 0;
// return;
// }
} else {
state = LimitState.INACTIVE;
m_u.setZero();
m_mass = 0.0f;
m_impulse = 0.0f;
length = 0;
return;
}
// Compute effective mass.
float crA = Tuple2f.cross(m_rA, m_u);
float crB = Tuple2f.cross(m_rB, m_u);
float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
// if (data.step.warmStarting) {
// Scale the impulse to support a variable time step.
m_impulse *= data.step.dtRatio * positionFactor;
float Px = m_impulse * m_u.x;
float Py = m_impulse * m_u.y;
vA.x -= m_invMassA * Px;
vA.y -= m_invMassA * Py;
wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px);
vB.x += m_invMassB * Px;
vB.y += m_invMassB * Py;
wB += m_invIB * (m_rB.x * Py - m_rB.y * Px);
// } else {
// m_impulse = 0.0f;
// }
// data.velocities[m_indexA].v = vA;
data.velocities[indexA].w = wA;
// data.velocities[m_indexB].v = vB;
data.velocities[indexB].w = wB;
}
use of spacegraph.space2d.phys.common.Rot in project narchy by automenta.
the class RopeJoint method solvePositionConstraints.
@Override
public boolean solvePositionConstraints(final SolverData data) {
final float targetLength = targetLength();
Tuple2f cA = data.positions[indexA];
float aA = data.positions[indexA].a;
Tuple2f cB = data.positions[indexB];
float aB = data.positions[indexB].a;
final Rot qA = pool.popRot();
final Rot qB = pool.popRot();
final Tuple2f u = pool.popVec2();
final Tuple2f rA = pool.popVec2();
final Tuple2f rB = pool.popVec2();
final Tuple2f temp = pool.popVec2();
qA.set(aA);
qB.set(aB);
// Compute the effective masses.
Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subbed(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subbed(m_localCenterB), rB);
u.set(cB).added(rB).subbed(cA).subbed(rA);
float length = u.normalize();
float C = length - targetLength;
// C = MathUtils.clamp(C, 0.0f, Settings.maxLinearCorrection);
float impulse = -m_mass * C;
float Px = impulse * u.x;
float Py = impulse * u.y;
cA.x -= m_invMassA * Px;
cA.y -= m_invMassA * Py;
aA -= m_invIA * (rA.x * Py - rA.y * Px);
cB.x += m_invMassB * Px;
cB.y += m_invMassB * Py;
aB += m_invIB * (rB.x * Py - rB.y * Px);
pool.pushRot(2);
pool.pushVec2(4);
// data.positions[m_indexA].c = cA;
data.positions[indexA].a = aA;
// data.positions[m_indexB].c = cB;
data.positions[indexB].a = aB;
return Math.abs(length - targetLength) < Settings.linearSlop;
}
Aggregations