@Override public Rotation clone() { Rotation copy = new Rotation(); copy.s = s; copy.c = c; return copy; }
/** Initialize using a position vector and a rotation matrix. */ public Transform(final Vec2 _position, final Rotation _R) { p = _position.clone(); q = _R.clone(); }
public float getAngle() { return m_transform.q.getAngle(); }
public final static void mulToOutUnsafe(final Transform A, final Transform B, final Transform out) { assert (out != B); assert (out != A); Rotation.mulUnsafe(A.q, B.q, out.q); Rotation.mulToOutUnsafe(A.q, B.p, out.p); out.p.addLocal(A.p); }
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); Rotation.mulToOutUnsafe(m_xf.q, m_sweep.localCenter, m_xf.p); m_xf.p.mulLocal(-1).addLocal(m_sweep.c); } }
Rotation.mulTransUnsafe(xfa.q, m_axis, axisA); Rotation.mulTransUnsafe(xfb.q, m_axis.negateLocal(), axisB); m_axis.negateLocal(); Rotation.mulToOutUnsafe(xfa.q, m_axis, normal); Transform.mulToOutUnsafe(xfa, m_localPoint, pointA); Rotation.mulTransUnsafe(xfb.q, normal.negateLocal(), axisB); normal.negateLocal(); Rotation.mulToOutUnsafe(xfb.q, m_axis, normal); Transform.mulToOutUnsafe(xfb, m_localPoint, pointB); Rotation.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(); Rotation.mulToOutUnsafe(qC, m_localAxisC, JvAC); Rotation.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC); Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA); JwC = Vec2.cross(rC, JvAC); JwA = Vec2.cross(rA, JvAC); Rotation.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(); Rotation.mulToOutUnsafe(qD, m_localAxisD, u); Rotation.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD); Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB); JvBD.set(u).mulLocal(m_ratio); JwD = Vec2.cross(rD, u); Rotation.mulTransUnsafe(qD, temp.set(rB).addLocal(cB).subLocal(cD), pB); coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD); pool.pushVec2(5);
final Vec2 temp = pool.popVec2(); qA.set(aA); qB.set(aB); Rotation.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rotation.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); Rotation.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) { Rotation.mulToOut(qA, m_localXAxisA, m_ax); m_sAx = Vec2.cross(temp.set(d).addLocal(rA), m_ax); m_sBx = Vec2.cross(rB, m_ax);
final Vec2 temp = pool.popVec2(); qA.set(aA); qB.set(aB); Rotation.mulToOut(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rotation.mulToOut(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA); Rotation.mulToOut(qA, m_localYAxisA, ay);
public final void getWorldVectorToOutUnsafe(Vec2 localVector, Vec2 out) { Rotation.mulToOutUnsafe(m_xf.q, localVector, out); }
public final void getLocalVectorToOutUnsafe(Vec2 worldVector, Vec2 out) { Rotation.mulTransUnsafe(m_xf.q, worldVector, out); }
public final void getLocalVectorToOut(Vec2 worldVector, Vec2 out) { Rotation.mulTrans(m_xf.q, worldVector, out); }
public final void getWorldVectorToOut(Vec2 localVector, Vec2 out) { Rotation.mulToOut(m_xf.q, localVector, out); }
xf.q.getXAxis(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; Rotation rotation = tempRotation; rotation.set(step.dt * group.m_angularVelocity); Rotation.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]); } } } }
Rotation.mulTransUnsafe(xfa.q, m_axis, axisA); Rotation.mulTransUnsafe(xfb.q, m_axis.negateLocal(), axisB); m_axis.negateLocal(); Rotation.mulToOutUnsafe(xfa.q, m_axis, normal); Transform.mulToOutUnsafe(xfa, m_localPoint, pointA); Rotation.mulTransUnsafe(xfb.q, normal.negateLocal(), axisB); normal.negateLocal(); Rotation.mulToOutUnsafe(xfb.q, m_axis, normal); Transform.mulToOutUnsafe(xfb, m_localPoint, pointB); Rotation.mulTransUnsafe(xfa.q, normal.negateLocal(), axisA); normal.negateLocal();
public final static Transform mul(final Transform A, final Transform B) { Transform C = new Transform(); Rotation.mulUnsafe(A.q, B.q, C.q); Rotation.mulToOutUnsafe(A.q, B.p, C.p); C.p.addLocal(A.p); return C; }
/** * 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) { vertexCount = 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 < vertexCount; ++i) { Transform.mulToOut(xf, m_vertices[i], m_vertices[i]); Rotation.mulToOut(xf.q, m_normals[i], m_normals[i]); } }
for (int i = 0; i < count1; i++) { Rotation.mulToOutUnsafe(xfq, n1s[i], n); Transform.mulToOutUnsafe(xf, v1s[i], v1);