int i_num = 40;
float f_r =0;
Unit[] ar_unit = new Unit[i_num];

void setup(){
  size(640,360);
  background(255);
  
  for(int i=0; i<i_num; i++){
    ar_unit[i] = new Unit();
  }
  for(int i=0; i<i_num; i++){
    ar_unit[i].setGroupArray(ar_unit);
  }
}

void loop(){
  f_r+=0.005;
  background(255,255,255);
  translate(width /2, height /2,0);
  //scale(0.8,0.8,0.8);
  rotateX(1.2);
  rotateZ(f_r);
  rectMode(CENTER_DIAMETER); 
  stroke(0);
  stroke(150,150,150);
  fill(240,240,240);
  rect(0,0, 400,400);
   for(int i=0; i<i_num; i++){
    ar_unit[i].calc();
  }
   for(int i=0; i<i_num; i++){
    ar_unit[i].update();
  }
}

class Unit{
  int i_groupID;
  float f_x, f_y, f_z;
  float f_v, f_rot, f_rotV;
  float f_vx, f_vy, f_vz;
  float f_okD = 80; 
  float f_ngD = 20;
  Unit[] ar_unit;
  
  Unit(){
    i_groupID = 0;
    initPos();
  }
  void initPos(){
    f_x = random(-200,200);
    f_y = random(-200,200);
    f_z = 0;
    
    f_v = random(0.5,1);
    f_rot = random(0,2);
  }
  
  void calc(){ 
    float nx =0;//平均位置用ベクトル
    float ny =0;//
    float rx =0;//平均向き用ベクトル
    float ry =0;
    float ax=0;//反対向きのベクトル
    float ay=0;
    float ndist =100000;
    float rdist;
    float dx, dy, dist;
    float rot =0;
    float sb_num =0;//視界内にいる個体の数
    int imax = ar_unit.length;
    Unit u;
        
    for(int i=0; i<imax; i++){
      u = ar_unit[i];
      dx = u.f_x - f_x;
      dy = u.f_y - f_y;
      dist = sqrt(sq(dx)+sq(dy));
      if (dist <0.1){ dist =0.1;}
      if (dist<f_okD){ //視界内のユニットのみ扱う
        sb_num ++;
        rx += cos(u.f_rot);
        ry += sin(u.f_rot);
        rdist = sqrt(sq(rx)+sq(ry));
        rx /= rdist;
        ry /= rdist;
        nx = nx + dx/dist;
        ny = ny + dy/dist;
        if (dist<f_ngD && dist < ndist && u.f_x != f_x){
          ax = dx/dist;
          ay = dy/dist;
          ndist = dist;
        }
      }
    }
    nx /= sb_num; //平均位置ベクトル
    ny /= sb_num;
    rx /= sb_num;
    ry /= sb_num;
    if (sb_num >1){
      float n = atan2(ny+ry*0.05 - ax*50,nx+rx*0.05 - ay*50);
      //float n = atan2(nx, ny);
      f_rot = f_rot + (n-f_rot)*0.1;
    }
    
    f_vx = cos(f_rot);
    f_vy = sin(f_rot);
  }
  
  void update(){
    f_x += f_vx* f_v;
    f_y += f_vy*f_v;
    
    if (f_x>200){
      f_x = -200;
    }
    if (f_y>200){
      f_y = -200;
    }
     if (f_x<-200){
      f_x = 200;
    }
    if (f_y<-200){
      f_y = 200;
    }
    push();
      translate(f_x, f_y,0);
      noFill();
      ellipseMode(CENTER_DIAMETER);
      stroke(255,150,150);
      ellipse(0, 0, f_okD, f_okD);
      stroke(150,150,255);
      ellipse(0, 0,f_ngD,f_ngD);
      stroke(0);
      line(0,0,f_vx*10, f_vy*10);
      translate(0,0,32);
      rotate(-atan2(f_vx,f_vy)); 
      stroke(0,0,0,80);
      fill(200,200,200);
      box(2,4,64);
    pop();
  }
  void setGroupArray(Unit[] u){
    ar_unit = u;
  }
}