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); } }
public Rot clone() { Rot copy = new Rot(); copy.s = s; copy.c = c; return copy; }
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); }
final Vec2 temp = pool.popVec2(); qA.set(aA); 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); d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); 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_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);
Rot.mulTransUnsafe(xfa.q, m_axis, axisA); Rot.mulTransUnsafe(xfb.q, m_axis.negateLocal(), axisB); m_axis.negateLocal(); Rot.mulToOutUnsafe(xfa.q, m_axis, normal); Transform.mulToOutUnsafe(xfa, m_localPoint, pointA); Rot.mulTransUnsafe(xfb.q, normal.negateLocal(), axisB); normal.negateLocal(); Rot.mulToOutUnsafe(xfb.q, m_axis, normal); Transform.mulToOutUnsafe(xfb, m_localPoint, pointB); Rot.mulTransUnsafe(xfa.q, normal.negateLocal(), axisA); normal.negateLocal();
qA.set(aA); qB.set(aB); qC.set(aC); qD.set(aD); 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); Rot.mulTransUnsafe(qC, temp.set(rA).addLocal(cA).subLocal(cC), pA); coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC); pool.pushVec2(4); 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); Rot.mulTransUnsafe(qD, temp.set(rB).addLocal(cB).subLocal(cD), pB); coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD); pool.pushVec2(5);
public final void getLocalVectorToOutUnsafe(Vec2 worldVector, Vec2 out) { Rot.mulTransUnsafe(m_xf.q, worldVector, out); }
public Rot(float angle) { set(angle); }
public final void getLocalVectorToOut(Vec2 worldVector, Vec2 out) { Rot.mulTrans(m_xf.q, worldVector, out); }
/** Initialize as a copy of another transform. */ public Transform(final Transform xf) { p = xf.p.clone(); q = xf.q.clone(); }
public final void getWorldVectorToOut(Vec2 localVector, Vec2 out) { Rot.mulToOut(m_xf.q, localVector, out); }
public final void getWorldVectorToOutUnsafe(Vec2 localVector, Vec2 out) { Rot.mulToOutUnsafe(m_xf.q, localVector, out); }
xf.q.getXAxis(axis);
public float getAngle() { return m_transform.q.getAngle(); }
/** * 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 final void setAsBox(final float hx, final float hy, final Vec2 center, final float 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.0f, -1.0f); m_normals[1].set(1.0f, 0.0f); m_normals[2].set(0.0f, 1.0f); m_normals[3].set(-1.0f, 0.0f); m_centroid.set(center); final 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]); } }
final Vec2 temp = pool.popVec2(); qA.set(aA); 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); d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); 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_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);
Rot.mulTransUnsafe(xfa.q, m_axis, axisA); Rot.mulTransUnsafe(xfb.q, m_axis.negateLocal(), axisB); m_axis.negateLocal(); Rot.mulToOutUnsafe(xfa.q, m_axis, normal); Transform.mulToOutUnsafe(xfa, m_localPoint, pointA); Rot.mulTransUnsafe(xfb.q, normal.negateLocal(), axisB); normal.negateLocal(); Rot.mulToOutUnsafe(xfb.q, m_axis, normal); Transform.mulToOutUnsafe(xfb, m_localPoint, pointB); Rot.mulTransUnsafe(xfa.q, normal.negateLocal(), axisA); normal.negateLocal();
qA.set(aA); qB.set(aB); qC.set(aC); qD.set(aD); 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); Rot.mulTransUnsafe(qC, temp.set(rA).addLocal(cA).subLocal(cC), pA); coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC); pool.pushVec2(4); 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); Rot.mulTransUnsafe(qD, temp.set(rB).addLocal(cB).subLocal(cD), pB); coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD); pool.pushVec2(5);