import SimpleOpenNI.*;

SimpleOpenNI kinect;

PVector ipos;
PVector rpos;
PVector[] realWorld;

void setup(){
    size(1280,480);
    background(0, 0, 0);
    
    kinect = new SimpleOpenNI(this);
    kinect.setMirror(false);
    kinect.enableDepth();
}

void draw(){
    background(0, 0, 0);
    
    kinect.update();
    ipos = new PVector();
    rpos = new PVector();
    realWorld = new PVector[640];

    for (int i = 0; i < 640; i++){
            int index = 240 * 640 + i;
            rpos = kinect.depthMapRealWorld()[index];
            realWorld[i] = rpos;
    }
    
    //draw image 
    image(kinect.depthImage(), 0, 0);
    line(0, 240, 639, 240);
    
    //draw grid
    stroke(150, 150, 150);
    for (int x = -300; x <= 600; x += 100) {
        line(640 + x + 320, 0, 640 + x + 320, 479);
    }
    for (int y = 0; y < 480; y += 100) {
        line(640, 480 - y, 640 + 639, 480 - y);
    }
    stroke(200, 200, 0);
    float offset = 480 * tan(29.0 * PI / 180.0);
    line(640 + 320 - offset, 0, 640 + 320, 480);
    line(640 + 320 + offset, 0, 640 + 320, 480);
      
    //draw RADAR
    fill(100, 200, 100);
    for (int j = 0; j < 640; j++){
        PVector world = realWorld[j];
        float scrX = world.x * 0.1 + 320;
        float scrY = 479 - world.z * 0.1;
        if (0 <= scrX && scrX < 640 && 0 <= scrY && scrY < 480)
            ellipse(640 + scrX, scrY, 5, 5);
    }
}