Point3D[] R = new Point3D[100];

Box bx;
int bxSize;

// for collision detection
int numCells;
int cellSize;

int[][][] numInEachCell;
Point3DCollection[][][] cells;

void setup()
{
  size(500,500);
  background(255);
  framerate(30);
  
  stroke(0);
      bxSize = 110;
    for(int t=0; t < R.length; t++){
         float theta = (PI * random(500)/500 );
          float phi = (TWO_PI * random(500)/500);
          float magnitude = random(2)+2;
          R[t] = new Point3D(bxSize - random(bxSize*2), bxSize-random(bxSize*2),bxSize-random(bxSize*2),0,0,0);
          R[t].setPolarV(theta,phi,magnitude);
    }
   numCells=4;
   bx = new Box(bxSize); // bxSize is halfwidth
   cellSize = bxSize*2/numCells;
         
   translate(width/2,height/2);
   rotateX(-PI);
   rotateY(-PI);
}


void loop(){
  background(255);

  mouseSpin();
  stroke(204);
  //drawAxes(20,50);
 
  stroke(0);
  
noSmooth();
  
  // let's hope this reinitialises
   numInEachCell = new int[numCells][numCells][numCells];
   cells = new Point3DCollection[numCells][numCells][numCells];
   for(int x=0; x < numCells; x++){
     for(int y=0; y < numCells; y++){
      for(int z=0; z < numCells; z++){
          numInEachCell[x][y][z]=0;
          cells[x][y][z] = new Point3DCollection();
       } 
     }
   }
   
  /*for(int t=0; t < R.length; t++){
          R[t].collided=false;
    }*/
  for(int t=0; t < R.length; t++){
        
          R[t].increment();
          
          R[t].testOutOfBounds(bxSize);
          R[t].draw(); 
    }
    
    // test for any collisions
   for(int x=0; x < numCells; x++){
     for(int y=0; y < numCells; y++){
      for(int z=0; z < numCells; z++){
            int numtotest = numInEachCell[x][y][z];
            Point3DCollection pList = cells[x][y][z];
            // need to test each point against all other uncollided ones in the list
            for(int l=0; l < numtotest; l++){
                Point3D p = pList.get(l);
                
                for(int n=0; n < numtotest; n++){
                    if(n == l){ // don't test against itself
                        break;
                     }
                      Point3D p2 = pList.get(n);
                      if(p2.collided==true){ // don't test if we know it's been collided
                        break;
                      }

                      if(testCollision(p,p2) == true){
                          p.collided=true;
                          p2.collided=true;
                          collide(p,p2);
                      }
                }
                
            }
          
       } 
     }
   }
 
  smooth();
  stroke(0);
  bx.draw();
  
  
}

boolean testCollision(Point3D a, Point3D b){
  int pr = 5;
  if(a.position.x > b.position.x - pr && a.position.x < b.position.x + pr){
    if(a.position.y > b.position.y - pr && a.position.y < b.position.y +pr){
      if(a.position.z> b.position.z - pr && a.position.z < b.position.z + pr){
        return true;
      }else{ return false; }
    }else{ return false; }
    }else{ return false; }
}

void collide(Point3D p,Point3D p2){
  if(p.weight < p2.weight){
      p.velocity = p2.velocity;
      p.position = p2.position;
      p2.weight++;
  }else{
    p2.velocity = p.velocity;
      p2.position = p.position;
      p.weight++;
  }

}

void mouseSpin(){
   
    translate(width/2,height/2);
    rotateX(-TWO_PI*(height - mouseY)/height);
    rotateY(-TWO_PI*(width - mouseX)/width);

}

void drawAxes(int interval, int maxnum){
  
   for(int f=-maxnum; f<maxnum;f++){
    int ch=f*interval;
    point(ch,0,0);
    point(0,ch,0);
    point(0,0,ch);

  }
}


class pxCol{
  int r,g,b;
  boolean on;
  pxCol(int _r, int _g, int _b){
      r = _r;
      g = _g;
      b = _b;
      on=false;
  }
  
  void turnOn(){
    on = true;
  }
}

class Point3D{
  Vector position, velocity;
  pxCol[][][] structure;
  boolean collided;
  int weight;
  Point3D(float x, float y, float z, float vx, float vy, float vz)
  {
    position = new Vector(x, y, z);
    velocity = new Vector(vx, vy, vz);
    collided=false;
    weight=0;
    structure = new pxCol[3][3][3];
    int redc=int(random(20));
    int greenc = int(random(200));
    int bluec = int(random(255));
    for(int a=0; a<3; a++){
      for(int b=0; b<3; b++){
        for(int c=0;c<3;c++){
          
           structure[a][b][c]=new pxCol(redc,greenc,bluec); 
            int active = int(random(3));
            if(active==1){
                structure[a][b][c].turnOn();
            }
        }
      }
    }
  }

