import controlP5.*;
import KinectPV2.*;
import blobDetection.*;
BlobDetection theBlobDetection;
KinectPV2 kinect;
ControlP5 cp5;
int KmaxD = 4000; // 4m
int KminD = 100; // 0m
int MoveX;
int MoveY;
float Scale_Kinect;
boolean kinect_on = false;
ArrayList grid = new ArrayList();
ArrayList flowpts = new ArrayList();
IntList trkpts = new IntList();
int num = 8;
int[] lpts = new int[num];
int[] dia = new int[num];
int[] pts = new int[num];
float mx = 0;
float my = 0;
float sx = 99999;
float sy = 99999;
PImage img;
PImage Kimg;
PGraphics pg;
int xsize = 70;
int ysize = 35;
int space = 35;
int bg = 0;
int blob_size;
void setup() {
//size(1600, 1000);
fullScreen();
smooth(4);
pg = createGraphics(512, 424);
cp5 = new ControlP5(this);
kinect = new KinectPV2(this);
kinect.enableDepthImg(true);
kinect.enablePointCloud(true);
kinect.enableColorImg(true);
kinect.init();
theBlobDetection = new BlobDetection(512, 424);
theBlobDetection.setPosDiscrimination(true);
theBlobDetection.setThreshold(0.1f);
cp5 = new ControlP5(this);
cp5.addToggle("kinect_on")
.setPosition(40,40)
.setSize(20,20)
.setColorForeground(color(20,20,20))
.setColorLabel(color(255))
.setColorBackground(color(70,70,70))
.setColorValue(0xffff88ff)
.setColorActive(color(0,200,200))
;
cp5.addSlider("KminD")
.setPosition(100,40)
.setRange(0,700)
.setSize(200,20)
.setValue(200)
.setColorForeground(color(20,200,200))
.setColorLabel(color(255))
.setColorBackground(color(70,70,70))
.setColorValue(color(0,0,0))
.setColorActive(color(0,255,255))
;
cp5.addSlider("KmaxD")
.setPosition(100,80)
.setRange(0,700)
.setSize(200,20)
.setValue(450)
.setColorForeground(color(20,200,200))
.setColorLabel(color(255))
.setColorBackground(color(70,70,70))
.setColorValue(color(0,0,0))
.setColorActive(color(0,255,255))
;
cp5.addSlider("Scale_Kinect")
.setPosition(340,40)
.setRange(0,5)
.setSize(200,20)
.setValue(5)
.setColorForeground(color(20,200,200))
.setColorLabel(color(255))
.setColorBackground(color(70,70,70))
.setColorValue(color(0,0,0))
.setColorActive(color(0,255,255))
;
cp5.addSlider("blob_size")
.setPosition(340,80)
.setRange(0,40000)
.setSize(200,20)
.setValue(8000)
.setColorForeground(color(20,200,200))
.setColorLabel(color(255))
.setColorBackground(color(70,70,70))
.setColorValue(color(0,0,0))
.setColorActive(color(0,255,255))
;
img = loadImage("heat.jpg");
for(int i = 0; i < xsize; ++i){
for(int j = 0; j < ysize; j++){
float xx = i*space + (width/2 - xsize*space/2);
float yy = j*space + (height/2 - ysize*space/2);
float zz = 0;
grid.add(new PVector(xx,yy,zz));
flowpts.add(new PVector(xx,yy,zz));
if(xx > mx){
mx = xx;
}
if(yy > my){
my = yy;
}
if(xx < sx){
sx = xx;
}
if(yy < sy){
sy = yy;
}
}
}
for(int i = 0; i < 0; i++){
lpts[i] = int(random(grid.size()-1));
trkpts.append(int(random(grid.size()-1)));
}
//noLoop();
background(30);
}
void draw() {
if(bg == 0){
background(30);
bg = 1;
}
kinect.setLowThresholdPC(KminD);
kinect.setHighThresholdPC(KmaxD);
Kimg = kinect.getPointCloudDepthImage();
trkpts = new IntList();
trkpts.append(120);
trkpts.append(450);
float sc = Scale_Kinect;
pushMatrix();
translate(width/2,height/2);
pg.beginDraw();
pg.background(100);
pg.image(Kimg,0,0);
pg.endDraw();
scale(sc);
if(kinect_on){
background(30);
image(pg,-512/2,-424/2);
bg = 0;
}
popMatrix();
pushMatrix();
theBlobDetection.computeBlobs(pg.pixels);
drawBlobsAndEdges(kinect_on,kinect_on);
popMatrix();
fill(0,0,0,3);
noStroke();
rect(0,0,width,height);
//image(img, 30, 30);
for(int i = 0; i < (grid.size()); ++i){
PVector vs = grid.get(i);
noStroke();
fill(100);
int rad = 7;
for(int j = 0; j < trkpts.size(); j++){
if(trkpts.get(j) == i){
fill(0,102,153);
rad = 30;
}
}
ellipse(vs.x,vs.y,rad,rad);
}
////////////weighted average/////////
////////////get low and high distance//////////////
PVector vs0 = flowpts.get(0);
PVector vsn = grid.get(trkpts.get(0));
float l = 1/dist(vs0.x,vs0.y,vsn.x,vsn.y);
float h = 1/dist(vs0.x,vs0.y,vsn.x,vsn.y);
for(int i = 0; i < flowpts.size(); i++){
for(int ac = 0; ac < trkpts.size(); ac++){
vsn = grid.get(trkpts.get(ac));
float d = dist(vs0.x,vs0.y,vsn.x,vsn.y);
if(d < l){
l = d;
}
if(d > h){
h = d;
}
}
}
for(int i = 0; i < flowpts.size(); i++){
PVector vs = flowpts.get(i);
float dv = 0;
float tx = 0;
float ty = 0;
float dd = 99999;
for(int ac = 0; ac < trkpts.size(); ac++){
if(ac < num){
vsn = grid.get(trkpts.get(ac));
}else{
vsn = (new PVector(mouseX,mouseY,0));
}
float nd = dist(vs.x,vs.y,vsn.x,vsn.y);
if(nd < dd){
dd = nd;
}
float d = 1/dist(vs.x,vs.y,vsn.x,vsn.y);
float rd = map(d,l,h,0.0,1.0);
tx = tx + vsn.x*rd*rd;
ty = ty + vsn.y*rd*rd;
dv = dv + rd*rd;
}
tx = tx/dv;
ty = ty/dv;
float na = -atan2(tx-vs.x, ty-vs.y);
stroke(0,255,255);
//line(vs.x,vs.y,tx,ty);
color c = img.get(int(dd),2);
stroke(c);
int ww = int(7-dd/22);
if(ww < 1){
ww = 1;
}
strokeWeight(ww*3);
if(vs.x > mx || vs.x < sx){
stroke(25);
}
if(vs.y > my || vs.y < sy){
stroke(25);
}
point(vs.x,vs.y);
float nx = vs.x;
float ny = vs.y;
nx = nx + 1 * cos(na);
ny = ny + 1 *sin(na);
flowpts.set(i,new PVector(nx,ny,0));
pushMatrix();
translate(vs.x,vs.y);
rotate(na);
stroke(255);
//println(na);
//line(-20,0,20,0);
popMatrix();
}
if(frameCount%600 == 0){
flowpts = new ArrayList();
for(int i = 0; i < (grid.size()); ++i){
PVector vs = grid.get(i);
flowpts.add(new PVector(vs.x,vs.y,0));
}
}
}
void mouseClicked() {
for(int i = 0; i < (grid.size()); ++i){
PVector vs = grid.get(i);
if(dist(mouseX,mouseY,vs.x,vs.y) < 9){
fill(255,0,0);
int[] npts = new int[num];
for(int j = 1; j < num; j++){
npts[j] = lpts[j-1];
}
lpts = npts;
lpts[0] = i;
}
}
}
// ==================================================
// drawBlobsAndEdges()
// ==================================================
void drawBlobsAndEdges(boolean drawBlobs, boolean drawEdges)
{
noFill();
Blob b;
EdgeVertex eA,eB;
for (int n=0 ; n blob_size){
float xc = b.xMin*512*Scale_Kinect+width/2-512*Scale_Kinect/2 + b.w*512*Scale_Kinect/2;
float yc = b.yMin*424*Scale_Kinect+height/2-424*Scale_Kinect/2 + b.h*424*Scale_Kinect/2;
if (drawBlobs){
rect(
b.xMin*512*Scale_Kinect+width/2-512*Scale_Kinect/2,b.yMin*424*Scale_Kinect+height/2-424*Scale_Kinect/2,
b.w*512*Scale_Kinect,b.h*424*Scale_Kinect
);
noStroke();
fill(255,0,0);
ellipse(xc,yc,10,10);
}
for(int i = 0; i < (grid.size()); ++i){
PVector vs = grid.get(i);
if(dist(xc,yc,vs.x,vs.y) < 20){
trkpts.append(i);
}
}
}
}
}
}