bubbleKinect2.pde

import org.openkinect.freenect.*;
import org.openkinect.processing.*;


Kinect2 kinect2;
PImage video, video1;
PImage rgb, secondImage;
float minThresh = 550;
float maxThresh =  1020;


Bubble[] bubbles = new Bubble[0];
int randPosX;  
int randPosY;
int  minR = 20; 
int  maxR = 50;
int [] radioses = new int[0];
int counter = 0;
int maxNumOfBubbles = 0;

boolean move = false;
int preXL =-1; 
int preXR =-1; 
int preY =-1;
final int tX = 120;
final int tY = 60;

int A, R, G, B;

void setup() {
  size(512, 424, P2D);
  //fullScreen(P2D,2);
  kinect2 = new Kinect2(this);
  kinect2.initDepth();
  kinect2.initRegistered();
  kinect2.initDevice();
  secondImage = loadImage("http://www.planetware.com/photos-large/USNY/new-york-niagara-falls-state-park.jpg");
  secondImage.resize(width, height);
  secondImage.loadPixels();
}

void draw() {

  background(0);
  rgb = kinect2.getVideoImage();

  //video = kinect2.getRegisteredImage();
  video = kinect2.getVideoImage();

  int [] depth = kinect2.getRawDepth();
  loadPixels();
  video.loadPixels();

  if (!move) {
    bubbles = new Bubble[0]; // renew(clear) the bubbles
    counter = 0;
  }

  int corXL = kinect2.depthWidth; 
  int corXR = 0; 
  int corY = kinect2.depthHeight;
  int resolution = 30; //15 ~ 20 is good
  for (int x =0; x<kinect2.depthWidth; x++) {
    for (int y = 0; y < kinect2.depthHeight; y++) {
      if (x%resolution == 0 && y%resolution == 0 && y>20) {
        int index = x+y*kinect2.depthWidth;

        if (depth[index]<maxThresh && depth[index]>minThresh) {
          if (!move) {
            addBubble(x, y);
          }
          if (x<corXL) {
            corXL = x;
          }
          if (x>corXR) {
            corXR = x;
          }
          if (y<corY) {

            corY = y;
          }
        }
      }
      //PxPGetPixel(x, y, secondImage.pixels, width);                             // if we are further away than threshold then copy the pixel from the second image
      //PxPSetPixel(x, y, R, G, B, 255, pixels, width);
      PxPGetPixel(x, y, video.pixels, video.width);                             // if we are further away than threshold then copy the pixel from the second image
      PxPSetPixel(x, y, R, G, B, 255, pixels, kinect2.depthWidth);
    }
  }


  updatePixels();
  video.updatePixels();
  //image(video1, 0, 0); 

  pictureBackground ();

  if ((abs(corXL-preXL)>tX || abs(corY-preY)>tY || abs(corXR-preXR)>tX) && preXL!=-1) {
    move = true;
    println(abs(corXL-preXL), abs(corY-preY), abs(corXR-preXR));
  }


  preXL = corXL;
  preXR = corXR;
  preY = corY;

  boolean allDown = true;
  for (int i = 0; i<bubbles.length; i++) {
    bubbles[i].dropBubble();
    allDown &= !bubbles[i].moveBubble;


    bubbles[i].drawBubble();
  }

  if (allDown) {
    move = false;
  }
  fill(255);
  textSize(14);
  text(mouseX*10, 10, 64 );
  text(bubbles.length, 10, 64+25 );


  maxNumOfBubbles++;
  //println(maxNumOfBubbles);
}

void pictureBackground () {
  if (key == 'c') {
    save("back");
  }
}

//maybe should be depth and height

void addBubble(int x, int y) {
  int r;
  if (radioses.length<=bubbles.length) {
    r = (int)random(minR, maxR);
    radioses = (int[])append(radioses, r);
  } else {
    r = radioses[counter];
  }
  bubbles = (Bubble[])append(bubbles, new Bubble(x, y, r)); //create a new bubble and add to the array
  counter++;
}


