/** * Get the extents of the AABB (half-widths). * * @return */ public final Vec2 getExtents() { final Vec2 center = new Vec2(upperBound); center.subLocal(lowerBound); center.mulLocal(.5f); return center; }
/** * Validate convexity. This is a very time consuming operation. * * @return */ public boolean validate() { for (int i = 0; i < m_count; ++i) { int i1 = i; int i2 = i < m_count - 1 ? i1 + 1 : 0; Vec2 p = m_vertices[i1]; Vec2 e = pool1.set(m_vertices[i2]).subLocal(p); for (int j = 0; j < m_count; ++j) { if (j == i1 || j == i2) { continue; } Vec2 v = pool2.set(m_vertices[j]).subLocal(p); float c = Vec2.cross(e, v); if (c < 0.0f) { return false; } } } return true; }
public float getMetric() { switch (m_count) { case 0: assert (false); return 0.0f; case 1: return 0.0f; case 2: return MathUtils.distance(m_v1.w, m_v2.w); case 3: case3.set(m_v2.w).subLocal(m_v1.w); case33.set(m_v3.w).subLocal(m_v1.w); // return Vec2.cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); return Vec2.cross(case3, case33); default: assert (false); return 0.0f; } }
public final static void mulTransToOutUnsafe(final Transform A, final Transform B, final Transform out) { assert (out != A); assert (out != B); Rot.mulTransUnsafe(A.q, B.q, out.q); pool.set(B.p).subLocal(A.p); Rot.mulTransUnsafe(A.q, pool, out.p); }
public final static void mulTransToOut(final Transform A, final Transform B, final Transform out) { assert (out != A); Rot.mulTrans(A.q, B.q, out.q); pool.set(B.p).subLocal(A.p); Rot.mulTrans(A.q, pool, out.p); }
@Override public void initVelocityConstraints(final SolverData step) { Velocity[] velocities = step.velocities; Position[] positions = step.positions; final Vec2[] d = pool.getVec2Array(bodies.length); for (int i = 0; i < bodies.length; ++i) { final int prev = (i == 0) ? bodies.length - 1 : i - 1; final int next = (i == bodies.length - 1) ? 0 : i + 1; d[i].set(positions[bodies[next].m_islandIndex].c); d[i].subLocal(positions[bodies[prev].m_islandIndex].c); } if (step.step.warmStarting) { m_impulse *= step.step.dtRatio; // float lambda = -2.0f * crossMassSum / dotMassSum; // System.out.println(crossMassSum + " " +dotMassSum); // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection, // Settings.maxLinearCorrection); // m_impulse = lambda; for (int i = 0; i < bodies.length; ++i) { velocities[bodies[i].m_islandIndex].v.x += bodies[i].m_invMass * d[i].y * .5f * m_impulse; velocities[bodies[i].m_islandIndex].v.y += bodies[i].m_invMass * -d[i].x * .5f * m_impulse; } } else { m_impulse = 0.0f; } }
public float getLength2() { final Vec2 p = pool.popVec2(); m_bodyB.getWorldPointToOut(m_localAnchorB, p); p.subLocal(m_groundAnchorB); float len = p.length(); pool.pushVec2(1); return len; }
public float getLength1() { final Vec2 p = pool.popVec2(); m_bodyA.getWorldPointToOut(m_localAnchorA, p); p.subLocal(m_groundAnchorA); float len = p.length(); pool.pushVec2(1); return len; }
public float getCurrentLengthA() { final Vec2 p = pool.popVec2(); m_bodyA.getWorldPointToOut(m_localAnchorA, p); p.subLocal(m_groundAnchorA); float length = p.length(); pool.pushVec2(1); return length; }
public float getCurrentLengthB() { final Vec2 p = pool.popVec2(); m_bodyB.getWorldPointToOut(m_localAnchorB, p); p.subLocal(m_groundAnchorB); float length = p.length(); pool.pushVec2(1); return length; }
public final static Transform mulTrans(final Transform A, final Transform B) { Transform C = new Transform(); Rot.mulTransUnsafe(A.q, B.q, C.q); pool.set(B.p).subLocal(A.p); Rot.mulTransUnsafe(A.q, pool, C.p); return C; }
@Override public void solveVelocityConstraints(final SolverData step) { float crossMassSum = 0.0f; float dotMassSum = 0.0f; Velocity[] velocities = step.velocities; Position[] positions = step.positions; final Vec2 d[] = pool.getVec2Array(bodies.length); for (int i = 0; i < bodies.length; ++i) { final int prev = (i == 0) ? bodies.length - 1 : i - 1; final int next = (i == bodies.length - 1) ? 0 : i + 1; d[i].set(positions[bodies[next].m_islandIndex].c); d[i].subLocal(positions[bodies[prev].m_islandIndex].c); dotMassSum += (d[i].lengthSquared()) / bodies[i].getMass(); crossMassSum += Vec2.cross(velocities[bodies[i].m_islandIndex].v, d[i]); } float lambda = -2.0f * crossMassSum / dotMassSum; // System.out.println(crossMassSum + " " +dotMassSum); // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection, // Settings.maxLinearCorrection); m_impulse += lambda; // System.out.println(m_impulse); for (int i = 0; i < bodies.length; ++i) { velocities[bodies[i].m_islandIndex].v.x += bodies[i].m_invMass * d[i].y * .5f * lambda; velocities[bodies[i].m_islandIndex].v.y += bodies[i].m_invMass * -d[i].x * .5f * lambda; } }
public float getJointTranslation() { Vec2 pA = pool.popVec2(), pB = pool.popVec2(), axis = pool.popVec2(); m_bodyA.getWorldPointToOut(m_localAnchorA, pA); m_bodyB.getWorldPointToOut(m_localAnchorB, pB); m_bodyA.getWorldVectorToOutUnsafe(m_localXAxisA, axis); pB.subLocal(pA); float translation = Vec2.dot(pB, axis); pool.pushVec2(3); return translation; }
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); }
Vec2 temp3 = pool.popVec2(); temp.set(m_localAnchorA).subLocal(bA.m_sweep.localCenter); Rot.mulToOutUnsafe(bA.m_xf.q, temp, rA); temp.set(m_localAnchorB).subLocal(bB.m_sweep.localCenter); Rot.mulToOutUnsafe(bB.m_xf.q, temp, rB); p2.set(bB.m_sweep.c).addLocal(rB); d.set(p2).subLocal(p1); Rot.mulToOutUnsafe(bA.m_xf.q, m_localXAxisA, axis); Vec2.crossToOutUnsafe(wA, rA, temp3); temp2.addLocal(vB).subLocal(vA).subLocal(temp3); float speed = Vec2.dot(d, temp) + Vec2.dot(axis, temp2);
public float getJointTranslation() { Body b1 = m_bodyA; Body b2 = m_bodyB; Vec2 p1 = pool.popVec2(); Vec2 p2 = pool.popVec2(); Vec2 axis = pool.popVec2(); b1.getWorldPointToOut(m_localAnchorA, p1); b2.getWorldPointToOut(m_localAnchorA, p2); p2.subLocal(p1); b1.getWorldVectorToOut(m_localXAxisA, axis); float translation = Vec2.dot(p2, axis); pool.pushVec2(3); return translation; }
Vec2.crossToOutUnsafe(wB, m_rB, vpB); vpB.addLocal(vB); float Cdot = Vec2.dot(m_u, vpB.subLocal(vpA));
public final void getSearchDirection(final Vec2 out) { switch (m_count) { case 1: out.set(m_v1.w).negateLocal(); return; case 2: e12.set(m_v2.w).subLocal(m_v1.w); // use out for a temp variable real quick out.set(m_v1.w).negateLocal(); float sgn = Vec2.cross(e12, out); if (sgn > 0f) { // Origin is left of e12. Vec2.crossToOutUnsafe(1f, e12, out); return; } else { // Origin is right of e12. Vec2.crossToOutUnsafe(e12, 1f, out); 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]); } } } }
m_impulse.mulLocal(maxImpulse / m_impulse.length()); impulse.set(m_impulse).subLocal(oldImpulse);