//--------------------------------------------------------------
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();
}
}
// GAUSS-LOBATTO QUADRATURE ALONG EDGE
void SetEdgeDataGL_Unst(const mesh& Mesh,
int NumQuadPoints,
int NumBasisOrder,
edge_data_Unst* EdgeData)
{
// Quick error check
if (NumQuadPoints<2 || NumQuadPoints>6 || NumBasisOrder<1 || NumBasisOrder>5)
{
printf(" \n");
printf(" Error in SetEdgeData_Unst.cpp \n");
printf(" NumQuadPoints must be 2,3,4,5, or 6.\n");
printf(" NumBasisOrder must be 1,2,3,4, or 5.\n");
printf(" NumQuadPoints = %i\n",NumQuadPoints);
printf(" NumBasisOrder = %i\n",NumBasisOrder);
printf("\n");
exit(1);
}
// ---------------------------------
// Set quadrature weights and points
// ---------------------------------
switch( NumQuadPoints )
{
case 2:
EdgeData->GL_wgts1d->set(1, 1.0 );
EdgeData->GL_wgts1d->set(2, 1.0 );
EdgeData->GL_xpts1d->set(1, 1.0 );
EdgeData->GL_xpts1d->set(2, -1.0 );
break;
case 3:
EdgeData->GL_wgts1d->set(1, onethird );
EdgeData->GL_wgts1d->set(2, 4.0*onethird );
EdgeData->GL_wgts1d->set(3, onethird );
EdgeData->GL_xpts1d->set(1, 1.0 );
EdgeData->GL_xpts1d->set(2, 0.0 );
EdgeData->GL_xpts1d->set(3, -1.0 );
break;
case 4:
EdgeData->GL_wgts1d->set(1, 0.5*onethird );
EdgeData->GL_wgts1d->set(2, 2.5*onethird );
EdgeData->GL_wgts1d->set(3, 2.5*onethird );
EdgeData->GL_wgts1d->set(4, 0.5*onethird );
EdgeData->GL_xpts1d->set(1, 1.0 );
EdgeData->GL_xpts1d->set(2, osq5 );
EdgeData->GL_xpts1d->set(3, -osq5 );
EdgeData->GL_xpts1d->set(4, -1.0 );
break;
case 5:
EdgeData->GL_wgts1d->set(1, 0.1 );
EdgeData->GL_wgts1d->set(2, 49.0/90.0 );
EdgeData->GL_wgts1d->set(3, 32.0/45.0 );
EdgeData->GL_wgts1d->set(4, 49.0/90.0 );
EdgeData->GL_wgts1d->set(5, 0.1 );
EdgeData->GL_xpts1d->set(1, 1.0 );
EdgeData->GL_xpts1d->set(2, sq3*osq7 );
EdgeData->GL_xpts1d->set(3, 0.0 );
EdgeData->GL_xpts1d->set(4, -sq3*osq7 );
EdgeData->GL_xpts1d->set(5, -1.0 );
break;
case 6:
EdgeData->GL_wgts1d->set(1, 0.2*onethird );
EdgeData->GL_wgts1d->set(2, (1.4 - 0.1*sq7)*onethird );
EdgeData->GL_wgts1d->set(3, (1.4 + 0.1*sq7)*onethird );
EdgeData->GL_wgts1d->set(4, (1.4 + 0.1*sq7)*onethird );
EdgeData->GL_wgts1d->set(5, (1.4 - 0.1*sq7)*onethird );
EdgeData->GL_wgts1d->set(6, 0.2*onethird );
EdgeData->GL_xpts1d->set(1, 1.0 );
EdgeData->GL_xpts1d->set(2, (1/21.0)*sqrt(147.0+42.0*sq7) );
EdgeData->GL_xpts1d->set(3, (1/21.0)*sqrt(147.0-42.0*sq7) );
EdgeData->GL_xpts1d->set(4, -(1/21.0)*sqrt(147.0-42.0*sq7) );
EdgeData->GL_xpts1d->set(5, -(1/21.0)*sqrt(147.0+42.0*sq7) );
EdgeData->GL_xpts1d->set(6, -1.0 );
break;
}
// ---------------------------------
// Legendre basis functions on the
// left and right of each edge
// ---------------------------------
const int NumEdges = Mesh.get_NumEdges();
const int NumBasisComps = (NumBasisOrder*(NumBasisOrder+1))/2;
dTensor1 xp1(3);
dTensor1 yp1(3);
dTensor1 xp2(3);
dTensor1 yp2(3);
dTensor1 xy1(2);
dTensor1 xy2(2);
dTensor1 mu1(NumBasisComps);
dTensor1 mu2(NumBasisComps);
for (int i=1; i<=NumEdges; i++)
{
// Get edge information
const double x1 = Mesh.get_edge(i,1);
const double y1 = Mesh.get_edge(i,2);
const double x2 = Mesh.get_edge(i,3);
const double y2 = Mesh.get_edge(i,4);
const int e1 = Mesh.get_eelem(i,1);
const int e2 = Mesh.get_eelem(i,2);
// Get element information about
// the two elements that meet at
// the current edge
const double Area1 = Mesh.get_area_prim(e1);
const double Area2 = Mesh.get_area_prim(e2);
for (int k=1; k<=3; k++)
{
xp1.set(k, Mesh.get_node(Mesh.get_tnode(e1,k),1) );
yp1.set(k, Mesh.get_node(Mesh.get_tnode(e1,k),2) );
xp2.set(k, Mesh.get_node(Mesh.get_tnode(e2,k),1) );
yp2.set(k, Mesh.get_node(Mesh.get_tnode(e2,k),2) );
}
const double xc1 = (xp1.get(1) + xp1.get(2) + xp1.get(3))/3.0;
const double yc1 = (yp1.get(1) + yp1.get(2) + yp1.get(3))/3.0;
const double xc2 = (xp2.get(1) + xp2.get(2) + xp2.get(3))/3.0;
const double yc2 = (yp2.get(1) + yp2.get(2) + yp2.get(3))/3.0;
// quadrature points on the edge
for (int m=1; m<=NumQuadPoints; m++)
{
// Take integration point s (in [-1,1])
// and map to physical domain
const double s = EdgeData->GL_xpts1d->get(m);
const double x = x1 + 0.5*(s+1.0)*(x2-x1);
const double y = y1 + 0.5*(s+1.0)*(y2-y1);
// Take physical point (x,y)
// and map into the coordinates
// of the two triangles that are
// adjacent to the current edge
xy1.set(1, ((yp1.get(3)-yp1.get(1))*(x-xc1)
+ (xp1.get(1)-xp1.get(3))*(y-yc1))/(2.0*Area1) );
xy1.set(2, ((yp1.get(1)-yp1.get(2))*(x-xc1)
+ (xp1.get(2)-xp1.get(1))*(y-yc1))/(2.0*Area1) );
xy2.set(1, ((yp2.get(3)-yp2.get(1))*(x-xc2)
+ (xp2.get(1)-xp2.get(3))*(y-yc2))/(2.0*Area2) );
xy2.set(2, ((yp2.get(1)-yp2.get(2))*(x-xc2)
+ (xp2.get(2)-xp2.get(1))*(y-yc2))/(2.0*Area2) );
// Evaluate monomials at locations xy1
double xi = xy1.get(1);
double xi2 = xi*xi;
double xi3 = xi*xi2;
double xi4 = xi*xi3;
double eta = xy1.get(2);
double eta2 = eta*eta;
double eta3 = eta*eta2;
double eta4 = eta*eta3;
switch( NumBasisOrder )
{
case 5: // fifth order
mu1.set(15, eta4 );
mu1.set(14, xi4 );
mu1.set(13, xi2*eta2 );
mu1.set(12, eta3*xi );
mu1.set(11, xi3*eta );
case 4: // fourth order
mu1.set(10, eta3 );
mu1.set(9, xi3 );
mu1.set(8, xi*eta2 );
mu1.set(7, eta*xi2 );
case 3: // third order
mu1.set(6, eta2 );
mu1.set(5, xi2 );
mu1.set(4, xi*eta );
case 2: // second order
mu1.set(3, eta );
mu1.set(2, xi );
case 1: // first order
mu1.set(1, 1.0 );
break;
}
// Evaluate monomials at locations xy2
xi = xy2.get(1);
xi2 = xi*xi;
xi3 = xi*xi2;
xi4 = xi*xi3;
eta = xy2.get(2);
eta2 = eta*eta;
eta3 = eta*eta2;
eta4 = eta*eta3;
switch( NumBasisOrder )
{
case 5: // fifth order
mu2.set(15, eta4 );
mu2.set(14, xi4 );
mu2.set(13, xi2*eta2 );
mu2.set(12, eta3*xi );
mu2.set(11, xi3*eta );
case 4: // fourth order
mu2.set(10, eta3 );
mu2.set(9, xi3 );
mu2.set(8, xi*eta2 );
mu2.set(7, eta*xi2 );
case 3: // third order
mu2.set(6, eta2 );
mu2.set(5, xi2 );
mu2.set(4, xi*eta );
case 2: // second order
mu2.set(3, eta );
mu2.set(2, xi );
case 1: // first order
mu2.set(1, 1.0 );
break;
}
// Finally, convert monomials to Legendre Polys
// on the two adjacent triangle
for (int k=1; k<=NumBasisComps; k++)
{
double tmp1 = 0.0;
double tmp2 = 0.0;
for (int j=1; j<=k; j++)
{
tmp1 = tmp1 + Mmat[k-1][j-1]*mu1.get(j);
tmp2 = tmp2 + Mmat[k-1][j-1]*mu2.get(j);
}
EdgeData->GL_phi_left->set(i,m,k, tmp1 );
EdgeData->GL_phi_right->set(i,m,k, tmp2 );
}
}
}
}