  void setPolar(float rho, float theta, float magnitude,
                float vrho, float vtheta, float vmagnitude){
         position.setPolar(rho, theta, magnitude);                       
        velocity.setPolar(vrho, vtheta, vmagnitude); 
  
  }
  
  void setPolarV(float vrho, float vtheta, float vmagnitude){
    velocity.setPolar(vrho, vtheta, vmagnitude); 
     
  
  }
  
  float  magnitudeV(){
    return velocity.magnitude();
  }
    void draw(){   
    
  for(int a=0; a<3; a++){
      for(int b=0; b<3; b++){
        for(int c=0;c<3;c++){
           if(structure[a][b][c].on){
             stroke(structure[a][b][c].r, structure[a][b][c].g,structure[a][b][c].b);
           point(position.x-3 + a*2, position.y-3+b*2,position.z-3+c*2);
           }
           
        }
      }
    }
    
    }
   //void draw(){ line(position.x, position.y, position.z, position.x+2,position.y,position.z+2);  }  
   
  void increment(){ 
  
    position = addvectors(position,velocity); 
    collided=false;
    allocateCell();
  }
  
  void testOutOfBounds(float b){
      if(position.x >= b || position.x <= -b){
          velocity.x = - velocity.x;
          if(position.x >= b){ position.x =b; }else{ position.x = -b; } 
          position = addvectors(position,velocity);
          // println("gone over x");
      }
      if(position.y >= b || position.y <= -b){
          velocity.y = - velocity.y;
          if(position.y >= b){ position.y =b; }else{ position.y = -b; }
          position = addvectors(position,velocity);
             position = addvectors(position,velocity);       
          //println("gone over y");
      }
      
      if(position.z >= b || position.z <= -b){
          velocity.z = - velocity.z;
          if(position.z >= b){ position.z =b; }else{ position.z = -b; }
          position = addvectors(position,velocity);
         //println("gone over z");
      }
    }
  

  
  
  
  void allocateCell(){
    int x = int(position.x);
    int y = int(position.y);
    int z = int(position.z);
    if(x < - bxSize){ x = -bxSize; }
    if(y < - bxSize){ y = -bxSize; }
    if(z < -bxSize){ z = -bxSize; }
    if(x >= bxSize){ x = bxSize-1; }
    if(y >= bxSize){ y = bxSize-1; }
    if(z >= bxSize){ z = bxSize-1; }
            
    int xcell = int( floor((x + bxSize) / cellSize) );
    int ycell = int( floor((y + bxSize) / cellSize ));
    int zcell = int( floor((z + bxSize) / cellSize ));
    //println("x: " +position.x+ " cell: " + xcell);
    //println("y: " + ycell);
    //println("z: " + zcell);
    
        cells[xcell][ycell][zcell].addPoint(this, xcell,ycell,zcell);  
  }
  
}

Vector addvectors(Vector a, Vector b){
    a.x = a.x + b.x;
    a.y = a.y + b.y;
    a.z = a.z + b.z;
    
    return a;
}

class Vector{
  float x, y, z;
  Vector(float ix, float iy, float iz)
  {
    x = (float)ix;
    y = (float)iy;
    z = (float)iz;
  }
  
  float magnitude(){
    return sqrt( (x * x) + (y * y) + (z * z));
  }
  
  void setPolar(float vrho, float vtheta, float vmagnitude){
    x = vmagnitude * sin(vtheta) * cos(vrho);
    y = vmagnitude * sin(vtheta) * sin(vrho);
    z = vmagnitude * cos(vtheta); 
  
  }
  
  float getTheta(){
         float angle =  atan2( z, x);
        return angle;
  }
  
  float getRho(){
        float angle =  atan2(y , x);
        return angle;
  }
  
  
}


class Point3DCollection{
  Point3D[] list;
  Point3DCollection(){
    list = new Point3D[200];
  }
  
  void addPoint(Point3D p,int x, int y, int z){
    int index = numInEachCell[x][y][z]++;
    list[index]=p;
  }
  
  Point3D get(int v){
    return list[v];
  }
}

class Box{
  // wall defined by top left and bottom right position vectors
  float b;

   Box(float bs){
     b = bs;
   }
   
   void draw(){
    line(-b,-b,-b,
        b,-b,-b);
    line(b,-b,-b,
        b,b,-b);
    line(b,b,-b,
        -b,b,-b);
    line(-b,b,-b,
        -b,-b,-b);
        
    line(-b,-b,-b,
        -b,-b,b);
   line(-b,b,-b,
        -b,b,b);
    line(b,b,-b,
        b,b,b);
     line(b,-b,-b,
        b,-b,b);
    line(-b,-b,b,
        b,-b,b);
    line(-b,b,b,
        b,b,b);
    line(-b,b,b,
        -b,-b,b);
    line(b,b,b,
        b,-b,b);
  }
}