void getKinectData() {
const int dataSize = width*height*3*4;
GLubyte* ptr;
glBindBuffer(GL_ARRAY_BUFFER, vboId);
glBufferData(GL_ARRAY_BUFFER, dataSize, 0, GL_DYNAMIC_DRAW);
ptr = (GLubyte*) glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
if (ptr) {
getDepthData(ptr);
}
glUnmapBuffer(GL_ARRAY_BUFFER);
glBindBuffer(GL_ARRAY_BUFFER, cboId);
glBufferData(GL_ARRAY_BUFFER, dataSize, 0, GL_DYNAMIC_DRAW);
ptr = (GLubyte*) glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
if (ptr) {
getRgbData(ptr);
}
glUnmapBuffer(GL_ARRAY_BUFFER);
getSkeletalData();
}
void drawKinectData() {
glBindTexture(GL_TEXTURE_2D, textureId);
getKinectData(data);
getSkeletalData();
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_BGRA_EXT, GL_UNSIGNED_BYTE, (GLvoid*)data);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glBegin(GL_QUADS);
glColor3f(1.f, 1.f, 1.f);
glTexCoord2f(0.0f, 0.0f);
glVertex3f(0, 0, 0);
glTexCoord2f(1.0f, 0.0f);
glVertex3f(width, 0, 0);
glTexCoord2f(1.0f, 1.0f);
glVertex3f(width, height, 0.0f);
glTexCoord2f(0.0f, 1.0f);
glVertex3f(0, height, 0.0f);
glEnd();
// Torso & Head
const Vector4& hip = skeletonPosition[NUI_SKELETON_POSITION_HIP_CENTER];
const Vector4& spn = skeletonPosition[NUI_SKELETON_POSITION_SPINE];
const Vector4& shc = skeletonPosition[NUI_SKELETON_POSITION_SHOULDER_CENTER];
const Vector4& head = skeletonPosition[NUI_SKELETON_POSITION_HEAD];
// Left Arm etc
const Vector4& lh = skeletonPosition[NUI_SKELETON_POSITION_HAND_LEFT];
const Vector4& le = skeletonPosition[NUI_SKELETON_POSITION_ELBOW_LEFT];
const Vector4& lw = skeletonPosition[NUI_SKELETON_POSITION_WRIST_LEFT];
const Vector4& ls = skeletonPosition[NUI_SKELETON_POSITION_SHOULDER_LEFT];
// Right Arm etc
const Vector4& rh = skeletonPosition[NUI_SKELETON_POSITION_HAND_RIGHT];
const Vector4& re = skeletonPosition[NUI_SKELETON_POSITION_ELBOW_RIGHT];
const Vector4& rw = skeletonPosition[NUI_SKELETON_POSITION_WRIST_RIGHT];
const Vector4& rs = skeletonPosition[NUI_SKELETON_POSITION_SHOULDER_RIGHT];
// Left Leg etc
const Vector4& lhip = skeletonPosition[NUI_SKELETON_POSITION_HIP_LEFT];
const Vector4& lk = skeletonPosition[NUI_SKELETON_POSITION_KNEE_LEFT];
const Vector4& la = skeletonPosition[NUI_SKELETON_POSITION_ANKLE_LEFT];
const Vector4& lf = skeletonPosition[NUI_SKELETON_POSITION_FOOT_LEFT];
// Right Leg etc
const Vector4& rhip = skeletonPosition[NUI_SKELETON_POSITION_HIP_RIGHT];
const Vector4& rk = skeletonPosition[NUI_SKELETON_POSITION_KNEE_RIGHT];
const Vector4& ra = skeletonPosition[NUI_SKELETON_POSITION_ANKLE_RIGHT];
const Vector4& rf = skeletonPosition[NUI_SKELETON_POSITION_FOOT_RIGHT];
glLineWidth(20.0);
glBegin(GL_LINES);
glColor3f(1.f, 0.f, 0.f);
if (testTracked){
// Torso & Head
if (hip.w == 1 && spn.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(hip)[0], SkeletonToScreen(hip)[1]);
glVertex2f(SkeletonToScreen(spn)[0], SkeletonToScreen(spn)[1]);
}
if (spn.w == 1 && shc.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(spn)[0], SkeletonToScreen(spn)[1]);
glVertex2f(SkeletonToScreen(shc)[0], SkeletonToScreen(shc)[1]);
}
if (shc.w == 1 && head.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(shc)[0], SkeletonToScreen(shc)[1]);
glVertex2f(SkeletonToScreen(head)[0], SkeletonToScreen(head)[1]);
}
// Left Arm etc
if (lh.w == 1 && lw.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(lh)[0], SkeletonToScreen(lh)[1]);
glVertex2f(SkeletonToScreen(lw)[0], SkeletonToScreen(lw)[1]);
}
if (lw.w == 1 && le.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(lw)[0], SkeletonToScreen(lw)[1]);
glVertex2f(SkeletonToScreen(le)[0], SkeletonToScreen(le)[1]);
}
if (le.w == 1 && ls.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(le)[0], SkeletonToScreen(le)[1]);
glVertex2f(SkeletonToScreen(ls)[0], SkeletonToScreen(ls)[1]);
}
if (ls.w == 1 && shc.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(ls)[0], SkeletonToScreen(ls)[1]);
glVertex2f(SkeletonToScreen(shc)[0], SkeletonToScreen(shc)[1]);
}
// Right Arm etc
if (rh.w == 1 && rw.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rh)[0], SkeletonToScreen(rh)[1]);
glVertex2f(SkeletonToScreen(rw)[0], SkeletonToScreen(rw)[1]);
}
if (rw.w == 1 && re.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rw)[0], SkeletonToScreen(rw)[1]);
glVertex2f(SkeletonToScreen(re)[0], SkeletonToScreen(re)[1]);
}
if (re.w == 1 && rs.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(re)[0], SkeletonToScreen(re)[1]);
glVertex2f(SkeletonToScreen(rs)[0], SkeletonToScreen(rs)[1]);
}
if (rs.w == 1 && shc.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rs)[0], SkeletonToScreen(rs)[1]);
glVertex2f(SkeletonToScreen(shc)[0], SkeletonToScreen(shc)[1]);
}
// Left Leg etc
if (lf.w == 1 && la.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(lf)[0], SkeletonToScreen(lf)[1]);
glVertex2f(SkeletonToScreen(la)[0], SkeletonToScreen(la)[1]);
}
if (la.w == 1 && lk.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(la)[0], SkeletonToScreen(la)[1]);
glVertex2f(SkeletonToScreen(lk)[0], SkeletonToScreen(lk)[1]);
}
if (lk.w == 1 && lhip.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(lk)[0], SkeletonToScreen(lk)[1]);
glVertex2f(SkeletonToScreen(lhip)[0], SkeletonToScreen(lhip)[1]);
}
if (lhip.w == 1 && hip.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(lhip)[0], SkeletonToScreen(lhip)[1]);
glVertex2f(SkeletonToScreen(hip)[0], SkeletonToScreen(hip)[1]);
}
// Right Leg etc
if (rf.w == 1 && ra.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rf)[0], SkeletonToScreen(rf)[1]);
glVertex2f(SkeletonToScreen(ra)[0], SkeletonToScreen(ra)[1]);
}
if (ra.w == 1 && rk.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(ra)[0], SkeletonToScreen(ra)[1]);
glVertex2f(SkeletonToScreen(rk)[0], SkeletonToScreen(rk)[1]);
}
if (rk.w == 1 && rhip.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rk)[0], SkeletonToScreen(rk)[1]);
glVertex2f(SkeletonToScreen(rhip)[0], SkeletonToScreen(rhip)[1]);
}
if (rhip.w == 1 && hip.w == 1){ // checking to see if both points are tracked
glVertex2f(SkeletonToScreen(rhip)[0], SkeletonToScreen(rhip)[1]);
glVertex2f(SkeletonToScreen(hip)[0], SkeletonToScreen(hip)[1]);
}
}
glEnd();
// Leg angles and their output to screen.
float fVal1 = 0;
float fVal2 = 0;
float facingAngle = 0;
if (testTracked && rhip.w == 1 && lhip.w == 1 && rk.w == 1 && lk.w == 1){ // checking to see if all the needed joints are tracked
Vector3D* UpperLegRotZ = NULL;
Vector3D* UpperLegRotX = NULL;
int screen_position[2];
if (active_leg == 1){ // Left leg is selected. Default value.
UpperLegRotZ = new Vector3D(lk.x - lhip.x, lk.y - lhip.y, 0);
UpperLegRotX = new Vector3D(0, lk.y - lhip.y, lk.z - lhip.z);
screen_position[0] = SkeletonToScreen(lhip)[0];
screen_position[1] = SkeletonToScreen(lhip)[1];
}
else if (active_leg == 2){ // Left leg is selected.
UpperLegRotZ = new Vector3D(rk.x - rhip.x, rk.y - rhip.y, 0);
UpperLegRotX = new Vector3D(0, rk.y - rhip.y, rk.z - rhip.z);
screen_position[0] = SkeletonToScreen(rhip)[0]+20; // 20 pixels moved to center it better above joint
screen_position[1] = SkeletonToScreen(rhip)[1];
}
// Creating vector to measure leg angle against and calculating the angles for rotation around z-achsis and x-achsis.
Vector3D& jointAttachedPendulum = Vector3D(0, -1, 0);
Vector3D& planeNormal = Vector3D(0, 0, 1);
fVal1 = twoVectorAngle(*UpperLegRotZ, jointAttachedPendulum, planeNormal);
planeNormal = Vector3D(-1, 0, 0);
fVal2 = twoVectorAngle(*UpperLegRotX, jointAttachedPendulum, planeNormal);
// Constructing the calculation for a camera facing vector
Vector3D& rightHipPointToLeft = Vector3D(lhip.x - rhip.x, 0, lhip.z - rhip.z);
planeNormal = Vector3D(0, -1, 0);
Vector3D& straightFacing = Vector3D(-1, 0, 0); // must align with rightHipPointToLeftVector for skeleton to be facing straight forward
facingAngle = twoVectorAngle(rightHipPointToLeft, straightFacing, planeNormal);
delete UpperLegRotZ;
UpperLegRotZ = NULL;
delete UpperLegRotX;
UpperLegRotX = NULL;
// Write angle of the rotation around z to screen by converting float to char array and printing to screen above the joints.
glColor3f(0.f, 1.f, 0.f);
glRasterPos2f(screen_position[0] - 30, screen_position[1] - 20);
char cVal[32];
sprintf_s(cVal, "%f", fVal1);
outputText = cVal;
for (int i = 0; i < 5; i++) {
glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, outputText[i]);
}
// New screen position for angle of the rotation around x.
glRasterPos2f(screen_position[0] - 30, screen_position[1] - 40);
sprintf_s(cVal, "%f", fVal2);
outputText = cVal;
for (int i = 0; i < 5; i++) {
glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, outputText[i]);
}
// screen position for facing angle
glRasterPos2f(545, 140);
sprintf_s(cVal, "%f", facingAngle);
outputText = cVal;
for (int i = 0; i < 5; i++) {
glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, outputText[i]);
}
glLineWidth(5.0);
glBegin(GL_LINES);
// blue arrow for facing
glColor3f(0.f, 0.f, 1.f);
glLineWidth(5.0);
// arrow shaft
glVertex2f(570 + sin(facingAngle) * 50, 70 - cos(facingAngle) * 50);
glVertex2f(570 - sin(facingAngle) * 50 , 70 + cos(facingAngle) * 50);
// arrow tip part 1
glVertex2f(570 - sin(facingAngle+0.4) * 30, 70 + cos(facingAngle+0.4) * 30);
glVertex2f(570 - sin(facingAngle) * 50, 70 + cos(facingAngle) * 50);
// arrow tip part 2
glVertex2f(570 - sin(facingAngle - 0.4) * 30, 70 + cos(facingAngle - 0.4) * 30);
glVertex2f(570 - sin(facingAngle) * 50, 70 + cos(facingAngle) * 50);
glEnd();
}
// Nag message about menu appears on screen if menu has not been used yet.
if (menu_used == FALSE){
outputText = "Left click on screen to use menu!";
glColor3f(255, 0, 0);
glRasterPos2f(10, 20);
int len;
len = (int)strlen(outputText);
for (int i = 0; i < len; i++) {
glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, outputText[i]);
}
}
// Guards:
// catch legs that are not fully tracked and bad angles
if (lk.w == 0 || lhip.w == 0){
guard_trap = TRUE;
}
if (fVal1 > 45.29){
guard_trap = TRUE;
}
if (guarded_data == FALSE){
guard_trap = FALSE;
}
// Program actions
if (record_data == TRUE && save_data == FALSE){
appendData(fVal1, fVal2, facingAngle);
}
else if (record_data == TRUE && save_data == TRUE && guard_trap == FALSE){
record_data = FALSE;
save_data = FALSE;
appendData(fVal1, fVal2, facingAngle);
writePickle();
}
else if (record_data == FALSE && save_data == TRUE && data_stream->next != NULL && guard_trap == FALSE){
save_data == FALSE;
writePickle();
}
}
