week15_王冠閔_02163042

import SimpleOpenNI.*;

SimpleOpenNI  openni;
color[]       userClr = new color[] {
  color(255, 0, 0),
  color(0, 255, 0),
  color(0, 0, 255),
  color(255, 255, 0),
  color(255, 0, 255),
  color(0, 255, 255)
};
PVector com = new PVector();                                 
PVector com2d = new PVector();
PVector rpos = new PVector(0, 0, 0);
PVector lpos = new PVector(0, 0, 0);
PVector hpos = new PVector(0, 0, 0);

void setup()
{
  size(640, 480);

  openni = new SimpleOpenNI(this);
  if (openni.isInit() == false)
  {
    println("Can't init SimpleOpenNI, maybe the camera is not connected!");
    exit();
    return;
  }
  openni.enableRGB();
  openni.enableDepth();
  openni.enableUser();
  stroke(0, 0, 255);
  strokeWeight(3);
  smooth();
}

void draw()
{
  openni.update();
  background(255);
  image(openni.userImage(), 0, 0);
  int[] userList = openni.getUsers();
  for (int userID : userList) {
    openni.getJointPositionSkeleton(userID, SimpleOpenNI.SKEL_RIGHT_HAND, rpos);
    openni.getJointPositionSkeleton(userID, SimpleOpenNI.SKEL_HEAD, hpos);
  }
  for (int i=0; i<userList.length; i++)
  {
    if (openni.isTrackingSkeleton(userList[i]))
    {
      stroke(userClr[ (userList[i] - 1) % userClr.length ] );
      drawSkeleton(userList[i]);
    }     

    if (openni.getCoM(userList[i], com))
    {
      openni.convertRealWorldToProjective(com, com2d);
      stroke(100, 255, 0);
      strokeWeight(1);
      beginShape(LINES);
      vertex(com2d.x, com2d.y - 5);
      vertex(com2d.x, com2d.y + 5);

      vertex(com2d.x - 5, com2d.y);
      vertex(com2d.x + 5, com2d.y);
      endShape();

      fill(0, 255, 100);
      text(Integer.toString(userList[i]), com2d.x, com2d.y);
    }
  }
  Swipe();
  Raise();
}

void drawSkeleton(int userId)
{
  openni.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

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

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

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

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

  openni.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
  openni.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
  openni.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);
}
void onNewUser(SimpleOpenNI curContext, int userId)
{
  println("onNewUser - userId: " + userId);
  println("\tstart tracking skeleton");

  curContext.startTrackingSkeleton(userId);
}

private float ScaleVector(int length, float position)
{
  float value = (((((float)length) / 1f) / 2f) * position) + (length / 2);
  if (value > length)
  {
    return (float)length;
  }
  if (value < 0f)
  {
    return 0f;
  }
  return value;
}

void Swipe()
{
  int[] userList = openni.getUsers();
  for (int userID : userList) {
    openni.getJointPositionSkeleton(userID, SimpleOpenNI.SKEL_RIGHT_HAND, rpos);
    openni.getJointPositionSkeleton(userID, SimpleOpenNI.SKEL_HEAD, hpos);
  }
  if (rpos.x > hpos.x + 100)
  {
    println("Wave!");
  }
}

void Raise()
{
  int[] userList = openni.getUsers();
  for (int userID : userList)
  {
    openni.getJointPositionSkeleton(userID, SimpleOpenNI.SKEL_LEFT_HAND, lpos);
    openni.getJointPositionSkeleton(userID, SimpleOpenNI.SKEL_HEAD, hpos);
  }
  if (lpos.y>hpos.y)
  {
    println("Raise!");
  }
}