//--------------------------------------------------------------
void TuioKinect::update()
{
ofBackground(10, 10, 10);
//Kinect + Feeds updating
kinect.update();
grayImage.setFromPixels(kinect.getDepthPixels(), kinect.width, kinect.height);
grayImage.mirror(false, true);
unsigned char * pix = grayImage.getPixels();
int numPixels = grayImage.getWidth() * grayImage.getHeight()-1;
depthImage.setFromPixels(pix, kinect.width, kinect.height);
depthImage.flagImageChanged();
colorImage.setFromPixels(kinect.getPixels(), kinect.width, kinect.height);
colorImage.mirror(false, true);
colorImage.convertToGrayscalePlanarImage(redImage, 0);
for(int i = numPixels; i > 0 ; i--){
if( pix[i] > nearThreshold && pix[i] < farThreshold ){
pix[i] = 255;
}else{
pix[i] = 0;
}
}
//update the cv image
grayImage.flagImageChanged();
unsigned char * red = redImage.getPixels();
numPixels = redImage.getWidth() * redImage.getHeight();
//
contourFinder.findContours(grayImage, 900, (kinect.width*kinect.height)/8, 20, false);
TuioTime frameTime = TuioTime::getSessionTime();
tuioServer->initFrame(frameTime);
int numBlobs = contourFinder.blobs.size() ;
int count = 0 ;
if ( numBlobs == 0 )
holdingLength = 0 ;
//detect all blobs
std::vector<ofxCvBlob>::iterator blob;
for (blob=contourFinder.blobs.begin(); blob!= contourFinder.blobs.end(); blob++)
{
float xpos = (*blob).centroid.x;
float ypos = (*blob).centroid.y;
TuioPoint tp(xpos/kinect.width,ypos/kinect.height);
TuioCursor *tcur = tuioServer->getClosestTuioCursor(tp.getX(),tp.getY());
//Adjust here to change minumum distance for new points
if ((tcur==NULL) || (tcur->getDistance(&tp)>0.1)) {
tcur = tuioServer->addTuioCursor(tp.getX(), tp.getY());
updateKalman(tcur->getCursorID(),tcur);
} else {
TuioPoint kp = updateKalman(tcur->getCursorID(),tp);
tuioServer->updateTuioCursor(tcur, kp.getX(), kp.getY());
}
xpos = tp.getX() * ofGetWidth() ;
ypos = tp.getY() * ofGetHeight() ;
if ( numBlobs < 3 )
{
kCursors[count].x = xpos ;
kCursors[count].y = ypos ;
}
count++ ;
}
float strength = 5.0f;
float damping = 0.5f;
if ( numBlobs == 2 )
{
if ( holdingLength < 40 )
holdingLength++ ;
float holdingRatio = 0.05f + (float)holdingLength/40.0f ;
float minDis = 100.0f;
ofPoint c1 = kCursors[1] ;
ofPoint c0 = kCursors[0] ;
for(int i=0; i< customParticles.size() ; i++)
{
CustomParticle& p = customParticles[i] ;
if ( i % 2 == 0 )
{
p.addAttractionPoint( c0.x, c0.y, (float)holdingLength*1.05 * strength, minDis);
}
else
{
p.addAttractionPoint( c1.x, c1.y, (float)holdingLength*1.05 * strength, minDis);
}
p.addDamping(damping, damping);
}
}
//remove non moving cursors
tuioServer->stopUntouchedMovingCursors();
std::list<TuioCursor*> dead_cursor_list = tuioServer->getUntouchedCursors();
std::list<TuioCursor*>::iterator dead_cursor;
for (dead_cursor=dead_cursor_list.begin(); dead_cursor!= dead_cursor_list.end(); dead_cursor++)
{
clearKalman((*dead_cursor)->getCursorID());
}
tuioServer->removeUntouchedStoppedCursors();
tuioServer->commitFrame();
box2d.update() ;
}
//--------------------------------------------------------------
void testApp::update(){
/************ UPDATE BALL ***********************/
//Update ball position
ballPositionX += ballVelocityX;
ballPositionY += ballVelocityY;
if(ballPositionX < 0 || ballPositionX > ofGetWidth()) {
ballVelocityX *= -1;
}
if (ballPositionY < 0 || ballPositionY > ofGetHeight()) {
ballVelocityY *= -1;
}
/************ UPDATE KINECT ***********************/
kinect.update();
// get color pixels
colorImageRGB = kinect.getPixels();
// get depth pixels
depthOrig = kinect.getDepthPixels();
// save original depth, and do some preprocessing
depthProcessed = depthOrig;
if(invert) depthProcessed.invert();
if(mirror) {
depthOrig.mirror(false, true);
depthProcessed.mirror(false, true);
colorImageRGB.mirror(false, true);
}
if(preBlur) cvSmooth(depthProcessed.getCvImage(), depthProcessed.getCvImage(), CV_BLUR , preBlur*2+1);
if(topThreshold) cvThreshold(depthProcessed.getCvImage(), depthProcessed.getCvImage(), topThreshold * 255, 255, CV_THRESH_TRUNC);
if(bottomThreshold) cvThreshold(depthProcessed.getCvImage(), depthProcessed.getCvImage(), bottomThreshold * 255, 255, CV_THRESH_TOZERO);
if(dilateBeforeErode) {
if(dilateAmount) cvDilate(depthProcessed.getCvImage(), depthProcessed.getCvImage(), 0, dilateAmount);
if(erodeAmount) cvErode(depthProcessed.getCvImage(), depthProcessed.getCvImage(), 0, erodeAmount);
} else {
if(erodeAmount) cvErode(depthProcessed.getCvImage(), depthProcessed.getCvImage(), 0, erodeAmount);
if(dilateAmount) cvDilate(depthProcessed.getCvImage(), depthProcessed.getCvImage(), 0, dilateAmount);
}
depthProcessed.flagImageChanged();
// find contours
depthContours.findContours(depthProcessed,
minBlobSize * minBlobSize * depthProcessed.getWidth() * depthProcessed.getHeight(),
maxBlobSize * maxBlobSize * depthProcessed.getWidth() * depthProcessed.getHeight(),
maxNumBlobs, findHoles, useApproximation);
// Clear old attraction points
attractPts.clear();
// Find centroid point for each blob area and add an attraction force to it
for (int i=0; i<depthContours.blobs.size(); i++) {
attractPts.push_back(new ofPoint(depthContours.blobs[i].centroid));
//printf("Blob %d: %f %f \n", i, depthContours.blobs[i].centroid.x, depthContours.blobs[i].centroid.y);
}
// if one blob found, find nearest point in blob area
static ofxVec3f newPoint;
if(depthContours.blobs.size() == 1) {
ofxCvBlob &blob = depthContours.blobs[0];
depthOrig.setROI(blob.boundingRect);
double minValue, maxValue;
CvPoint minLoc, maxLoc;
cvMinMaxLoc(depthOrig.getCvImage(), &minValue, &maxValue, &minLoc, &maxLoc, NULL);
depthOrig.resetROI();
newPoint.x = maxLoc.x + blob.boundingRect.x;
newPoint.y = maxLoc.y + blob.boundingRect.y;
// newPoint.z = (maxValue + offset) * depthScale; // read from depth map
//printf("Min: %f %f Max: %f %f \n", minLoc.x, minLoc.y, maxLoc.x, maxLoc.y);
// read directly from distance (in cm)
// this doesn't seem to work, need to look into it
newPoint.z = (kinect.getDistanceAt(newPoint) + depthOffset) * depthScale;
// apply kalman filtering
if(doKalman) {
newPoint.x = updateKalman(0, newPoint.x);
newPoint.y = updateKalman(1, newPoint.y);
newPoint.z = updateKalman(2, newPoint.z);
}
} else {
clearKalman(0);
clearKalman(1);
clearKalman(2);
}
pointHead = (pointHead + 1) % kNumPoints;
curPoint += (newPoint - curPoint) * lerpSpeed;
points[pointHead] = curPoint;
}
