Kinect 5: Body Tracking + Processing

import controlP5.*;
import megamu.mesh.*;
import blobDetection.*;
import KinectPV2.*;
import fisica.*;

ControlP5 cp5;

FWorld world;

FPoly poly;

BlobDetection theBlobDetection;

KinectPV2 kinect;

boolean foundUsers = false;

int blob_size = 200; 
PGraphics pg;
boolean blob = true;

PImage img;

int oldx = 0;
int oldy = 0;

ArrayList outline = new ArrayList();
ArrayList deleteP = new ArrayList();
ArrayList circles = new ArrayList();
ArrayList ordered;
ArrayList usedpts;
ArrayList shdist;

boolean show_kinect;
boolean show_outline;
boolean show_polygons;

FBody hovered;

void setup() {
  size(1280, 960);
  //frameRate(30);
  
  cp5 = new ControlP5(this);
  cp5.addToggle("show_kinect")
     .setPosition(40,40)
     .setSize(20,20)
     .setValue(true)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
     ;
   
   cp5.addToggle("show_outline")
     .setPosition(40,80)
     .setSize(20,20)
     .setValue(true)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
     ;
     
    cp5.addToggle("show_polygons")
     .setPosition(40,120)
     .setSize(20,20)
     .setValue(true)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
     ;
  
  pg = createGraphics(width/8, height/8);
  
  kinect = new KinectPV2(this);

  kinect.enableBodyTrackImg(true);

  kinect.init();
  
  theBlobDetection = new BlobDetection(width/8, height/8);
  theBlobDetection.setPosDiscrimination(true);
  theBlobDetection.setThreshold(0.1f); 
  
  Fisica.init(this);

  world = new FWorld();
  world.setGravity(0, 400);
  world.setEdges(55);
}

void draw() {
  background(55);

  float sc = 4;
  pushMatrix();
  pg.beginDraw();
  pg.image(kinect.getBodyTrackImage(), 0, 0, width/8, height/8);
  pg.endDraw();
  if(show_kinect){
    image(pg,width-width/8,0);
  }
  popMatrix();
  
  outline = new ArrayList();
  
  theBlobDetection.computeBlobs(pg.pixels);
  drawBlobsAndEdges(false,show_outline);

  //get the body track combined with the depth information

  //obtain an ArrayList of the users currently being tracked
  ArrayList bodyTrackList = kinect.getBodyTrackUser();
  
  fill(255);
  stroke(0);
  textSize(12);
 //text(kinect.getNumOfUsers(), 50, 50);
  //text(bodyTrackList.size(), width-100, 140);
  //text(frameRate, width-100, 160);
  

  
  if (frameCount % 20 == 0) {
    float sz = random(20, 150);
    FCircle b = new FCircle(sz);
    b.setPosition(random(0+100, width-100), 50);
    b.setVelocity(0, 300);
    b.setRestitution(0.7);
    b.setDamping(0.01);
    b.setDensity(1);
    b.setNoStroke();
    b.setFill(random(255),random(255),random(255));
    circles.add(b);
    world.add(b);
  }
  
  if(circles.size() > 150){
    int ind = int(random(circles.size()-1));
     FBody hovered = circles.get(ind);
     world.remove(hovered);
     circles.remove(ind);
  }

  for(int i = 0; i < circles.size(); i++){
    FBody cc = circles.get(i);
    float xpos = cc.getX();
    float ypos = cc.getY();
    
    int loc = int(int(xpos/8) + int(ypos/8) * 160);
    fill(255,0,0);
    noStroke();
    float r = red(pg.pixels[loc]);
    if(xpos > width || xpos  < 0){
      world.remove(cc);
      circles.remove(i);
      r = 255;
    }
    if(ypos > height || ypos  < 0){
      world.remove(cc);
      circles.remove(i);
      r = 255;
    }
    if(r < 50){
      world.remove(cc);
      circles.remove(i);
    }
  }

  
  
for(int i = 0; i < deleteP.size(); i++){
      FBody hovered = deleteP.get(i);
      world.remove(hovered);
    }
    
  if(frameCount > 60 && outline.size()>0){
    ordered = new ArrayList();
    usedpts = new ArrayList();
    shdist = new ArrayList();
    for(int i = 0; i < outline.size(); i++){
        PVector fp =  outline.get(i);
        usedpts.add(fp);
    }
    
   if(kinect.getNumOfUsers() > 0){
     PVector closestp =  outline.get(0);
    ordered.add(closestp);
    usedpts.remove(0);
    
    stroke(255,0,0);
    strokeWeight(3);
     
    for(int i = 0; i < outline.size()-1; i++){
      int ind = 0;
      float dd = 2000;
      for(int j = 0; j < usedpts.size(); j++){
        PVector fp =  usedpts.get(j);
        if(PVector.dist(closestp,fp) < dd){
          dd = PVector.dist(closestp,fp);
          ind = j;
        }
      }
      closestp =  usedpts.get(ind);
      ordered.add(closestp);
      usedpts.remove(ind);
    }
     for(int i = 0; i < ordered.size()-ordered.size()%10;){
       float[][] points = new float[10][2];
       float minx = 2000;
       float maxx = 0;
       float miny = 2000;
       float maxy = 0;
       for(int j = 0; j < 10;j++){ 
        PVector fp =  ordered.get(i);
        points[j][0] = fp.x;
        if(fp.x < minx){minx = fp.x;}
        if(fp.x > maxx){maxx = fp.x;}
        points[j][1] = fp.y;
        if(fp.y < miny){miny = fp.y;}
        if(fp.y > maxy){maxy = fp.y;}
        i++;
       }
       float polyarea = (maxx-minx)*(maxy-miny);
       if(polyarea < 8000 && polyarea > 1500 && maxx-minx > 10 && maxy-miny > 10){
         Hull myHull = new Hull( points );
         MPolygon myRegion = myHull.getRegion();
         fill(255,0,0);
         //myRegion.draw(this);
         int[] extrema = myHull.getExtrema();
         
            poly = new FPoly();
            poly.setFill(55);
            if(show_polygons){
              poly.setFill(0);
            }
            poly.setDensity(0);
            poly.setRestitution(0.5);
            poly.setNoStroke();
            
            for(int j = 0; j < extrema.length; j++){
                poly.vertex(points[extrema[j]][0],points[extrema[j]][1]);
            }
            deleteP.add(poly);
            world.add(poly);
         }
       }
   }
   
  }

  world.draw();
  world.step();
  //world.remove(poly);


}


// ==================================================
// drawBlobsAndEdges()
// ==================================================
void drawBlobsAndEdges(boolean drawBlobs, boolean drawEdges)
{
  noFill();
  Blob b;
  EdgeVertex eA, eB;
  for (int n=0 ; n50) {
        b.setSize(b.getSize()*0.95);
    }
  } 

  if (!c.getBody2().isStatic()) {
    FCircle b = (FCircle)c.getBody2();
    if (b.getSize()>50) {
        b.setSize(b.getSize()*0.95);
    }
  }
}