public final void getWorldVectorToOutUnsafe(Vec2 localVector, Vec2 out) { Rot.mulToOutUnsafe(m_xf.q, localVector, out); }
public final static void mulToOutUnsafe(final Transform A, final Transform B, final Transform out) { assert (out != B); assert (out != A); Rot.mulUnsafe(A.q, B.q, out.q); Rot.mulToOutUnsafe(A.q, B.p, out.p); out.p.addLocal(A.p); }
public final static Transform mul(final Transform A, final Transform B) { Transform C = new Transform(); Rot.mulUnsafe(A.q, B.q, C.q); Rot.mulToOutUnsafe(A.q, B.p, C.p); C.p.addLocal(A.p); return C; }
for (int i = 0; i < count1; i++) { Rot.mulToOutUnsafe(xfq, n1s[i], n); Transform.mulToOutUnsafe(xf, v1s[i], v1);
protected final void advance(float t) { // Advance to the new safe time. This doesn't sync the broad-phase. m_sweep.advance(t); m_sweep.c.set(m_sweep.c0); m_sweep.a = m_sweep.a0; m_xf.q.set(m_sweep.a); // m_xf.position = m_sweep.c - Mul(m_xf.R, m_sweep.localCenter); Rot.mulToOutUnsafe(m_xf.q, m_sweep.localCenter, m_xf.p); m_xf.p.mulLocal(-1).addLocal(m_sweep.c); } }
Rot.mulToOutUnsafe(bA.m_xf.q, temp, rA); Rot.mulToOutUnsafe(bB.m_xf.q, temp, rB); Rot.mulToOutUnsafe(bA.m_xf.q, m_localXAxisA, axis);
void solveRigid(final TimeStep step) { for (ParticleGroup group = m_groupList; group != null; group = group.getNext()) { if ((group.m_groupFlags & ParticleGroupType.b2_rigidParticleGroup) != 0) { group.updateStatistics(); Vec2 temp = tempVec; Vec2 cross = tempVec2; Rot rotation = tempRot; rotation.set(step.dt * group.m_angularVelocity); Rot.mulToOutUnsafe(rotation, group.m_center, cross); temp.set(group.m_linearVelocity).mulLocal(step.dt).addLocal(group.m_center).subLocal(cross); tempXf.p.set(temp); tempXf.q.set(rotation); Transform.mulToOut(tempXf, group.m_transform, group.m_transform); final Transform velocityTransform = tempXf2; velocityTransform.p.x = step.inv_dt * tempXf.p.x; velocityTransform.p.y = step.inv_dt * tempXf.p.y; velocityTransform.q.s = step.inv_dt * tempXf.q.s; velocityTransform.q.c = step.inv_dt * (tempXf.q.c - 1); for (int i = group.m_firstIndex; i < group.m_lastIndex; i++) { Transform.mulToOutUnsafe(velocityTransform, m_positionBuffer.data[i], m_velocityBuffer.data[i]); } } } }
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
qB.set(aB); Rot.mulToOutUnsafe(qA, u.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, u.set(m_localAnchorB).subLocal(m_localCenterB), rB); u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
Rot.mulToOutUnsafe(qA, m_u.set(m_localAnchorA).subLocal(m_localCenterA), m_rA); Rot.mulToOutUnsafe(qB, m_u.set(m_localAnchorB).subLocal(m_localCenterB), m_rB); m_u.set(cB).addLocal(m_rB).subLocal(cA).subLocal(m_rA);
Rot.mulToOutUnsafe(xfa.q, m_axis, normal); Transform.mulToOutUnsafe(xfa, m_localPoint, pointA); Rot.mulToOutUnsafe(xfb.q, m_axis, normal); Transform.mulToOutUnsafe(xfb, m_localPoint, pointB);
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
final Vec2 impulse = pool.popVec2(); Rot.mulToOutUnsafe(qA, C.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, C.set(m_localAnchorB).subLocal(m_localCenterB), rB); C.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); positionError = C.length();
Rot.mulToOutUnsafe(xfa.q, m_axis, normal); Transform.mulToOutUnsafe(xfa, m_localPoint, pointA); Rot.mulToOutUnsafe(xfb.q, m_axis, normal); Transform.mulToOutUnsafe(xfb, m_localPoint, pointB);
Rot.mulToOutUnsafe(xfA.q, m_localAnchorA, temp); temp.addLocal(xfA.p).subLocal(xfC.p); Rot.mulTransUnsafe(xfC.q, temp, pA); Rot.mulToOutUnsafe(xfB.q, m_localAnchorB, temp); temp.addLocal(xfB.p).subLocal(xfD.p); Rot.mulTransUnsafe(xfD.q, temp, pB);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
qB.set(aB); Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
float iA = m_invIA, iB = m_invIB; Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); float positionError, angularError;
m_axis.normalize(); Rot.mulToOutUnsafe(xfb.q, m_axis, normal); m_axis.normalize(); Rot.mulToOutUnsafe(xfa.q, m_axis, normal);