März 4, 2012

Processing

Main

import SimpleOpenNI.*;
Hashtable users = new Hashtable();

SimpleOpenNI context;
float        zoomF =0.3f;
float        rotX = radians(160);  // by default rotate the hole scene 180deg around the x-axis, 
                                   // the data from openni comes upside down
float        rotY = radians(12);
boolean      autoCalib=true;
float bgLimitDepth=3000;

Hashtable userColor = new Hashtable();
Hashtable userBalls = new Hashtable();
Hashtable userTarget = new Hashtable();
PVector boxForce = new PVector();
PVector lineDir = new PVector();
Hashtable lineForce = new Hashtable();
boolean recording=false;

void setup()
{
  size(1024,768,P3D);  // strange, get drawing error in the cameraFrustum if i use P3D, in opengl there is no problem
  context = new SimpleOpenNI(this);
  
  // play recording
  context.openFileRecording("limbpong_double.oni");
  
  // enable mirror
  context.setMirror(true);

  // enable depthMap generation 
  if(context.enableDepth() == false)
  {
     println("Can't open the depthMap, maybe the camera is not connected!"); 
     exit();
     return;
  }

  // enable skeleton generation for all joints
  context.enableUser(SimpleOpenNI.SKEL_PROFILE_UPPER);

  stroke(255,255,255);
  smooth();  
  perspective(radians(45),
              float(width)/float(height),
              10,150000);

  // setup the recording 
  if (recording==true) {
    context.enableRecorder(SimpleOpenNI.RECORD_MEDIUM_FILE, "limbpong_double.oni");

    // select the recording channels
    context.addNodeToRecording(SimpleOpenNI.NODE_DEPTH, 
    SimpleOpenNI.CODEC_16Z_EMB_TABLES);
    context.addNodeToRecording(SimpleOpenNI.NODE_IMAGE, 
    SimpleOpenNI.CODEC_JPEG);
  }
 }

void draw()
{
  // update the cam
  context.update();

  background(255);
  
  // set the scene pos
  translate(width/2, height/2, 0);
  rotateX(rotX);
  rotateY(rotY);
  scale(zoomF);
  
  PImage  rgbImage = context.rgbImage();
  int[]   depthMap = context.depthMap();
  int     steps   = 4;  // to speed up the drawing, draw every third point
  int     index;
  PVector realWorldPoint;
  color   pixelColor;
 
  translate(0,0,-1000);  // set the rotation center of the scene 1000 infront of the camera
  
  //int userCount = context.getNumberOfUsers();
  //context.alternativeViewPointDepthToImage();
  
  stroke(153); 
  strokeWeight(2);
  for(int y=0;y < context.depthHeight();y+=steps)
  {
    for(int x=0;x < context.depthWidth();x+=steps)
    {
      index = x + y * context.depthWidth();
      if(depthMap[index] > 0 && depthMap[index]<bgLimitDepth)
      { 
        // draw the projected point
        realWorldPoint = context.depthMapRealWorld()[index];
        point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);
      }
    } 
  }
  
  // loop through users (update ball, target)
  ArrayList tmpUsers = new ArrayList();
  ArrayList tmpBalls = new ArrayList();
  for (Enumeration e = users.keys() ; e.hasMoreElements() 😉 {
    int user = (Integer) e.nextElement();
    Ball userBall = (Ball) userBalls.get(user);
    userBall.draw();
    tmpUsers.add(user);
    tmpBalls.add(userBall.boxPos);
    Target userT = (Target) userTarget.get(user);
    if (user==userT.parent) {
        userT.draw();
    }
    // draw the skeleton if it's available
    if(context.isTrackingSkeleton(user)) drawSkeleton(user);
  }
  
  // COLLISION
  for(int i=0; i<tmpUsers.size(); i++) {
    for(int i2=0; i2<tmpUsers.size(); i2++) {
      if ((Integer) tmpUsers.get(i2)!=(Integer) tmpUsers.get(i)) {
        Target userT = (Target) userTarget.get(tmpUsers.get(i2));
        for(int k=0; k<tmpBalls.size(); k++) {
            userT.checkCollision((PVector) tmpBalls.get(i),(Integer)tmpUsers.get(i)); 
        }
      }
    }
  }

  // draw the kinect cam
  context.drawCamFrustum();
  
}

// draw the skeleton with the selected joints
void drawSkeleton(int userId)
{
  strokeWeight(3);

  // to get the 3d joint data
  drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK, false);

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

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

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

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

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

  strokeWeight(1);
 
}

