Box2D Forums

/**
* Liquid Flow
*/

#ifndef LIQUID_FLOW_H
#define LIQUID_FLOW_H

#define LIQUID_PARTICLES_MAX            1000
#define FLOAT_EPSILON                  1.1920928955078125E-7f

#include <vector>
#include <cmath>

class LiquidFlow : public Test
{
private:
   bool firstTime;

   int nParticles;
   float totalMass;
   float boxWidth;
   float boxHeight;

   float fluidMinX;
   float fluidMaxX;
   float fluidMinY;
   float fluidMaxY;

   b2Body* liquid[LIQUID_PARTICLES_MAX];
   b2Body* bod;

   float rad;
   float visc;// 0.005f;

   vector<int> hash[40][40];
   int hashWidth, hashHeight;

   float map(float val, float fromMin, float fromMax, float toMin,
         float toMax) {
      float mult = (val - fromMin) / (fromMax - fromMin);
      float res = toMin + mult * (toMax - toMin);
      return res;
   }

   float randomFloat(float argLow, float argHigh) {
      return (float) rand() / RAND_MAX * (argHigh - argLow) + argLow;
   }

   int hashX(float x) {
      float f = map(x, fluidMinX, fluidMaxX, 0, hashWidth - .001f);
      return (int) f;
   }

   int hashY(float y) {
      float f = map(y, fluidMinY, fluidMaxY, 0, hashHeight - .001f);
      return (int) f;
   }

   b2MassData buildMassData(float mass) {
      b2MassData md;
      md.mass = mass;
      md.center.SetZero();
      md.I = 1.0f;

      return md;
   }

   b2CircleShape getParticleShape() {
      b2CircleShape shape = b2CircleShape();
      shape.m_radius = .15f;
      return shape;
   }

   b2FixtureDef getParticleFixtureDef(b2Shape* shape) {
      b2FixtureDef fd = b2FixtureDef();
      fd.shape = shape;
      fd.density = 1;
      fd.filter.groupIndex = -10;
      fd.restitution = 0.4f;
      fd.friction = 0.0f;

      return fd;
   }

   void hashLocations() {
      for (int a = 0; a < hashWidth; a++) {
         for (int b = 0; b < hashHeight; b++) {
            hash[a][b].clear();
         }
      }

      b2Vec2 center;
      for (int a = 0; a < nParticles; a++) {
         center = liquid[a]->GetWorldCenter();
         int hcell = hashX(center.x);
         int vcell = hashY(center.y);
         if (hcell > -1 && hcell < hashWidth && vcell > -1
               && vcell < hashHeight)
            hash[hcell][vcell].push_back(a);
      }
   }

   void applyLiquidConstraint(float deltaT) {
      const float idealRad = 50.0f;
      float multiplier = idealRad / rad;

      vector<float> xchange(nParticles);
      vector<float> ychange(nParticles);
      vector<float> xs(nParticles);
      vector<float> ys(nParticles);
      vector<float> vxs(nParticles);
      vector<float> vys(nParticles);

      b2Body* particle;
      for (int i = 0; i < nParticles; ++i) {
         particle = liquid[i];
         xs[i] = multiplier * particle->GetWorldCenter().x;
         ys[i] = multiplier * particle->GetWorldCenter().y;
         vxs[i] = multiplier * particle->GetLinearVelocity().x;
         vys[i] = multiplier * particle->GetLinearVelocity().y;
      }

      float changex = 0.0f;
      float changey = 0.0f;
      
      for (int i = 0; i < nParticles; i++) {
         // Populate the neighbor list from the 9 proximate cells
         particle = liquid[i];

         int hcell = hashX(particle->GetWorldCenter().x);
         int vcell = hashY(particle->GetWorldCenter().y);

         vector<int> neighbors;

         for (int nx = -1; nx < 2; nx++) {
            for (int ny = -1; ny < 2; ny++) {
               int xc = hcell + nx;
               int yc = vcell + ny;

               if (xc > -1 && xc < hashWidth && yc > -1 && yc < hashHeight
                     && hash[xc][yc].size() > 0) {

                  for (int a = 0; a < hash[xc][yc].size(); a++) {
                     int ne = hash[xc][yc][a];
                     if (ne != i)
                        neighbors.push_back(ne);
                  }
               }
            }
         }

         // Particle pressure calculated by particle proximity
         // Pressures = 0 if all particles within range are idealRad
         // distance away
         int neighbourCount = neighbors.size();
         changex = 0.0f;
         changey = 0.0f;
         
         if (neighbourCount > 0) {
            vector<float> vlen(neighbourCount);
            float p = 0.0f;
            float pnear = 0.0f;
            for (int a = 0; a < neighbourCount; a++) {
               int j = neighbors[a];
               float vx = xs[j] - xs[i];
               float vy = ys[j] - ys[i];

               // early exit check
               if (vx > -idealRad && vx < idealRad && vy > -idealRad
                     && vy < idealRad) {
                  float vlensqr = (vx * vx + vy * vy);
                  // within idealRad check
                  if (vlensqr < idealRad * idealRad) {
                     vlen[a] = (float) sqrt(vlensqr);
                     if (vlen[a] < FLOAT_EPSILON)
                        vlen[a] = idealRad - .01f;
                     float oneminusq = 1.0f - (vlen[a] / idealRad);
                     p = (p + oneminusq * oneminusq);
                     pnear = (pnear + oneminusq * oneminusq * oneminusq);
                  } else {
                     vlen[a] = FLT_MAX;
                  }
               }
            }

            // Now actually apply the forces
            float pressure = (p - 5) / 2.0f;   // normal pressure term
            float presnear = pnear / 2.0f;      // near particles term
            for (int a = 0; a < neighbourCount; a++) {
               int j = neighbors[a];
               float vx = xs[j] - xs[i];
               float vy = ys[j] - ys[i];
               if (vx > -idealRad && vx < idealRad && vy > -idealRad
                     && vy < idealRad) {
                  if (vlen[a] < idealRad) {
                     float q = vlen[a] / idealRad;
                     float oneminusq = 1.0f - q;
                     float factor = oneminusq
                           * (pressure + presnear * oneminusq)
                           / (2.0f * vlen[a]);
                     float dx = vx * factor;
                     float dy = vy * factor;
                     float relvx = vxs[j] - vxs[i];
                     float relvy = vys[j] - vys[i];
                     factor = visc * oneminusq * deltaT;
                     dx -= relvx * factor;
                     dy -= relvy * factor;
                     xchange[j] += dx;
                     ychange[j] += dy;
                     changex -= dx;
                     changey -= dy;
                  }
               }
            }
         }

         xchange[i] += changex;
         ychange[i] += changey;
      }
      
      for (int i = 0; i < nParticles; ++i) {
         particle = liquid[i];
         b2Vec2 vel = particle->GetLinearVelocity();
         vel += b2Vec2(xchange[i] / (multiplier * deltaT),
            ychange[i] / (multiplier * deltaT));

         particle->SetLinearVelocity(vel);
      }
   }

