/*
   Simple crowd swarming simulation, with grid speed-up
   Recommended numbers: sz = 200
                        diameter = .05f
*/

import java.util.*;

public class Swarm3
{
// CONSTRUCTOR

   public Swarm3() {
      X = null;
      Y = null;
      G = null;
      diameter = 0;
      splitY = 0;
      sz = 0;
   }

// PUBLIC METHODS

   public boolean create(int sz, float diameter) {
      this.sz = sz;
      this.diameter = diameter;
      X = new float[sz];
      Y = new float[sz];
      G = new int[sz];
      for (int i = 0 ; i < sz ; i++) {
         X[i] = M_random() - .5f;
         Y[i] = M_random() - .5f;
      }
      return true;
   }

   public void setSplitX(boolean tf) { isSplitX = tf; }
   public void setSplitY(boolean tf) { isSplitY = tf; }

   public float getX(int i) { return X[i]; }
   public float getY(int i) { return Y[i]; }

   public void setSplitY(float y) { splitY = y; }

   public void setLineUp(boolean tf) { lineUp = tf; }
   public boolean getLineUp() { return lineUp; }

   public int size() { return sz; }

   public void animate(float dt) {
      int g, I, J, I0, J0, I1, J1;

      // PUT EACH SWARM MEMBER INTO THE APPROPRIATE GRID BOX

      for (I = 0 ; I < NG ; I++)
      for (J = 0 ; J < NG ; J++)
         grid[I][J][0] = -1;

      for (int i = 0 ; i < sz ; i++) {
         I = (int)(NG * (X[i] + .5));
         J = (int)(NG * (Y[i] + .5));
	 I = I >= NG ? NG-1 : I < 0 ? 0 : I;
	 J = J >= NG ? NG-1 : J < 0 ? 0 : J;

	 int k = 0;
	 while (grid[I][J][k] != -1)
            k++;

	 grid[I][J][k  ] =  i;
	 grid[I][J][k+1] = -1;
         G[i] = I + J * NG;
      }

      // KEEP SWARM CENTERED ON SCREEN

      float centerX = 0;
      float centerY = 0;
      for (int i = 0 ; i < sz ; i++) {
         centerX += X[i];
         centerY += Y[i];
      }
      centerX /= sz;
      centerY /= sz;

      // FOR EACH MEMBER OF THE SWARM

      for (int i = 0 ; i < sz ; i++) {

         // PUSH AWAY FROM CENTER AISLE

         if (isSplitX && Y[i] < splitY)
            X[i] += dt * .005f / X[i] * (.4f - Y[i]*Y[i]);

         if (isSplitY)
            Y[i] += dt * .005f / Y[i] * (.4f - X[i]*X[i]);

         // PUSH TOWARD CENTER

         X[i] -= centerX + .07f * dt * X[i];
         Y[i] -= centerY + .07f * dt * Y[i];

         if (lineUp) {
            X[i] += .007f * Y[i];
            Y[i] -= .007f * X[i];
         }

         // TRY TO AVOID NEIGHBORS, WHILE ROILING AROUND INTERESTINGLY

         int count = 0;
         float weight = .2f * dt / sz;

         g = G[i];
         I = g % NG;
         J = g / NG;
         I0 = I-1 >= 0 ? I-1 : I;
         J0 = J-1 >= 0 ? J-1 : J;
         I1 = I+1 < NG ? I+1 : I;
         J1 = J+1 < NG ? J+1 : J;

         for (I = I0 ; I <= I1 ; I++)
         for (J = J0 ; J <= J1 ; J++)
         for (int k = 0 ; grid[I][J][k] != -1 ; k++) {
            int j = grid[I][J][k];

            if (j != i) {
               float dx = X[j] - X[i];
               float dy = Y[j] - Y[i];
               float dd = dx * dx + dy * dy;
               if (dd < .3f) {
                  X[i] -= weight * (dx + (lineUp ? 0 : 2.0f * (count++ % 29 < 12 ? dy : -dy)) / dd);
                  Y[i] -= weight * (dy + (lineUp ? 0 : 2.0f * (count++ % 37 <  8 ? dx : -dx)) / dd);
               }
            }
         }

         // HARD CONSTRAINT NOT TO INTERPENETRATE ANY NEIGHBORS

         float dXd = diameter * diameter;

         g = G[i];
         I = g % NG;
         J = g / NG;
         I0 = I-1 >= 0 ? I-1 : I;
         J0 = J-1 >= 0 ? J-1 : J;
         I1 = I+1 < NG ? I+1 : I;
         J1 = J+1 < NG ? J+1 : J;

         for (I = I0 ; I <= I1 ; I++)
         for (J = J0 ; J <= J1 ; J++)
         for (int k = 0 ; grid[I][J][k] != -1 ; k++) {
            int j = grid[I][J][k];

            if (j != i) {
               float dx = X[j] - X[i];
               float dy = Y[j] - Y[i];
               float dd = dx * dx + dy * dy;
               if (dd < dXd) {
                  float f = (diameter - M_sqrt(dd)) / diameter;
                  X[i] -= dx * f;
                  Y[i] -= dy * f;
               }
            }
         }
      }
   }

// PRIVATE METHODS

   private float M_random() { return (float)(Math.abs(R.nextDouble()) % 1); }
   private float M_sqrt(float t) { return (float)Math.sqrt(t); }

// PRIVATE DATA

   private float X[], Y[], diameter, splitY;
   private int G[];
   private int sz;
   private float BX0 = -.2f, BX1 = .2f, BY0 = 0, BY1 = .2f;
   private boolean isSplitX = false;
   private boolean isSplitY = false;
   private boolean lineUp = false;

   private int grid[][][] = new int[NG][NG][20];

   private Random R = new Random();

   private static final int NG = 20;
}