/** * Get the center of the AABB * * @return */ public final Vec2 getCenter() { final Vec2 center = new Vec2(lowerBound); center.addLocal(upperBound); center.mulLocal(.5f); return center; }
public final static void mulToOut(final Transform A, final Transform B, final Transform out) { assert (out != A); Rot.mul(A.q, B.q, out.q); Rot.mulToOut(A.q, B.p, out.p); out.p.addLocal(A.p); }
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); }
@Override public void computeMass(MassData massData, float density) { massData.mass = 0.0f; massData.center.set(m_vertex1).addLocal(m_vertex2).mulLocal(0.5f); massData.I = 0.0f; }
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; }
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 final void computeCentroidToOut(final Vec2[] vs, final int count, final Vec2 out) { assert (count >= 3); out.set(0.0f, 0.0f); float area = 0.0f; // pRef is the reference point for forming triangles. // It's location doesn't change the result (except for rounding error). final Vec2 pRef = pool1; pRef.setZero(); final Vec2 e1 = pool2; final Vec2 e2 = pool3; final float inv3 = 1.0f / 3.0f; for (int i = 0; i < count; ++i) { // Triangle vertices. final Vec2 p1 = pRef; final Vec2 p2 = vs[i]; final Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0]; e1.set(p2).subLocal(p1); e2.set(p3).subLocal(p1); final float D = Vec2.cross(e1, e2); final float triangleArea = 0.5f * D; area += triangleArea; // Area weighted centroid e1.set(p1).addLocal(p2).addLocal(p3).mulLocal(triangleArea * inv3); out.addLocal(e1); } // Centroid assert (area > Settings.EPSILON); out.mulLocal(1.0f / area); }
@Override public void getReactionForce(float inv_dt, Vec2 argOut) { final Vec2 temp = pool.popVec2(); temp.set(m_ay).mulLocal(m_impulse); argOut.set(m_ax).mulLocal(m_springImpulse).addLocal(temp).mulLocal(inv_dt); pool.pushVec2(1); }
@Override public void getReactionForce(float inv_dt, Vec2 argOut) { Vec2 temp = pool.popVec2(); temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z); argOut.set(m_perp).mulLocal(m_impulse.x).addLocal(temp).mulLocal(inv_dt); pool.pushVec2(1); }
public float raycastCallback(RayCastInput input, int nodeId) { Object userData = broadPhase.getUserData(nodeId); FixtureProxy proxy = (FixtureProxy) userData; Fixture fixture = proxy.fixture; int index = proxy.childIndex; boolean hit = fixture.raycast(output, input, index); if (hit) { float fraction = output.fraction; // Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2; temp.set(input.p2).mulLocal(fraction); point.set(input.p1).mulLocal(1 - fraction).addLocal(temp); return callback.reportFixture(fixture, point, output.normal, fraction); } return input.maxFraction; }
/** * this returns pooled objects. don't keep or modify them * * @return */ public void getClosestPoint(final Vec2 out) { switch (m_count) { case 0: assert (false); out.setZero(); return; case 1: out.set(m_v1.w); return; case 2: case22.set(m_v2.w).mulLocal(m_v2.a); case2.set(m_v1.w).mulLocal(m_v1.a).addLocal(case22); out.set(case2); return; case 3: out.setZero(); return; default: assert (false); out.setZero(); return; } }
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]); } } } }
u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);