   void dampenLiquid() {
      for (int i = 0; i < nParticles; ++i) {
         b2Vec2 vel = liquid[i]->GetLinearVelocity();
         vel *= 0.995f;

         liquid[i]->SetLinearVelocity(vel);
      }
   }

   void checkBounds() {
      float massPerParticle = totalMass / nParticles;

      b2CircleShape pd = getParticleShape();
      b2FixtureDef fd = getParticleFixtureDef(&pd);

      float cx = 0.0f;
      float cy = 25.0f;
      for (int i = 0; i < nParticles; ++i) {
         if (liquid[i]->GetWorldCenter().y < -10.0f) {
            m_world->DestroyBody(liquid[i]);

            b2BodyDef bd = b2BodyDef();
            bd.position = b2Vec2(randomFloat(cx - boxWidth * .5f,
                  cx + boxWidth * .5f), randomFloat(cy - boxHeight
                  * .5f, cy + boxHeight * .5f));
            bd.fixedRotation = true;
            bd.type = b2_dynamicBody;

            b2Body* b = m_world->CreateBody(&bd);

            b->CreateFixture(&fd);

            b2MassData md = buildMassData(massPerParticle);
            b->SetMassData(&md);
            b->SetSleepingAllowed(false);
            liquid[i] = b;
         }
      }

      // some box
      if (bod->GetWorldCenter().y < -15.0f) {
         m_world->DestroyBody(bod);

         b2PolygonShape polyDef;
         polyDef.SetAsBox(randomFloat(0.3f, 0.7f), randomFloat(0.3f, 0.7f));
         b2BodyDef bodyDef;
         bodyDef.position = b2Vec2(0.0f, 25.0f);
         bodyDef.type = b2_dynamicBody;
         bod = m_world->CreateBody(&bodyDef);
         bod->CreateFixture(&polyDef, 1);
      }
   }

public:
   void initParameters() {
      firstTime = TRUE;

      nParticles = LIQUID_PARTICLES_MAX;
      totalMass = 10.0f;
      boxWidth = 2.0f;
      boxHeight = 20.0f;

      fluidMinX = -11.0f;
      fluidMaxX = 5.0f;
      fluidMinY = -10.0f;
      fluidMaxY = 10.0f;

      rad = 0.6f;
      visc = 0.004f;

      hashWidth = 40;
      hashHeight = 40;
   }