void drawLimb(int userId,int jointType1,int jointType2, boolean drawBox)
{
  PVector jointPos1 = new PVector();
  PVector jointPos2 = new PVector();
  float  confidence;
  
  // draw the joint position
  confidence = context.getJointPositionSkeleton(userId,jointType1,jointPos1);
  confidence = context.getJointPositionSkeleton(userId,jointType2,jointPos2);

  stroke(255,0,0,confidence * 200 + 55);
  //line(jointPos1.x,jointPos1.y,jointPos1.z,
  //     jointPos2.x,jointPos2.y,jointPos2.z);
       
  float limbLength = dist(jointPos1.x,jointPos1.y,jointPos1.z,jointPos2.x,jointPos2.y,jointPos2.z);
  
  PVector pVdir = jointPos1.cross(jointPos2); // direction of limb
  drawJointOrientation(userId,jointType1,jointPos1,jointType2,jointPos2,50,drawBox, limbLength, pVdir);
}

void drawJointOrientation(int userId,int jointType,PVector pos,int jointType2,PVector pos2,float length, boolean drawBox, float limbLength, PVector pVdir)
{
  // draw the joint orientation  
  PMatrix3D  orientation = new PMatrix3D();
  float confidence = context.getJointOrientationSkeleton(userId,jointType,orientation);
  if(confidence < 0.001f) 
    // nothing to draw, orientation data is useless
    return;
  
  Ball userBall = (Ball) userBalls.get(userId);
    
  // target
  Target userT = (Target) userTarget.get(userId);
  
  // skeleton head -> draw target
  if (jointType==SimpleOpenNI.SKEL_NECK) {
    userT.ini = false;
    userT.targetPos = pos;
    userT.orientation = orientation;
  }
    
  if (drawBox==true) { 
    
    color uColor = (Integer) userColor.get(userId);
    
    float offsetDefaultY = -700;
    strokeWeight(1);
    
    pushStyle();
    stroke(red(uColor),green(uColor),blue(uColor));
    strokeWeight(2);
    // line target to hand
    line(userBall.boxPos.x,userBall.boxPos.y,userBall.boxPos.z,
       pos2.x,pos2.y,pos2.z);
    popStyle();
    
    PVector targetPos = new PVector(userBall.boxPos.x,userBall.boxPos.y,userBall.boxPos.z);
    PVector lineDir = PVector.sub(pos2,targetPos);
  
    // default position
    //line(userBall.boxPos.x,userBall.boxPos.y,userBall.boxPos.z,pos2.x,pos2.y+offsetDefaultY,pos2.z);
    pushMatrix();
    translate(pos2.x,pos2.y+offsetDefaultY,pos2.z);
    fill(204);
    stroke(102);
    box(50,50,50);
    popMatrix();
    
    PVector defaultPos = new PVector(pos2.x,pos2.y-700,pos2.z);
    PVector defaultDir = PVector.sub(defaultPos,targetPos);
    boxForce.add(defaultDir);
    boxForce.add(lineDir);
    boxForce.normalize();
    boxForce.mult(180);
    
    userBall.boxDir.add(boxForce);
    
    // box hand
    pushMatrix();
      translate(pos2.x,pos2.y,pos2.z);
      
      // set the local coordsys
      applyMatrix(orientation);
      
      if (pVdir.z>0) {
      //translate(limbLength,0,0);
      } else {
      //  translate(-limbLength,0,0);
      }
      
      fill(red(uColor),green(uColor),blue(uColor));
      box(200,100,20);
      
      // coordsys lines are 100mm long
      // x - r
      //stroke(255,0,0,confidence * 200 + 55);
      //line(0,0,0,
      //     length,0,0);
      // y - g
      //stroke(0,255,0,confidence * 200 + 55);
      //line(0,0,0,
      //     0,length,0);
      // z - b    
      //stroke(0,0,255,confidence * 200 + 55);
      //line(0,0,0,
      //     0,0,length);
           
    popMatrix();
  }
}

// -----------------------------------------------------------------
// SimpleOpenNI user events

void onNewUser(int userId)
{
  users.put(userId, userId);
  userColor.put(userId, color(random(0,204),random(0,204),random(0,204)));
  
  color tmpC = (Integer) userColor.get(userId);
  
  // BALL ini            
  Ball tmp = new Ball(tmpC);
  userBalls.put(userId, tmp);
  
  Target userT = new Target(userId);
  userT.userColor = tmpC;
  userTarget.put(userId,userT);

  println("onNewUser - userId: " + userId);
  println("  start pose detection");
  
  if(autoCalib)
    context.requestCalibrationSkeleton(userId,true);
  else    
    context.startPoseDetection("Psi",userId);
}

void onLostUser(int userId)
{
  println("onLostUser - userId: " + userId);
  users.remove(userId);
  userBalls.remove(userId);
}

