use of org.jbox2d.common.Vec2 in project libgdx by libgdx.
the class GearJoint method solvePositionConstraints.
@Override
public boolean 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;
Vec2 cC = data.positions[m_indexC].c;
float aC = data.positions[m_indexC].a;
Vec2 cD = data.positions[m_indexD].c;
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;
Vec2 temp = pool.popVec2();
Vec2 JvAC = pool.popVec2();
Vec2 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 {
Vec2 rC = pool.popVec2();
Vec2 rA = pool.popVec2();
Vec2 pC = pool.popVec2();
Vec2 pA = pool.popVec2();
Rot.mulToOutUnsafe(qC, m_localAxisC, JvAC);
Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC);
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA);
JwC = Vec2.cross(rC, JvAC);
JwA = Vec2.cross(rA, JvAC);
mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
pC.set(m_localAnchorC).subLocal(m_lcC);
Rot.mulTransUnsafe(qC, temp.set(rA).addLocal(cA).subLocal(cC), pA);
coordinateA = Vec2.dot(pA.subLocal(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 {
Vec2 u = pool.popVec2();
Vec2 rD = pool.popVec2();
Vec2 rB = pool.popVec2();
Vec2 pD = pool.popVec2();
Vec2 pB = pool.popVec2();
Rot.mulToOutUnsafe(qD, m_localAxisD, u);
Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB);
JvBD.set(u).mulLocal(m_ratio);
JwD = Vec2.cross(rD, u);
JwB = Vec2.cross(rB, u);
mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
pD.set(m_localAnchorD).subLocal(m_lcD);
Rot.mulTransUnsafe(qD, temp.set(rB).addLocal(cB).subLocal(cD), pB);
coordinateB = Vec2.dot(pB.subLocal(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 org.jbox2d.common.Vec2 in project libgdx by libgdx.
the class WheelJoint method initVelocityConstraints.
@Override
public 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;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
final Rot qA = pool.popRot();
final Rot qB = pool.popRot();
final 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);
d.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(d).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.0f) {
m_mass = 1.0f / m_mass;
}
}
// Spring constraint
m_springMass = 0.0f;
m_bias = 0.0f;
m_gamma = 0.0f;
if (m_frequencyHz > 0.0f) {
Rot.mulToOut(qA, m_localXAxisA, m_ax);
m_sAx = Vec2.cross(temp.set(d).addLocal(rA), m_ax);
m_sBx = Vec2.cross(rB, m_ax);
float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
if (invMass > 0.0f) {
m_springMass = 1.0f / invMass;
float C = Vec2.dot(d, m_ax);
// Frequency
float omega = 2.0f * MathUtils.PI * m_frequencyHz;
// Damping coefficient
float d = 2.0f * m_springMass * m_dampingRatio * omega;
// Spring stiffness
float k = m_springMass * omega * omega;
// magic formulas
float h = data.step.dt;
m_gamma = h * (d + h * k);
if (m_gamma > 0.0f) {
m_gamma = 1.0f / m_gamma;
}
m_bias = C * h * k * m_gamma;
m_springMass = invMass + m_gamma;
if (m_springMass > 0.0f) {
m_springMass = 1.0f / m_springMass;
}
}
} else {
m_springImpulse = 0.0f;
}
// Rotational motor
if (m_enableMotor) {
m_motorMass = iA + iB;
if (m_motorMass > 0.0f) {
m_motorMass = 1.0f / m_motorMass;
}
} else {
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
}
if (data.step.warmStarting) {
final 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;
float LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
float 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.0f;
m_springImpulse = 0.0f;
m_motorImpulse = 0.0f;
}
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;
}
use of org.jbox2d.common.Vec2 in project libgdx by libgdx.
the class WheelJoint method solvePositionConstraints.
@Override
public boolean 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;
final Rot qA = pool.popRot();
final Rot qB = pool.popRot();
final 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);
d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA);
Vec2 ay = pool.popVec2();
Rot.mulToOut(qA, m_localYAxisA, ay);
float sAy = Vec2.cross(temp.set(d).addLocal(rA), ay);
float sBy = Vec2.cross(rB, ay);
float C = Vec2.dot(d, ay);
float k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
float impulse;
if (k != 0.0f) {
impulse = -C / k;
} else {
impulse = 0.0f;
}
final Vec2 P = pool.popVec2();
P.x = impulse * ay.x;
P.y = impulse * ay.y;
float LA = impulse * sAy;
float 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;
}
use of org.jbox2d.common.Vec2 in project libgdx by libgdx.
the class ParticleSystem method solveCollision.
public void solveCollision(TimeStep step) {
final AABB aabb = temp;
final Vec2 lowerBound = aabb.lowerBound;
final Vec2 upperBound = aabb.upperBound;
lowerBound.x = Float.MAX_VALUE;
lowerBound.y = Float.MAX_VALUE;
upperBound.x = -Float.MAX_VALUE;
upperBound.y = -Float.MAX_VALUE;
for (int i = 0; i < m_count; i++) {
final Vec2 v = m_velocityBuffer.data[i];
final Vec2 p1 = m_positionBuffer.data[i];
final float p1x = p1.x;
final float p1y = p1.y;
final float p2x = p1x + step.dt * v.x;
final float p2y = p1y + step.dt * v.y;
final float bx = p1x < p2x ? p1x : p2x;
final float by = p1y < p2y ? p1y : p2y;
lowerBound.x = lowerBound.x < bx ? lowerBound.x : bx;
lowerBound.y = lowerBound.y < by ? lowerBound.y : by;
final float b1x = p1x > p2x ? p1x : p2x;
final float b1y = p1y > p2y ? p1y : p2y;
upperBound.x = upperBound.x > b1x ? upperBound.x : b1x;
upperBound.y = upperBound.y > b1y ? upperBound.y : b1y;
}
sccallback.step = step;
sccallback.system = this;
m_world.queryAABB(sccallback, aabb);
}
use of org.jbox2d.common.Vec2 in project libgdx by libgdx.
the class ParticleSystem method computeParticleCollisionEnergy.
public float computeParticleCollisionEnergy() {
float sum_v2 = 0;
for (int k = 0; k < m_contactCount; k++) {
final ParticleContact contact = m_contactBuffer[k];
int a = contact.indexA;
int b = contact.indexB;
Vec2 n = contact.normal;
final Vec2 va = m_velocityBuffer.data[a];
final Vec2 vb = m_velocityBuffer.data[b];
final float vx = vb.x - va.x;
final float vy = vb.y - va.y;
float vn = vx * n.x + vy * n.y;
if (vn < 0) {
sum_v2 += vn * vn;
}
}
return 0.5f * getParticleMass() * sum_v2;
}
Aggregations