class Bubble {
  int x0, y0, sRadius, d, speed;
  boolean moveBubble = false;
  Bubble(int _posX, int _posY, int _sRadius) {
    x0 = _posX;
    y0 = _posY;
    sRadius =  _sRadius;//(int)random(minR, maxR);
    d = 0;
    speed = (int)random(5, 7);
  }

  void drawBubble() {
    int num = floor(sRadius/sqrt(2));
    for (int i = x0-num; i<=x0+num; i++) {
      for (int j = y0-num; j<=y0+num; j++) {
        ///sphere projection values
        int pX, pY, qX, qY;
        //Write the point in a coordinate system centered at the center of the sphere (x0,y0,z0)
        pX = i-x0;
        pY = j-y0;


        // Compute the length of this vector
        int pNorm = (int)sqrt(sq(pX) +sq(pY));
        //Scale the vector so that it has length equal to the radius of the sphere

        // pNorm is the distance between the center and the pixel
        if (pNorm > num) continue;

        // make sure to consider the case of pNorm == 0.
        if (pNorm == 0) {
          qX = x0;
          qY = y0;
        } else {
          qX = (sRadius/pNorm)*pX+x0;
          qY = (sRadius/pNorm)*pY+y0;
        }
        //constrain the bubbles so they will be in the screen borders

        int borderWidth = kinect2.depthWidth;
        int borderHeight = kinect2.depthHeight;
        qX = constrain(qX, 0, borderWidth-1);
        qY = constrain(qY, 0, borderHeight-1);
        int newI = constrain(i, 0, borderWidth-1);
        int newJ = constrain(j, 0, borderHeight-1);


        PxPGetPixel(qX, qY, video.pixels, video.width);                             // if we are further away than threshold then copy the pixel from the second image
        //if (R<10 && G<10 && B<10) {
        //  PxPGetPixel(qX, qY, video1.pixels, video1.width);        //working with the registered - replacing all the black pixels (the artifacts) with the pixels from the RGB image

        //}
        PxPSetPixel (newI, newJ+d, R, G, B, 255, pixels, borderWidth); // set the RGB of our to screen
      }
    }
  }
  void dropBubble() { 
    if (move) {
      if (d+sRadius+y0<height) {
        d+=speed;
        moveBubble = true;
      } else {
        moveBubble = false;
      }
    }
  }
}





void PxPGetPixel(int x, int y, int[] pixelArray, int pixelsWidth) {

  // map
  int mappedWidth = kinect2.depthWidth*video.height / kinect2.depthHeight;
  int diffrence = (video.width - mappedWidth)/2;
  x = floor(map( x, 0, kinect2.depthWidth, diffrence, video.width-diffrence+120));//video.height will put it more on the person but distorts the image, mappedWidth will fix the image
  y = floor(map( y, 0, kinect2.depthHeight, 0, video.height));

  int thisPixel=pixelArray[x+y*pixelsWidth];     // getting the colors as an int from the pixels[]
  A = (thisPixel >> 24) & 0xFF;                  // we need to shift and mask to get each component alone
  R = (thisPixel >> 16) & 0xFF;                  // this is faster than calling red(), green() , blue()
  G = (thisPixel >> 8) & 0xFF;   
  B = thisPixel & 0xFF;
}


//our function for setting color components RGB into the pixels[] , we need to define the XY of where
// to set the pixel, the RGB values we want and the pixels[] array we want to use and it's width

void PxPSetPixel(int x, int y, int r, int g, int b, int a, int[] pixelArray, int pixelsWidth) {
  a =(a << 24);                       
  r = r << 16;                       // We are packing all 4 composents into one int
  g = g << 8;                        // so we need to shift them to their places
  color argb = a | r | g | b;        // binary "or" operation adds them all into one int
  pixelArray[x+y*pixelsWidth]= argb;    // finaly we set the int with te colors into the pixels[]
}