112 lines
3.2 KiB
C++
112 lines
3.2 KiB
C++
#include "includes.h"
|
|
Vector4 leftLegPosPreview = { 1.5F, 1.5F, 1.5F, 1.5F };
|
|
Vector4 rightLegPosPreview = { 1.5F, 1.5F, 1.5F, 1.5F };
|
|
|
|
int kinectTest(float xGrad, float xOffset, float yGrad, float yOffset, float zGrad, float zOffset) {
|
|
//std::cout << "Kinect Preview\n";
|
|
//std::cin >> xGrad;
|
|
//std::cin >> xOffset;
|
|
// Wait 10 secs for kinect shutdown
|
|
// Sleep(10000);
|
|
// initialize Kinect
|
|
HRESULT hr = NuiInitialize(NUI_INITIALIZE_FLAG_USES_SKELETON);
|
|
if (FAILED(hr)) {
|
|
std::cout << "Failed to initialize Kinect." << std::endl;
|
|
return 1;
|
|
}
|
|
|
|
// open the skeleton stream
|
|
HANDLE skeletonStream = nullptr;
|
|
hr = NuiSkeletonTrackingEnable(nullptr, 0);
|
|
if (FAILED(hr)) {
|
|
std::cout << "Failed to open the skeleton stream." << std::endl;
|
|
NuiShutdown();
|
|
return 1;
|
|
}
|
|
|
|
// main loop to read and process skeleton data
|
|
NUI_SKELETON_FRAME skeletonFrame = { 0 };
|
|
CONSOLE_SCREEN_BUFFER_INFO csbi;
|
|
int width = 0;
|
|
float drsLeft = 0;
|
|
float drsRight = 0;
|
|
int toolLeft = 0;
|
|
int toolRight = 0;
|
|
int toolWidth = 0;
|
|
bool leftTouch = false;
|
|
bool rightTouch = false;
|
|
|
|
while (true) {
|
|
// get the latest skeleton frame
|
|
hr = NuiSkeletonGetNextFrame(0, &skeletonFrame);
|
|
if (FAILED(hr)) {
|
|
continue;
|
|
}
|
|
|
|
// Process each tracked skeleton
|
|
for (int i = 0; i < NUI_SKELETON_COUNT; ++i) {
|
|
if (skeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED) {
|
|
// get the position of both legs
|
|
leftLegPosPreview = skeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_ANKLE_LEFT];
|
|
rightLegPosPreview = skeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_ANKLE_RIGHT];
|
|
}
|
|
}
|
|
|
|
// keep updating screen width for fancy
|
|
GetConsoleScreenBufferInfo(GetStdHandle(STD_OUTPUT_HANDLE), &csbi);
|
|
width = (int)(csbi.srWindow.Right - csbi.srWindow.Left + 1);
|
|
|
|
// fetch calibrated feet
|
|
drsLeft = xGrad * leftLegPosPreview.x + xOffset;
|
|
drsRight = xGrad * rightLegPosPreview.x + xOffset;
|
|
|
|
// translate to console window width
|
|
toolLeft = width * drsLeft;
|
|
toolRight = width * drsRight;
|
|
toolWidth = width * 0.05;
|
|
|
|
// foot lifting detection
|
|
// see how far foot currently is from known ground value
|
|
float fixedLeft = leftLegPosPreview.y - (yGrad * leftLegPosPreview.z + yOffset);
|
|
float fixedRight = rightLegPosPreview.y - (yGrad * rightLegPosPreview.z + yOffset);
|
|
|
|
// check for stepping
|
|
float errorMargin = 0.05;
|
|
|
|
if (fixedLeft > (fixedRight + errorMargin)) {
|
|
rightTouch = true;
|
|
leftTouch = false;
|
|
// std::cout << "right step\n";
|
|
}
|
|
else if (fixedRight > (fixedLeft + errorMargin)) {
|
|
leftTouch = true;
|
|
rightTouch = false;
|
|
// std::cout << "left step \n";
|
|
}
|
|
else {
|
|
leftTouch = true;
|
|
rightTouch = true;
|
|
// std::cout << "both step\n";
|
|
}
|
|
|
|
// print feet
|
|
for (int i = 0; i < width; i++) {
|
|
if ((i <= (toolLeft + toolWidth)) && (i >= (toolLeft - toolWidth)) && leftTouch) {
|
|
std::cout << "L";
|
|
}
|
|
else if ((i <= (toolRight + toolWidth)) && (i >= (toolRight - toolWidth)) && rightTouch) {
|
|
std::cout << "R";
|
|
}
|
|
else {
|
|
std::cout << " ";
|
|
}
|
|
}
|
|
|
|
// std::cout << std::to_string(fixedLeft) << " " << std::to_string(fixedRight) << std::endl;
|
|
}
|
|
|
|
// Clean up and exit
|
|
NuiSkeletonTrackingDisable();
|
|
NuiShutdown();
|
|
return 0;
|
|
} |