void TuioKinect::drawTuioPath( std::list<TuioPoint> acPath )
{
int count = -1 ;
std::list<TuioPoint>::iterator curPoint;
for ( curPoint = acPath.end() ; curPoint != acPath.begin() ; curPoint-- )
{
TuioPoint ac = (*curPoint) ;
float ratio = (float)(20-count)/20.0f ;
float xpos = ac.getX()*ofGetWidth() ;
float ypos = ac.getY()*ofGetHeight() + ( count * 3 ) ;
float lastX , lastY ;
float midX , midY ;
int fadedValue = 255*(ratio) ;
if ( count < 25 )
{
ofSetColor(255,fadedValue,fadedValue ,fadedValue) ;
ofCircle(xpos,ypos ,15*ratio);
}
else
break ;
count++ ;
}
}
TuioPoint updateKalman(int id, TuioPoint tp) {
if (id>=16) return NULL;
if(tuioPointSmoothed[id*2] == NULL) {
tuioPointSmoothed[id*2] = new ofxCvKalman(tp.getX());
tuioPointSmoothed[id*2+1] = new ofxCvKalman(tp.getY());
} else {
tp.update(tuioPointSmoothed[id*2]->correct(tp.getX()),tuioPointSmoothed[id*2+1]->correct(tp.getY()));
}
return tp;
}
void TuioBlob::update (TuioTime ttime, float xp, float yp, float a, float w, float h, float f) {
TuioPoint lastPoint = path.back();
TuioContainer::update(ttime,xp,yp);
TuioTime diffTime = currentTime - lastPoint.getTuioTime();
float dt = diffTime.getTotalMilliseconds()/1000.0f;
float last_rotation_speed = rotation_speed;
float prev_angle = angle_sum;
float da = a-angle;
if (da > M_PI/2.0f) angle_sum += (da-2*M_PI);
else if (da < M_PI/-2.0f) angle_sum += (da+2*M_PI);
else angle_sum += da;
if (angleFilter) angle_sum = angleFilter->filter(angle_sum,dt);
if (fabs(angle_sum-prev_angle)<angleThreshold) angle_sum = prev_angle;
int m = floor(angle_sum/(2*M_PI));
angle = angle_sum-(m*(2*M_PI));
da = (angle-a)/(2*M_PI);
if (da > 0.75f) da-=1.0f;
else if (da < -0.75f) da+=1.0f;
if (widthFilter && heightFilter) {
w = widthFilter->filter(w,dt);
h = heightFilter->filter(h,dt);
}
float dw = fabs(width - w);
float dh = fabs(height - h);
if ((dw>sizeThreshold) || (dh>sizeThreshold)) {
width = w;
height = h;
}
area = f;
rotation_speed = (float)da/dt;
rotation_accel = (rotation_speed - last_rotation_speed)/dt;
if ((rotation_accel!=0) && (state==TUIO_STOPPED)) state = TUIO_ROTATING;
}
void TuioObject::update (TuioTime ttime, float xp, float yp, float a) {
TuioPoint lastPoint = path.back();
TuioContainer::update(ttime,xp,yp);
TuioTime diffTime = currentTime - lastPoint.getTuioTime();
float dt = diffTime.getTotalMilliseconds()/1000.0f;
float last_angle = angle;
float last_rotation_speed = rotation_speed;
angle = a;
double da = (angle-last_angle)/(2*M_PI);
if (da > 0.75f) da-=1.0f;
else if (da < -0.75f) da+=1.0f;
rotation_speed = (float)da/dt;
rotation_accel = (rotation_speed - last_rotation_speed)/dt;
if ((rotation_accel!=0) && (state==TUIO_STOPPED)) state = TUIO_ROTATING;
}
void TuioKinect::updateTuio() {
TuioTime frameTime = TuioTime::getSessionTime();
tuioServer->initFrame(frameTime);
std::vector<ofxCvBlob>::iterator blob;
// printf("Blobs: %d\n", contourFinder.blobs.size());
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);
//if ((tp.getY() > 0.8) && (tp.getX()>0.25) && (tp.getX()<0.75)) continue;
TuioCursor *tcur = tuioServer->getClosestTuioCursor(tp.getX(),tp.getY());
if ((tcur==NULL) || (tcur->getDistance(&tp)>0.2)) {
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());
}
}
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();
}
void TuioContainer::update (TuioTime ttime, float xp, float yp) {
TuioPoint lastPoint = path.back();
TuioPoint::update(ttime,xp, yp);
TuioTime diffTime = currentTime - lastPoint.getTuioTime();
float dt = diffTime.getTotalMilliseconds()/1000.0f;
float dx = xpos - lastPoint.getX();
float dy = ypos - lastPoint.getY();
float dist = sqrt(dx*dx+dy*dy);
float last_motion_speed = motion_speed;
x_speed = dx/dt;
y_speed = dy/dt;
motion_speed = dist/dt;
motion_accel = (motion_speed - last_motion_speed)/dt;
TuioPoint p(currentTime,xpos,ypos);
path.push_back(p);
if (motion_accel>0) state = TUIO_ACCELERATING;
else if (motion_accel<0) state = TUIO_DECELERATING;
else state = TUIO_STOPPED;
}
//--------------------------------------------------------------
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() ;
}
