/**Function********************************************************************
Synopsis [Prints a ZDD to the standard output. One line per node is
printed.]
Description [Prints a ZDD to the standard output. One line per node is
printed. Returns 1 if successful; 0 otherwise.]
SideEffects [None]
SeeAlso [Cudd_zddPrintDebug]
******************************************************************************/
int
cuddZddP(
DdManager * zdd,
DdNode * f)
{
int retval;
st_table *table = st_init_table(st_ptrcmp, st_ptrhash);
if (table == NULL) return(0);
retval = zp2(zdd, f, table);
st_free_table(table);
(void) fputc('\n', zdd->out);
return(retval);
} /* end of cuddZddP */
//--------------------------------------------------------------
void testApp::update(){
ofBackground(100, 100, 100);
// Pull in new frame
kinect.update();
grayImage.setFromPixels(kinect.getDepthPixels(), kinect.width, kinect.height);
colorImg.setFromPixels(kinect.getCalibratedRGBPixels(), kinect.width, kinect.height);
// Quick and dirty noise filter on the depth map. Needs work
grayImage.dilate();
grayImage.erode();
// If the user pressed spacebar, capture the depth iamge and save for later
if (bLearnBakground == true){
grayBg = grayImage;
bLearnBakground = false;
}
// The next few steps mask the depthmap so that only pixels
// that have changed since background was captured are considered
// Subtract the saved background from the current one
grayDiff = grayImage;
grayDiff -= grayBg;
// anything that is > 1 has changed, so keep it
grayDiff.threshold(1);
// multiply in the current depth values, to mask it
grayDiff *= grayImage;
// cut off anything that is too far away
grayDiff.threshold(threshold);
// Find blobs (should be hands) in the filtered depthmap
contourFinder.findContours(grayDiff, 1000, (kinect.width*kinect.height)/2, 5, false);
// if at least 2 blobs were detected (presumably 2 hands), figure out
// their locations and calculate the new size and rotation of the teapot
if (contourFinder.blobs.size() >= 2) {
// Find the x,y, and z of the center of the first 2 blobs
float x1 = contourFinder.blobs[0].centroid.x;
float y1 = contourFinder.blobs[0].centroid.y;
float x2 = contourFinder.blobs[1].centroid.x;
float y2 = contourFinder.blobs[1].centroid.y;
float z1 = kinect.getDistanceAt(x1,y1);
float z2 = kinect.getDistanceAt(x2,y2);
// zp# are used to rotate about the z axis
// the x1<x2 check is to ensure that p1 is always the leftmost blob (right hand)
ofPoint zp1(x1<x2 ? x1 : x2,x1<x2 ? y1 : y2, 0);
ofPoint zp2(x2<x1 ? x1 : x2,x2<x1 ? y1 : y2, 0);
ofxVec3f zVec = zp1-zp2; // Vector connecting both hands excluding z direction
// yp# about the y
ofPoint yp1(x1<x2 ? x1 : x2, 0, x1<x2 ? z1 : z2);
ofPoint yp2(x2<x1 ? x1 : x2, 0, x1>x2 ? z1 : z2);
ofxVec3f yVec = yp1-yp2; // Vector connecting both hands excluding y direction
// flat vector to base angle offsets on
ofxVec3f horizonVec(zp1.x+1,0,0);
// calculate the rotation angle about the z axis by finding the
// the anlge between the horizon and the respective hand vectors
potZangle = (zp1.y > zp2.y ? -1 : 1 ) * horizonVec.angle(zVec);
potYangle = (yp1.z > yp2.z ? -10 : 10 ) * horizonVec.angle(yVec);
// calculate scale based on the distance of the hands from eachother
potSize = zVec.length();
}
}
/**Function********************************************************************
Synopsis [Performs the recursive step of cuddZddP.]
Description [Performs the recursive step of cuddZddP. Returns 1 in
case of success; 0 otherwise.]
SideEffects [None]
SeeAlso []
******************************************************************************/
static int
zp2(
DdManager * zdd,
DdNode * f,
st_table * t)
{
DdNode *n;
int T, E;
DdNode *base = DD_ONE(zdd);
if (f == NULL)
return(0);
if (Cudd_IsConstant(f)) {
(void)fprintf(zdd->out, "ID = %d\n", (f == base));
return(1);
}
if (st_is_member(t, (char *)f) == 1)
return(1);
if (st_insert(t, (char *) f, NULL) == ST_OUT_OF_MEM)
return(0);
#if SIZEOF_VOID_P == 8
(void) fprintf(zdd->out, "ID = 0x%lx\tindex = %d\tr = %d\t",
(unsigned long)f / (unsigned long) sizeof(DdNode), f->index, f->ref);
#else
(void) fprintf(zdd->out, "ID = 0x%x\tindex = %d\tr = %d\t",
(unsigned)f / (unsigned) sizeof(DdNode), f->index, f->ref);
#endif
n = cuddT(f);
if (Cudd_IsConstant(n)) {
(void) fprintf(zdd->out, "T = %d\t\t", (n == base));
T = 1;
} else {
#if SIZEOF_VOID_P == 8
(void) fprintf(zdd->out, "T = 0x%lx\t", (unsigned long) n /
(unsigned long) sizeof(DdNode));
#else
(void) fprintf(zdd->out, "T = 0x%x\t", (unsigned) n / (unsigned) sizeof(DdNode));
#endif
T = 0;
}
n = cuddE(f);
if (Cudd_IsConstant(n)) {
(void) fprintf(zdd->out, "E = %d\n", (n == base));
E = 1;
} else {
#if SIZEOF_VOID_P == 8
(void) fprintf(zdd->out, "E = 0x%lx\n", (unsigned long) n /
(unsigned long) sizeof(DdNode));
#else
(void) fprintf(zdd->out, "E = 0x%x\n", (unsigned) n / (unsigned) sizeof(DdNode));
#endif
E = 0;
}
if (E == 0)
if (zp2(zdd, cuddE(f), t) == 0) return(0);
if (T == 0)
if (zp2(zdd, cuddT(f), t) == 0) return(0);
return(1);
} /* end of zp2 */
