private float getSolverArea(Position[] positions) { float area = 0.0f; for (int i = 0; i < bodies.Length; ++i) { int next = (i == bodies.Length - 1) ? 0 : i + 1; area += positions[bodies[i].m_islandIndex].c.x*positions[bodies[next].m_islandIndex].c.y - positions[bodies[next].m_islandIndex].c.x*positions[bodies[i].m_islandIndex].c.y; } area *= .5f; return area; }
public void init(int bodyCapacity, int contactCapacity, int jointCapacity, ContactListener listener) { // System.ref.println("Initializing Island"); m_bodyCapacity = bodyCapacity; m_contactCapacity = contactCapacity; m_jointCapacity = jointCapacity; m_bodyCount = 0; m_contactCount = 0; m_jointCount = 0; m_listener = listener; if (m_bodies == null || m_bodyCapacity > m_bodies.Length) { m_bodies = new Body[m_bodyCapacity]; } if (m_joints == null || m_jointCapacity > m_joints.Length) { m_joints = new Joint[m_jointCapacity]; } if (m_contacts == null || m_contactCapacity > m_contacts.Length) { m_contacts = new Contact[m_contactCapacity]; } // dynamic array if (m_velocities == null || m_bodyCapacity > m_velocities.Length) { Velocity[] old = m_velocities == null ? new Velocity[0] : m_velocities; m_velocities = new Velocity[m_bodyCapacity]; Array.Copy(old, 0, m_velocities, 0, old.Length); for (int i = old.Length; i < m_velocities.Length; i++) { m_velocities[i] = new Velocity(); } } // dynamic array if (m_positions == null || m_bodyCapacity > m_positions.Length) { Position[] old = m_positions == null ? new Position[0] : m_positions; m_positions = new Position[m_bodyCapacity]; Array.Copy(old, 0, m_positions, 0, old.Length); for (int i = old.Length; i < m_positions.Length; i++) { m_positions[i] = new Position(); } } }
private bool constrainEdges(Position[] positions) { float perimeter = 0.0f; for (int i = 0; i < bodies.Length; ++i) { int next = (i == bodies.Length - 1) ? 0 : i + 1; float dx = positions[bodies[next].m_islandIndex].c.x - positions[bodies[i].m_islandIndex].c.x; float dy = positions[bodies[next].m_islandIndex].c.y - positions[bodies[i].m_islandIndex].c.y; float dist = MathUtils.sqrt(dx*dx + dy*dy); if (dist < Settings.EPSILON) { dist = 1.0f; } normals[i].x = dy/dist; normals[i].y = -dx/dist; perimeter += dist; } Vec2 delta = pool.popVec2(); float deltaArea = targetVolume - getSolverArea(positions); float toExtrude = 0.5f*deltaArea/perimeter; // *relaxationFactor // float sumdeltax = 0.0f; bool done = true; for (int i = 0; i < bodies.Length; ++i) { int next = (i == bodies.Length - 1) ? 0 : i + 1; delta.set(toExtrude*(normals[i].x + normals[next].x), toExtrude *(normals[i].y + normals[next].y)); // sumdeltax += dx; float normSqrd = delta.lengthSquared(); if (normSqrd > Settings.maxLinearCorrection*Settings.maxLinearCorrection) { delta.mulLocal(Settings.maxLinearCorrection/MathUtils.sqrt(normSqrd)); } if (normSqrd > Settings.linearSlop*Settings.linearSlop) { done = false; } positions[bodies[next].m_islandIndex].c.x += delta.x; positions[bodies[next].m_islandIndex].c.y += delta.y; // bodies[next].m_linearVelocity.x += delta.x * step.inv_dt; // bodies[next].m_linearVelocity.y += delta.y * step.inv_dt; } pool.pushVec2(1); // System.ref.println(sumdeltax); return done; }