void onStartCalibration(int userId)
{
  println("onStartCalibration - userId: " + userId);
}

void onEndCalibration(int userId, boolean successfull)
{
  println("onEndCalibration - userId: " + userId + ", successfull: " + successfull);
  
  if (successfull) 
  { 
    println("  User calibrated !!!");
    context.startTrackingSkeleton(userId); 
  } 
  else 
  { 
    println("  Failed to calibrate user !!!");
    println("  Start pose detection");
    context.startPoseDetection("Psi",userId);
  }
}

void onStartPose(String pose,int userId)
{
  println("onStartdPose - userId: " + userId + ", pose: " + pose);
  println(" stop pose detection");
  
  context.stopPoseDetection(userId); 
  context.requestCalibrationSkeleton(userId, true);
 
}

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

// -----------------------------------------------------------------
// Keyboard events

void keyPressed()
{
  switch(key)
  {
  case ' ':
    context.setMirror(!context.mirror());
    break;
  case 'p':
     save("screens/points_"+year()+"-"+month()+"-"+day()+"_"+hour()+"-"+minute()+"-"+second());
   break;
  }
    
  switch(keyCode)
  {
    case LEFT:
      rotY += 0.1f;
      break;
    case RIGHT:
      // zoom out
      rotY -= 0.1f;
      break;
    case UP:
      if(keyEvent.isShiftDown())
        zoomF += 0.01f;
      else
        rotX += 0.1f;
      break;
    case DOWN:
      if(keyEvent.isShiftDown())
      {
        zoomF -= 0.01f;
        if(zoomF < 0.01)
          zoomF = 0.01;
      }
      else
        rotX -= 0.1f;
      break;
  }
}

Class Ball

class Ball {
  
  PVector boxPos = new PVector();
  PVector boxDir = new PVector(0,0,0);
  float ballRotX;
  float ballRotY;
  float gravity = 0;
  float friction = 0.92;
  color ballColor;
  int ballSize = 30;
  
  Ball(color _c) {
    ballColor = _c;
  } 
  
  void draw() {  
    boxDir.x*=friction;
    boxDir.y*=friction;
    boxDir.z*=friction;
    boxPos.add(boxDir);
    
    pushMatrix();
    translate(boxPos.x,boxPos.y,boxPos.z);
    //rotateX(radians(ballRotX));
    //rotateY(radians(ballRotY));
  
    fill(red(ballColor),green(ballColor),blue(ballColor));
    stroke(red(ballColor)-24,green(ballColor)-24,blue(ballColor)-24);
    sphereDetail(8);
    sphere(ballSize);
    popMatrix();
  
    boxPos.y-=gravity;
    //boxPos.y+=1;
  } 
}

Class Target

class Target {
 
 boolean hit=false;
 color targetColor = color(204,204,153);
 PVector targetSize = new PVector(300,500,50);
 PVector targetPos = new PVector(0,0,1000);
 PVector shiftPos = new PVector(0,400,0);
 PVector ballPos = new PVector(0,0,0);
 color userColor = color(0,0,0);
 int parent;
 int xTarget,yTarget,zTarget;
 PMatrix3D  orientation = new PMatrix3D();
 boolean ini=true;
 float hitLength = 30;
 float hitLengthCounter=0;
 float hitArea=400;
 
 Target(int _parent) {
   parent = _parent;
 }

 void draw() {
   if (parent>0 && ini==false) {
     pushMatrix();
     fill(userColor);
     if ((hit==true || hitLengthCounter<hitLength) && ini==false) {
       fill(targetColor);
     }
     targetPos.add(shiftPos);
    
     translate(targetPos.x, targetPos.y, targetPos.z);
    
     // set the local coordsys
     applyMatrix(orientation);
    
     box(targetSize.x,targetSize.y,targetSize.z);
     popMatrix();
     hit=false;
     hitLengthCounter++;
   }
 } 
 
 boolean checkCollision(PVector _p,int user) {  
   ballPos = _p;
    
   if (ini==false && parent != user ) {
     //println("CHECK2"); 
   if((int(ballPos.x) >= (targetPos.x-hitArea) && int(ballPos.x) <= (targetPos.x+hitArea + targetSize.x)) 
     && (int(ballPos.y) >= (targetPos.y-hitArea) && int(ballPos.y) <= (targetPos.y+hitArea + targetSize.y))
     && (int(ballPos.z) >= (targetPos.z-hitArea) && int(ballPos.z) <= (targetPos.z+hitArea + targetSize.z))
     ){
       println("Kollision  parent " + parent + " user " + user);
       hit=true;
       hitLengthCounter=0;
     } 
   }
   return hit;
 }
}