   void initWorld() {
      if (firstTime) {
         // setCamera(new Vec2(0, 2), 35f);
         firstTime = false;
      }

        b2Body* ground = NULL;
      {
         b2Vec2 p1(0.0f, 0.0f);
         b2BodyDef bd = b2BodyDef();
         bd.position = p1;
            ground = m_world->CreateBody(&bd);

         b2PolygonShape shape;
         shape.SetAsBox(5.0f, 0.5f);
         ground->CreateFixture(&shape, 0);

         shape.SetAsBox(1.0f, 0.2f, b2Vec2(0.0f, 4.0f), -0.2f);
         ground->CreateFixture(&shape, 0);
         shape.SetAsBox(1.5f, 0.2f, b2Vec2(-1.2f, 5.2f), -1.5f);
         ground->CreateFixture(&shape, 0);
         shape.SetAsBox(0.5f, 50.0f, b2Vec2(5.0f, 0.0f), 0.0f);
         ground->CreateFixture(&shape, 0);

         shape.SetAsBox(0.5f, 3.0f, b2Vec2(-8.0f, 0.0f), 0.0f);
         ground->CreateFixture(&shape, 0);

         shape.SetAsBox(2.0f, 0.1f, b2Vec2(-6.0f, -2.8f), 0.1f);
         ground->CreateFixture(&shape, 0);

         b2CircleShape cd = b2CircleShape();
         cd.m_radius = 0.5f;
         cd.m_p.Set(-0.5f, -4.0f);
         ground->CreateFixture(&cd, 0);
      }

      float massPerParticle = totalMass / nParticles;
      b2CircleShape pd = getParticleShape();
      b2FixtureDef fd = getParticleFixtureDef(&pd);

      float cx = 0.0f;
      float cy = 25.0f;
      for (int i = 0; i < nParticles; ++i) {
         b2BodyDef bd = b2BodyDef();
         bd.position = b2Vec2(randomFloat(cx - boxWidth * .5f,
               cx + boxWidth * .5f), randomFloat(cy - boxHeight * .5f,
               cy + boxHeight * .5f));
         bd.fixedRotation = true;
         bd.type = b2_dynamicBody;

         b2Body* b = m_world->CreateBody(&bd);

         b->CreateFixture(&fd);

         b2MassData md = buildMassData(massPerParticle);
         b->SetMassData(&md);
         b->SetSleepingAllowed(false);
         liquid[i] = b;
      }

      // some box
      b2PolygonShape polyDef;
      polyDef.SetAsBox(randomFloat(0.3f, 0.7f), randomFloat(0.3f, 0.7f));
      b2BodyDef bodyDef = b2BodyDef();
      bodyDef.position = b2Vec2(0.0f, 25.0f);
      bodyDef.type = b2_dynamicBody;
      bod = m_world->CreateBody(&bodyDef);
      bod->CreateFixture(&polyDef, 1);
   }

    LiquidFlow()
    {
      initParameters();
      initWorld();
    }

    void Step(Settings* settings)
    {
        Test::Step(settings);

      float hz = 60; // settings.getSetting(TestbedSettings.Hz).value;
      float dt = 1.0f / hz;

      b2Timer timer;
      float startTime = timer.GetMilliseconds();
      hashLocations();
      applyLiquidConstraint(dt);
      
      dampenLiquid();

      checkBounds();

      b2Profile profile = m_world->GetProfile();
      profile.step += timer.GetMilliseconds();
    }

    static Test* Create()
    {
        return new LiquidFlow;
    }
};

#endif

inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
{
   b2GrowableStack<int32, 256> stack;

   public final void query(TreeCallback callback, AABB aabb) {
      intStack.reset();

procedure Tb2DynamicTree.Query(callback: Tb2GenericCallBackWrapper; const _aabb: Tb2AABB);
var
   nodeId: Int32;
begin
   growable_stack.Reset;

/**
 * Liquid Flow by Daniel Murphy
 */

#ifndef LIQUID_FLOW_H
#define LIQUID_FLOW_H

#define LIQUID_PARTICLES_MAX            1000
#define FLOAT_EPSILON                  1.1920928955078125E-7f

#include <vector>
#include <cmath>

class LiquidFlow : public Test
{
private:
   bool firstTime;

   int nParticles;
   float totalMass;
   float boxWidth;
   float boxHeight;

   float fluidMinX;
   float fluidMaxX;
   float fluidMinY;
   float fluidMaxY;

   b2Body* liquid[LIQUID_PARTICLES_MAX];
   b2Body* bod;

   float rad;
   float visc;// 0.005f;

   vector<int> hash[40][40];
   int hashWidth, hashHeight;

   float map(float val, float fromMin, float fromMax, float toMin,
         float toMax) {
      float mult = (val - fromMin) / (fromMax - fromMin);
      float res = toMin + mult * (toMax - toMin);
      return res;
   }

   float randomFloat(float argLow, float argHigh) {
      return (float) rand() / RAND_MAX * (argHigh - argLow) + argLow;
   }

   int hashX(float x) {
      float f = map(x, fluidMinX, fluidMaxX, 0, hashWidth - .001f);
      return (int) f;
   }

   int hashY(float y) {
      float f = map(y, fluidMinY, fluidMaxY, 0, hashHeight - .001f);
      return (int) f;
   }

   b2MassData buildMassData(float mass) {
      b2MassData md;
      md.mass = mass;
      md.center.SetZero();
      md.I = 1.0f;

      return md;
   }

   b2CircleShape getParticleShape() {
      b2CircleShape shape = b2CircleShape();
      shape.m_radius = .15f;
      return shape;
   }

   b2FixtureDef getParticleFixtureDef(b2Shape* shape) {
      b2FixtureDef fd = b2FixtureDef();
      fd.shape = shape;
      fd.density = 1;
      fd.filter.groupIndex = -10;
      fd.restitution = 0.4f;
      fd.friction = 0.0f;

      return fd;
   }

   void hashLocations() {
      for (int a = 0; a < hashWidth; a++) {
         for (int b = 0; b < hashHeight; b++) {
            hash[a][b].clear();
         }
      }

      b2Vec2 center;
      for (int a = 0; a < nParticles; a++) {
         center = liquid[a]->GetWorldCenter();
         int hcell = hashX(center.x);
         int vcell = hashY(center.y);
         if (hcell > -1 && hcell < hashWidth && vcell > -1
               && vcell < hashHeight)
            hash[hcell][vcell].push_back(a);
      }
   }

   void applyLiquidConstraint(float deltaT) {
      const float idealRad = 50.0f;
      float multiplier = idealRad / rad;

      vector<float> xchange(nParticles);
      vector<float> ychange(nParticles);
      vector<float> xs(nParticles);
      vector<float> ys(nParticles);
      vector<float> vxs(nParticles);
      vector<float> vys(nParticles);

      b2Body* particle;
      for (int i = 0; i < nParticles; ++i) {
         particle = liquid[i];
         xs[i] = multiplier * particle->GetWorldCenter().x;
         ys[i] = multiplier * particle->GetWorldCenter().y;
         vxs[i] = multiplier * particle->GetLinearVelocity().x;
         vys[i] = multiplier * particle->GetLinearVelocity().y;
      }

      float changex = 0.0f;
      float changey = 0.0f;
      
      vector<int> neighbors;
      neighbors.reserve(100);

      vector<float> vlen;
      for (int i = 0; i < nParticles; i++, neighbors.clear()) {
         // Populate the neighbor list from the 9 proximate cells
         particle = liquid[i];

         int hcell = hashX(particle->GetWorldCenter().x);
         int vcell = hashY(particle->GetWorldCenter().y);

         for (int nx = -1; nx < 2; nx++) {
            for (int ny = -1; ny < 2; ny++) {
               int xc = hcell + nx;
               int yc = vcell + ny;

               if (xc > -1 && xc < hashWidth && yc > -1 && yc < hashHeight
                     && hash[xc][yc].size() > 0) {

                  for (int a = 0; a < hash[xc][yc].size(); a++) {
                     int ne = hash[xc][yc][a];
                     if (ne != i)
                        neighbors.push_back(ne);
                  }
               }
            }
         }

         // Particle pressure calculated by particle proximity
         // Pressures = 0 if all particles within range are idealRad
         // distance away
         int neighbourCount = neighbors.size();
         changex = 0.0f;
         changey = 0.0f;
         
         if (neighbourCount > 0) {
            vlen.resize(neighbourCount);
            float p = 0.0f;
            float pnear = 0.0f;
            for (int a = 0; a < neighbourCount; a++) {
               int j = neighbors[a];
               float vx = xs[j] - xs[i];
               float vy = ys[j] - ys[i];

               // early exit check
               if (vx > -idealRad && vx < idealRad && vy > -idealRad
                     && vy < idealRad) {
                  float vlensqr = (vx * vx + vy * vy);
                  // within idealRad check
                  if (vlensqr < idealRad * idealRad) {
                     vlen[a] = (float) sqrt(vlensqr);
                     if (vlen[a] < FLOAT_EPSILON)
                        vlen[a] = idealRad - .01f;
                     float oneminusq = 1.0f - (vlen[a] / idealRad);
                     p = (p + oneminusq * oneminusq);
                     pnear = (pnear + oneminusq * oneminusq * oneminusq);
                  } else {
                     vlen[a] = FLT_MAX;
                  }
               }
            }

            // Now actually apply the forces
            float pressure = (p - 5) / 2.0f;   // normal pressure term
            float presnear = pnear / 2.0f;      // near particles term
            for (int a = 0; a < neighbourCount; a++) {
               int j = neighbors[a];
               float vx = xs[j] - xs[i];
               float vy = ys[j] - ys[i];
               if (vx > -idealRad && vx < idealRad && vy > -idealRad
                     && vy < idealRad) {
                  if (vlen[a] < idealRad) {
                     float q = vlen[a] / idealRad;
                     float oneminusq = 1.0f - q;
                     float factor = oneminusq
                           * (pressure + presnear * oneminusq)
                           / (2.0f * vlen[a]);
                     float dx = vx * factor;
                     float dy = vy * factor;
                     float relvx = vxs[j] - vxs[i];
                     float relvy = vys[j] - vys[i];
                     factor = visc * oneminusq * deltaT;
                     dx -= relvx * factor;
                     dy -= relvy * factor;
                     xchange[j] += dx;
                     ychange[j] += dy;
                     changex -= dx;
                     changey -= dy;
                  }
               }
            }

            vlen.clear();
         }

         xchange[i] += changex;
         ychange[i] += changey;
      }
      
      for (int i = 0; i < nParticles; ++i) {
         particle = liquid[i];
         b2Vec2 vel = particle->GetLinearVelocity();
         vel += b2Vec2(xchange[i] / (multiplier * deltaT),
            ychange[i] / (multiplier * deltaT));

         particle->SetLinearVelocity(vel);
      }
   }

   void dampenLiquid() {
      for (int i = 0; i < nParticles; ++i) {
         b2Vec2 vel = liquid[i]->GetLinearVelocity();
         vel *= 0.995f;

         liquid[i]->SetLinearVelocity(vel);
      }
   }

   void checkBounds() {
      float massPerParticle = totalMass / nParticles;

      b2CircleShape pd = getParticleShape();
      b2FixtureDef fd = getParticleFixtureDef(&pd);

      float cx = 0.0f;
      float cy = 25.0f;
      for (int i = 0; i < nParticles; ++i) {
         if (liquid[i]->GetWorldCenter().y < -10.0f) {
            m_world->DestroyBody(liquid[i]);

            b2BodyDef bd = b2BodyDef();
            bd.position = b2Vec2(randomFloat(cx - boxWidth * .5f,
                  cx + boxWidth * .5f), randomFloat(cy - boxHeight
                  * .5f, cy + boxHeight * .5f));
            bd.fixedRotation = true;
            bd.type = b2_dynamicBody;

            b2Body* b = m_world->CreateBody(&bd);

            b->CreateFixture(&fd);

            b2MassData md = buildMassData(massPerParticle);
            b->SetMassData(&md);
            b->SetSleepingAllowed(false);
            liquid[i] = b;
         }
      }

      // some box
      if (bod->GetWorldCenter().y < -15.0f) {
         m_world->DestroyBody(bod);

         b2PolygonShape polyDef;
         polyDef.SetAsBox(randomFloat(0.3f, 0.7f), randomFloat(0.3f, 0.7f));
         b2BodyDef bodyDef;
         bodyDef.position = b2Vec2(0.0f, 25.0f);
         bodyDef.type = b2_dynamicBody;
         bod = m_world->CreateBody(&bodyDef);
         bod->CreateFixture(&polyDef, 1);
      }
   }

public:
   void initParameters() {
      firstTime = TRUE;

      nParticles = LIQUID_PARTICLES_MAX;
      totalMass = 10.0f;
      boxWidth = 2.0f;
      boxHeight = 20.0f;

      fluidMinX = -11.0f;
      fluidMaxX = 5.0f;
      fluidMinY = -10.0f;
      fluidMaxY = 10.0f;

      rad = 0.6f;
      visc = 0.004f;

      hashWidth = 40;
      hashHeight = 40;
   }

   void initWorld() {
      if (firstTime) {
         // setCamera(new Vec2(0, 2), 35f);
         firstTime = false;
      }

        b2Body* ground = NULL;
      {
         b2Vec2 p1(0.0f, 0.0f);
         b2BodyDef bd = b2BodyDef();
         bd.position = p1;
            ground = m_world->CreateBody(&bd);

         b2PolygonShape shape;
         shape.SetAsBox(5.0f, 0.5f);
         ground->CreateFixture(&shape, 0);

         shape.SetAsBox(1.0f, 0.2f, b2Vec2(0.0f, 4.0f), -0.2f);
         ground->CreateFixture(&shape, 0);
         shape.SetAsBox(1.5f, 0.2f, b2Vec2(-1.2f, 5.2f), -1.5f);
         ground->CreateFixture(&shape, 0);
         shape.SetAsBox(0.5f, 50.0f, b2Vec2(5.0f, 0.0f), 0.0f);
         ground->CreateFixture(&shape, 0);

         shape.SetAsBox(0.5f, 3.0f, b2Vec2(-8.0f, 0.0f), 0.0f);
         ground->CreateFixture(&shape, 0);

         shape.SetAsBox(2.0f, 0.1f, b2Vec2(-6.0f, -2.8f), 0.1f);
         ground->CreateFixture(&shape, 0);

         b2CircleShape cd = b2CircleShape();
         cd.m_radius = 0.5f;
         cd.m_p.Set(-0.5f, -4.0f);
         ground->CreateFixture(&cd, 0);
      }

      float massPerParticle = totalMass / nParticles;
      b2CircleShape pd = getParticleShape();
      b2FixtureDef fd = getParticleFixtureDef(&pd);

      float cx = 0.0f;
      float cy = 25.0f;
      for (int i = 0; i < nParticles; ++i) {
         b2BodyDef bd = b2BodyDef();
         bd.position = b2Vec2(randomFloat(cx - boxWidth * .5f,
               cx + boxWidth * .5f), randomFloat(cy - boxHeight * .5f,
               cy + boxHeight * .5f));
         bd.fixedRotation = true;
         bd.type = b2_dynamicBody;

         b2Body* b = m_world->CreateBody(&bd);

         b->CreateFixture(&fd);

         b2MassData md = buildMassData(massPerParticle);
         b->SetMassData(&md);
         b->SetSleepingAllowed(false);
         liquid[i] = b;
      }

      // some box
      b2PolygonShape polyDef;
      polyDef.SetAsBox(randomFloat(0.3f, 0.7f), randomFloat(0.3f, 0.7f));
      b2BodyDef bodyDef = b2BodyDef();
      bodyDef.position = b2Vec2(0.0f, 25.0f);
      bodyDef.type = b2_dynamicBody;
      bod = m_world->CreateBody(&bodyDef);
      bod->CreateFixture(&polyDef, 1);
   }

    LiquidFlow()
    {
      initParameters();
      initWorld();
    }

    void Step(Settings* settings)
    {
       Test::Step(settings);

      float hz = 60; // settings.getSetting(TestbedSettings.Hz).value;
      float dt = 1.0f / hz;

      hashLocations();
      applyLiquidConstraint(dt);
      
      dampenLiquid();

      checkBounds();
    }

    static Test* Create()
    {
        return new LiquidFlow;
    }
};

#endif

/**
 * Liquid Flow by Daniel Murphy in jbox2d testbed, here's a ported copy
 */

#ifndef LIQUID_FLOW_H
#define LIQUID_FLOW_H

#define LIQUID_PARTICLES_MAX            1000
#define FLOAT_EPSILON                  1.1920928955078125E-7f

#include <cmath>

// optimized DynamicArray, useless anywhere else
template<class T>
class DynamicArray {
public:
   T data[LIQUID_PARTICLES_MAX];
   int size;

   void Clear() {
      size = 0;
   }

   void Add(int val) {
      data[size++] = val;
   }

   T Get(int index) {
      return data[index];
   }
};

class LiquidFlow : public Test
{
private:
   bool m_firstTime;

   int m_nParticles;
   float m_totalMass;
   float m_boxWidth;
   float m_boxHeight;

   float m_fluidMinX;
   float m_fluidMaxX;
   float m_fluidMinY;
   float m_fluidMaxY;

   b2Body* m_liquid[LIQUID_PARTICLES_MAX];
   b2Body* m_bod;

   float m_rad;
   float m_visc;// 0.005f;

   DynamicArray <int>m_hash[40][40];
   int m_hashWidth, m_hashHeight;

   // temp variants
   int m_neighbors[LIQUID_PARTICLES_MAX];
   float m_vlen[LIQUID_PARTICLES_MAX];
   int m_neighbourCount;

   float m_xchange[LIQUID_PARTICLES_MAX];
   float m_ychange[LIQUID_PARTICLES_MAX];
   float m_xs[LIQUID_PARTICLES_MAX];
   float m_ys[LIQUID_PARTICLES_MAX];
   float m_vxs[LIQUID_PARTICLES_MAX];
   float m_vys[LIQUID_PARTICLES_MAX];

   // common functions, might be useless in anywhere outside this unit
   float map(float val, float fromMin, float fromMax, float toMin,
         float toMax) {
      float mult = (val - fromMin) / (fromMax - fromMin);
      float res = toMin + mult * (toMax - toMin);
      return res;
   }

   float randomFloat(float argLow, float argHigh) {
      return (float) rand() / RAND_MAX * (argHigh - argLow) + argLow;
   }

   int hashX(float x) {
      float f = map(x, m_fluidMinX, m_fluidMaxX, 0, m_hashWidth - .001f);
      return (int) f;
   }

   int hashY(float y) {
      float f = map(y, m_fluidMinY, m_fluidMaxY, 0, m_hashHeight - .001f);
      return (int) f;
   }

   b2MassData buildMassData(float mass) {
      b2MassData md;
      md.mass = mass;
      md.center.SetZero();
      md.I = 1.0f;

      return md;
   }

   b2CircleShape getParticleShape() {
      b2CircleShape shape = b2CircleShape();
      shape.m_radius = .15f;
      return shape;
   }

   b2FixtureDef getParticleFixtureDef(b2Shape* shape) {
      b2FixtureDef fd = b2FixtureDef();
      fd.shape = shape;
      fd.density = 1;
      fd.filter.groupIndex = -10;
      fd.restitution = 0.4f;
      fd.friction = 0.0f;

      return fd;
   }

   void hashLocations() {
      for (int a = 0; a < m_hashWidth; a++) {
         for (int b = 0; b < m_hashHeight; b++) {
            m_hash[a][b].Clear();
         }
      }

      b2Vec2 center;
      int hcell, vcell;
      for (int a = 0; a < m_nParticles; a++) {
         center = m_liquid[a]->GetWorldCenter();
         hcell = hashX(center.x);
         vcell = hashY(center.y);

         if (hcell > -1 && hcell < m_hashWidth && vcell > -1
               && vcell < m_hashHeight)
            m_hash[hcell][vcell].Add(a);
      }
   }

   void applyLiquidConstraint(float deltaT) {
      const float idealRad = 50.0f;
      float multiplier = idealRad / m_rad;

      b2Body* particle;
      for (int i = 0; i < m_nParticles; ++i) {
         particle = m_liquid[i];
         m_xs[i] = multiplier * particle->GetWorldCenter().x;
         m_ys[i] = multiplier * particle->GetWorldCenter().y;
         m_vxs[i] = multiplier * particle->GetLinearVelocity().x;
         m_vys[i] = multiplier * particle->GetLinearVelocity().y;
         m_xchange[i] = m_ychange[i] = 0;
      }

      float changex = 0.0f;
      float changey = 0.0f;
      
      for (int i = 0; i < m_nParticles; i++) {
         // Populate the neighbor list from the 9 proximate cells
         particle = m_liquid[i];

         m_neighbourCount = 0;

         int hcell = hashX(particle->GetWorldCenter().x);
         int vcell = hashY(particle->GetWorldCenter().y);

         for (int nx = -1; nx < 2; nx++) {
            for (int ny = -1; ny < 2; ny++) {
               int xc = hcell + nx;
               int yc = vcell + ny;

               if (xc > -1 && xc < m_hashWidth && yc > -1 && yc < m_hashHeight
                     && m_hash[xc][yc].size > 0) {

                  for (int a = 0; a < m_hash[xc][yc].size; a++) {
                     int ne = m_hash[xc][yc].Get(a);
                     if (ne != i)
                        m_neighbors[m_neighbourCount++] = ne;
                  }
               }
            }
         }

         // Particle pressure calculated by particle proximity
         // Pressures = 0 if all particles within range are idealRad
         // distance away
         // int m_neighbourCount = m_neighbors.size();
         changex = 0.0f;
         changey = 0.0f;
         
         if (m_neighbourCount > 0) {
            float p = 0.0f;
            float pnear = 0.0f;
            for (int a = 0; a < m_neighbourCount; a++) {
               int j = m_neighbors[a];
               float vx = m_xs[j] - m_xs[i];
               float vy = m_ys[j] - m_ys[i];

               // early exit check
               if (vx > -idealRad && vx < idealRad && vy > -idealRad
                     && vy < idealRad) {
                  float vlensqr = (vx * vx + vy * vy);
                  // within idealRad check
                  if (vlensqr < idealRad * idealRad) {
                     m_vlen[a] = (float) sqrt(vlensqr);
                     if (m_vlen[a] < FLOAT_EPSILON)
                        m_vlen[a] = idealRad - .01f;
                     float oneminusq = 1.0f - (m_vlen[a] / idealRad);
                     p = (p + oneminusq * oneminusq);
                     pnear = (pnear + oneminusq * oneminusq * oneminusq);
                  } else {
                     m_vlen[a] = FLT_MAX;
                  }
               }
            }

            // Now actually apply the forces
            float pressure = (p - 5) / 2.0f;   // normal pressure term
            float presnear = pnear / 2.0f;      // near particles term
            for (int a = 0; a < m_neighbourCount; a++) {
               int j = m_neighbors[a];
               float vx = m_xs[j] - m_xs[i];
               float vy = m_ys[j] - m_ys[i];
               if (vx > -idealRad && vx < idealRad && vy > -idealRad
                     && vy < idealRad) {
                  if (m_vlen[a] < idealRad) {
                     float q = m_vlen[a] / idealRad;
                     float oneminusq = 1.0f - q;
                     float factor = oneminusq
                           * (pressure + presnear * oneminusq)
                           / (2.0f * m_vlen[a]);
                     float dx = vx * factor;
                     float dy = vy * factor;
                     float relvx = m_vxs[j] - m_vxs[i];
                     float relvy = m_vys[j] - m_vys[i];
                     factor = m_visc * oneminusq * deltaT;
                     dx -= relvx * factor;
                     dy -= relvy * factor;
                     m_xchange[j] += dx;
                     m_ychange[j] += dy;
                     changex -= dx;
                     changey -= dy;
                  }
               }
            }
         }

         m_xchange[i] += changex;
         m_ychange[i] += changey;
      }
      
      for (int i = 0; i < m_nParticles; ++i) {
         particle = m_liquid[i];
         b2Vec2 vel = particle->GetLinearVelocity();
         vel += b2Vec2(m_xchange[i] / (multiplier * deltaT),
            m_ychange[i] / (multiplier * deltaT));

         particle->SetLinearVelocity(vel);
      }
   }

   void dampenLiquid() {
      for (int i = 0; i < m_nParticles; ++i) {
         b2Vec2 vel = m_liquid[i]->GetLinearVelocity();
         vel *= 0.995f;

         m_liquid[i]->SetLinearVelocity(vel);
      }
   }

   void checkBounds() {
      float massPerParticle = m_totalMass / m_nParticles;

      b2CircleShape pd = getParticleShape();
      b2FixtureDef fd = getParticleFixtureDef(&pd);

      float cx = 0.0f;
      float cy = 25.0f;
      for (int i = 0; i < m_nParticles; ++i) {
         if (m_liquid[i]->GetWorldCenter().y < -10.0f) {
            m_world->DestroyBody(m_liquid[i]);

            b2BodyDef bd = b2BodyDef();
            bd.position = b2Vec2(randomFloat(cx - m_boxWidth * .5f,
                  cx + m_boxWidth * .5f), randomFloat(cy - m_boxHeight
                  * .5f, cy + m_boxHeight * .5f));
            bd.fixedRotation = true;
            bd.type = b2_dynamicBody;

            b2Body* b = m_world->CreateBody(&bd);

            b->CreateFixture(&fd);

            b2MassData md = buildMassData(massPerParticle);
            b->SetMassData(&md);
            b->SetSleepingAllowed(false);
            m_liquid[i] = b;
         }
      }

      // some box
      if (m_bod->GetWorldCenter().y < -15.0f) {
         m_world->DestroyBody(m_bod);

         b2PolygonShape polyDef;
         polyDef.SetAsBox(randomFloat(0.3f, 0.7f), randomFloat(0.3f, 0.7f));
         b2BodyDef bodyDef;
         bodyDef.position = b2Vec2(0.0f, 25.0f);
         bodyDef.type = b2_dynamicBody;
         m_bod = m_world->CreateBody(&bodyDef);
         m_bod->CreateFixture(&polyDef, 1);
      }
   }

public:
   void initParameters() {
      m_firstTime = TRUE;

      m_nParticles = LIQUID_PARTICLES_MAX;
      m_totalMass = 10.0f;
      m_boxWidth = 2.0f;
      m_boxHeight = 20.0f;

      m_fluidMinX = -11.0f;
      m_fluidMaxX = 5.0f;
      m_fluidMinY = -10.0f;
      m_fluidMaxY = 10.0f;

      m_rad = 0.6f;
      m_visc = 0.004f;

      m_hashWidth = 40;
      m_hashHeight = 40;
   }

   void initWorld() {
      if (m_firstTime) {
         // setCamera(new Vec2(0, 2), 35f);
         m_firstTime = false;
      }

        b2Body* ground = NULL;
      {
         b2Vec2 p1(0.0f, 0.0f);
         b2BodyDef bd = b2BodyDef();
         bd.position = p1;
            ground = m_world->CreateBody(&bd);

         b2PolygonShape shape;
         shape.SetAsBox(5.0f, 0.5f);
         ground->CreateFixture(&shape, 0);

         shape.SetAsBox(1.0f, 0.2f, b2Vec2(0.0f, 4.0f), -0.2f);
         ground->CreateFixture(&shape, 0);
         shape.SetAsBox(1.5f, 0.2f, b2Vec2(-1.2f, 5.2f), -1.5f);
         ground->CreateFixture(&shape, 0);
         shape.SetAsBox(0.5f, 50.0f, b2Vec2(5.0f, 0.0f), 0.0f);
         ground->CreateFixture(&shape, 0);

         shape.SetAsBox(0.5f, 3.0f, b2Vec2(-8.0f, 0.0f), 0.0f);
         ground->CreateFixture(&shape, 0);

         shape.SetAsBox(2.0f, 0.1f, b2Vec2(-6.0f, -2.8f), 0.1f);
         ground->CreateFixture(&shape, 0);

         b2CircleShape cd = b2CircleShape();
         cd.m_radius = 0.5f;
         cd.m_p.Set(-0.5f, -4.0f);
         ground->CreateFixture(&cd, 0);
      }

      float massPerParticle = m_totalMass / m_nParticles;
      b2CircleShape pd = getParticleShape();
      b2FixtureDef fd = getParticleFixtureDef(&pd);

      float cx = 0.0f;
      float cy = 25.0f;
      for (int i = 0; i < m_nParticles; ++i) {
         b2BodyDef bd = b2BodyDef();
         bd.position = b2Vec2(randomFloat(cx - m_boxWidth * .5f,
               cx + m_boxWidth * .5f), randomFloat(cy - m_boxHeight * .5f,
               cy + m_boxHeight * .5f));
         bd.fixedRotation = true;
         bd.type = b2_dynamicBody;

         b2Body* b = m_world->CreateBody(&bd);

         b->CreateFixture(&fd);

         b2MassData md = buildMassData(massPerParticle);
         b->SetMassData(&md);
         b->SetSleepingAllowed(false);
         m_liquid[i] = b;
      }

      // some box
      b2PolygonShape polyDef;
      polyDef.SetAsBox(randomFloat(0.3f, 0.7f), randomFloat(0.3f, 0.7f));
      b2BodyDef bodyDef = b2BodyDef();
      bodyDef.position = b2Vec2(0.0f, 25.0f);
      bodyDef.type = b2_dynamicBody;
      m_bod = m_world->CreateBody(&bodyDef);
      m_bod->CreateFixture(&polyDef, 1);
   }

    LiquidFlow()
    {
      initParameters();
      initWorld();
    }

    void Step(Settings* settings)
    {
      Test::Step(settings);

      float hz = 60; // settings.getSetting(TestbedSettings.Hz).value;
      float dt = 1.0f / hz;

      b2Timer timer;
      hashLocations();
      m_stepTime0 = timer.GetMilliseconds();
      timer.Reset();

      applyLiquidConstraint(dt);
      
      dampenLiquid();

      checkBounds();
      m_stepTime1 = timer.GetMilliseconds();
    }

    static Test* Create()
    {
        return new LiquidFlow;
    }
};

#endif

// b2BroadPhase.h
void b2BroadPhase::UpdatePairs(T* callback)
{
...
   // Perform tree queries for all moving proxies.
   for (int32 i = 0; i < m_moveCount; ++i) {
             ...
             m_tree.Query(this, fatAABB);         // This cost [b]120ms[/b] in cocos2dx, but [b]20ms[/b] in Box2D project (when m_moveCount about 1000)
        }
...

    // And the sorting, 60ms in cocos2dx, when 20ms in Box2D project (when m_pairCount about [b]20000[/b] for cocos2dx, and [b]40000[/b] for Box2D), it seems amazing!

    // Sort the pair buffer to expose duplicates.
    std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan);