@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; } }
float normSqrd = delta.lengthSquared(); if (normSqrd > Settings.maxLinearCorrection * Settings.maxLinearCorrection) { delta.mulLocal(Settings.maxLinearCorrection / MathUtils.sqrt(normSqrd));
if (c == 0.0f && v.lengthSquared() > r.lengthSquared()) { ie = j; edge.set(m_vertices[i2]).subLocal(m_vertices[i1]); assert (edge.lengthSquared() > Settings.EPSILON * Settings.EPSILON); Vec2.crossToOutUnsafe(edge, 1f, m_normals[i]); m_normals[i].normalize();
if (m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse) { m_linearImpulse.normalize(); m_linearImpulse.mulLocal(maxImpulse);
float distanceSqr1 = closestPoint.lengthSquared(); float distanceSqr2 = distanceSqr1; distanceSqr2 = closestPoint.lengthSquared(); if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON) {
Vec2 bp = b.getWorldCenter(); float bm = b.getMass(); float bI = b.getInertia() - bm * b.getLocalCenter().lengthSquared(); float invBm = bm > 0 ? 1 / bm : 0; float invBI = bI > 0 ? 1 / bI : 0;
m_impulse.addLocal(impulse); float maxImpulse = data.step.dt * m_maxForce; if (m_impulse.lengthSquared() > maxImpulse * maxImpulse) { m_impulse.mulLocal(maxImpulse / m_impulse.length());
if (m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse) { m_linearImpulse.normalize(); m_linearImpulse.mulLocal(maxImpulse);
@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; } }
@Override public void step(TestbedSettings settings) { // TODO Auto-generated method stub super.step(settings); // Traverse the contact results. Apply a force on shapes // that overlap the sensor. for (int i = 0; i < e_count; ++i) { if (m_touching[i].tf == false) { continue; } Body body = m_bodies[i]; Body ground = m_sensor.getBody(); CircleShape circle = (CircleShape) m_sensor.getShape(); Vec2 center = ground.getWorldPoint(circle.m_p); Vec2 position = body.getPosition(); Vec2 d = center.sub(position); if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON) { continue; } d.normalize(); Vec2 F = d.mulLocal(100f); body.applyForce(F, position); } }
float normSqrd = delta.lengthSquared(); if (normSqrd > Settings.maxLinearCorrection * Settings.maxLinearCorrection) { delta.mulLocal(Settings.maxLinearCorrection / MathUtils.sqrt(normSqrd));
if (c == 0.0f && v.lengthSquared() > r.lengthSquared()) { ie = j; edge.set(m_vertices[i2]).subLocal(m_vertices[i1]); assert (edge.lengthSquared() > Settings.EPSILON * Settings.EPSILON); Vec2.crossToOutUnsafe(edge, 1f, m_normals[i]); m_normals[i].normalize();
if (m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse) { m_linearImpulse.normalize(); m_linearImpulse.mulLocal(maxImpulse);
float distanceSqr1 = closestPoint.lengthSquared(); float distanceSqr2 = distanceSqr1; distanceSqr2 = closestPoint.lengthSquared(); if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON) {
Vec2 bp = b.getWorldCenter(); float bm = b.getMass(); float bI = b.getInertia() - bm * b.getLocalCenter().lengthSquared(); float invBm = bm > 0 ? 1 / bm : 0; float invBI = bI > 0 ? 1 / bI : 0;
m_impulse.addLocal(impulse); float maxImpulse = data.step.dt * m_maxForce; if (m_impulse.lengthSquared() > maxImpulse * maxImpulse) { m_impulse.mulLocal(maxImpulse / m_impulse.length());
if (m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse) { m_linearImpulse.normalize(); m_linearImpulse.mulLocal(maxImpulse);
@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; } }
@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; } }
@Override public void step(TestbedSettings settings) { // TODO Auto-generated method stub super.step(settings); // Traverse the contact results. Apply a force on shapes // that overlap the sensor. for (int i = 0; i < e_count; ++i) { if (m_touching[i].tf == false) { continue; } Body body = m_bodies[i]; Body ground = m_sensor.getBody(); CircleShape circle = (CircleShape) m_sensor.getShape(); Vec2 center = ground.getWorldPoint(circle.m_p); Vec2 position = body.getPosition(); Vec2 d = center.sub(position); if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON) { continue; } d.normalize(); Vec2 F = d.mulLocal(100f); body.applyForce(F, position); } }