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;
}
