kinect_tree.pde

import SimpleOpenNI.*;
import processing.opengl.*;

SimpleOpenNI  kinect;

boolean running = false;
boolean devinMode = false;

boolean debug = true;

float angle = 20;
float treeSize = 100;
float divisor = 0.8;
float maxBranchSize = 0.04;
float maxRandomAngle = 30;
float maxLevel = 13;
color trunk = #8b4513;
color leaves = #666666 & 0x70FFFFFF;

float leftAngle = 42.0;
float rightAngle = 10.0;
float leftSize = 0.02;
float rightSize = 0.02;

void setup()
{
  size(640, 480, OPENGL);
  hint(ENABLE_OPENGL_4X_SMOOTH);
  hint(DISABLE_DEPTH_TEST);

  kinect = new SimpleOpenNI(this, SimpleOpenNI.RUN_MODE_MULTI_THREADED);
  kinect.enableDepth();

  kinect.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);
  if (debug) {
    PFont font = loadFont("Menlo-Bold-48.vlw"); 
    textFont(font);
    fill(153);
  }

  //  size(640, 480);
  stroke(0, 0, 0, 64);
  strokeCap(ROUND);
}

void draw()
{
  kinect.update();

  IntVector userList = new IntVector();
  kinect.getUsers(userList);

  if (userList.size() < 1)
    return;

  int user = userList.get(0);
  if (!kinect.isTrackingSkeleton(user))
    return;
  
  PVector lHand = new PVector();
  kinect.getJointPositionSkeleton(user, SimpleOpenNI.SKEL_LEFT_HAND, lHand);
  PVector rHand = new PVector();
  kinect.getJointPositionSkeleton(user, SimpleOpenNI.SKEL_RIGHT_HAND, rHand);
  
  PVector rShoulder = new PVector();
  kinect.getJointPositionSkeleton(user, SimpleOpenNI.SKEL_RIGHT_SHOULDER, rShoulder);
  PVector lShoulder = new PVector();
  kinect.getJointPositionSkeleton(user, SimpleOpenNI.SKEL_LEFT_SHOULDER, lShoulder);
  
  PVector torso = new PVector();
  kinect.getJointPositionSkeleton(user, SimpleOpenNI.SKEL_TORSO, torso);
  
  float lDist = lHand.dist(lShoulder);
  leftSize = maxBranchSize * (lDist - 100.0) / 500.0;
  
  float rDist = rHand.dist(rShoulder);
  rightSize = maxBranchSize * (rDist - 100.0) / 500.0;
  
  PVector lHVec = new PVector();
  kinect.getJointPositionSkeleton(user, SimpleOpenNI.SKEL_LEFT_HAND, lHVec);
  lHVec.sub(lShoulder);
  PVector lSVec = new PVector();
  kinect.getJointPositionSkeleton(user, SimpleOpenNI.SKEL_RIGHT_SHOULDER, lSVec);
  lSVec.sub(lShoulder);
  
  float lAngle = PVector.angleBetween(lHVec, lSVec);
  leftAngle = 90 - degrees(lAngle);
  

  PVector rHVec = new PVector();
  kinect.getJointPositionSkeleton(user, SimpleOpenNI.SKEL_RIGHT_HAND, rHVec);
  rHVec.sub(rShoulder);
  PVector rSVec = new PVector();
  kinect.getJointPositionSkeleton(user, SimpleOpenNI.SKEL_LEFT_SHOULDER, rSVec);
  rSVec.sub(rShoulder);

  float rAngle = PVector.angleBetween(rHVec, rSVec);
  rightAngle = 90 - degrees(rAngle);
  
  drawTree(width / 2, height);
  image(kinect.depthImage(), width - 160, height - 120, 160, 120);
  if (debug) {
//    text("l: " + round(leftAngle) + ", " + leftSize, 5, 48);
//    text("r: " + round(rightAngle) + ", " + rightSize, 5, 108);
    drawSkeleton(user);
  }
}

void drawSkeleton(int userId)
{
  // to get the 3d joint data
  /*
  PVector jointPos = new PVector();
   context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_NECK,jointPos);
   println(jointPos);
   */

  kinect.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

  kinect.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);

  kinect.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND);

  kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);

  kinect.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);

  kinect.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);
}

void keyPressed()
{   
  if (key == 'l') {
    // load Devin calibration file
    IntVector userList = new IntVector();
    kinect.getUsers(userList);
    if (userList.size() < 1)
    {
      println("You need at least one active user!");
      return;
    }

    int user = userList.get(0);

    if (kinect.loadCalibrationDataSkeleton(user, "devin.skel"))
    {
      kinect.startTrackingSkeleton(user);
      devinMode = true;
      println("Loaded calibration from file.");
    }
    else
      println("Can't load calibration file.");
  }
  if (key == 'r') {
    // reset
    devinMode = false;
    running = false;
  }
}

void onNewUser(int userId)
{
  if (devinMode)
    return;

  println("New user: " + userId);
  println("  start pose detection");

  kinect.startPoseDetection("Psi", userId);
}

void onLostUser(int userId)
{
  println("Lost user " + userId);
}

void onStartCalibration(int userId)
{
  println("Started calibration for user " + userId);
}

void onEndCalibration(int userId, boolean successfull)
{
  println("End calibration for user " + userId + ", successfull: " + successfull);

  if (successfull) 
  { 
    println("  User calibrated !!!");
    kinect.startTrackingSkeleton(userId);
  } 
  else 
  { 
    println("  Failed to calibrate user !!!");
    println("  Start pose detection");
    kinect.startPoseDetection("Psi", userId);
  }
}

void onStartPose(String pose, int userId)
{
  if (devinMode)
    return;

  println("Started pose for user " + userId + ", pose: " + pose);

  kinect.stopPoseDetection(userId); 
  kinect.requestCalibrationSkeleton(userId, true);
}

void onEndPose(String pose, int userId)
{
  println("End pose for user " + userId + ", pose: " + pose);
}

void drawTree(float x, float y)
{
  pushMatrix();
  pushStyle();
  background(255);
  translate(x, y);
  drawBranch(1, (leftSize + rightSize) / 2);
  popStyle();
  popMatrix();
}

void drawBranch(int level, float branchSize)
{
  if (level > maxLevel) return;

  branchSize = -treeSize * pow(divisor + branchSize, level);

  // draw the current branch: we're already rotated t
  strokeWeight(0.1 * -(branchSize));
//  stroke(lerpColor(trunk, leaves, level/maxLevel));
  line(0, 0, 0, branchSize);

  // move to the end of the branch
  translate(0, branchSize);

  //  float thisAngle = angle + random(maxRandomAngle * 2.) - maxRandomAngle;
  rotate(radians(-leftAngle));

  drawBranch(level + 1, leftSize);

  rotate(radians(leftAngle));

  //  thisAngle = angle + random(maxRandomAngle * 2.) - maxRandomAngle;

  rotate(radians(rightAngle));

  drawBranch(level + 1, rightSize);

  rotate(radians(-rightAngle));
  translate(0, -branchSize);
}