diff --git a/.gitignore b/.gitignore index 33de78e..ee08d32 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,7 @@ .vs/ x64/ -depthrush/x64/ Release/ -depthrush/Release/ \ No newline at end of file +depthrush/x64/ +depthrush/Release/ +depthrushConfig/x64/ +depthrushConfig/Release/ \ No newline at end of file diff --git a/README.md b/README.md index 5e4b2ab..5274c9c 100644 --- a/README.md +++ b/README.md @@ -3,4 +3,5 @@ Work in progress Kinect driver for Dancerush Stardom. ## Installation 1. Patch SpiceTools to disable all DRS pad hooks. -2. Copy `depthrush.dll` to the same folder as spice64 and use spicecfg to inject depthrush. +2. Copy all three of the files from to the same folder as spice64 and use spicecfg to inject `depthrush.dll`. +3. **IMPORTANT:** Open `depthrushConfig.exe` and calibrate your Kinect environment! Otherwise it won't work well with the game. diff --git a/depthrush.sln b/depthrush.sln index 9cbc62f..10a9a64 100644 --- a/depthrush.sln +++ b/depthrush.sln @@ -5,6 +5,8 @@ VisualStudioVersion = 17.2.32616.157 MinimumVisualStudioVersion = 10.0.40219.1 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "depthrush", "depthrush\depthrush.vcxproj", "{04B17470-CB82-4724-904B-25445926AB86}" EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "depthrushConfig", "depthrushConfig\depthrushConfig.vcxproj", "{AF89F70A-7F60-4B1C-9532-CE7BBD02EA56}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -21,6 +23,14 @@ Global {04B17470-CB82-4724-904B-25445926AB86}.Release|x64.Build.0 = Release|x64 {04B17470-CB82-4724-904B-25445926AB86}.Release|x86.ActiveCfg = Release|Win32 {04B17470-CB82-4724-904B-25445926AB86}.Release|x86.Build.0 = Release|Win32 + {AF89F70A-7F60-4B1C-9532-CE7BBD02EA56}.Debug|x64.ActiveCfg = Debug|x64 + {AF89F70A-7F60-4B1C-9532-CE7BBD02EA56}.Debug|x64.Build.0 = Debug|x64 + {AF89F70A-7F60-4B1C-9532-CE7BBD02EA56}.Debug|x86.ActiveCfg = Debug|Win32 + {AF89F70A-7F60-4B1C-9532-CE7BBD02EA56}.Debug|x86.Build.0 = Debug|Win32 + {AF89F70A-7F60-4B1C-9532-CE7BBD02EA56}.Release|x64.ActiveCfg = Release|x64 + {AF89F70A-7F60-4B1C-9532-CE7BBD02EA56}.Release|x64.Build.0 = Release|x64 + {AF89F70A-7F60-4B1C-9532-CE7BBD02EA56}.Release|x86.ActiveCfg = Release|Win32 + {AF89F70A-7F60-4B1C-9532-CE7BBD02EA56}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/depthrush/depthrush.vcxproj b/depthrush/depthrush.vcxproj index af97827..9348b74 100644 --- a/depthrush/depthrush.vcxproj +++ b/depthrush/depthrush.vcxproj @@ -171,6 +171,7 @@ + diff --git a/depthrush/depthrush.vcxproj.filters b/depthrush/depthrush.vcxproj.filters index e55c07d..6324a0e 100644 --- a/depthrush/depthrush.vcxproj.filters +++ b/depthrush/depthrush.vcxproj.filters @@ -103,6 +103,9 @@ Header Files + + Header Files + diff --git a/depthrush/drs.cpp b/depthrush/drs.cpp index 4709ac1..ed17e85 100644 --- a/depthrush/drs.cpp +++ b/depthrush/drs.cpp @@ -228,9 +228,27 @@ void pollKinect() { return 1; } + // load config from ini + std::cout << "Loading depthrush.ini\n"; + mINI::INIFile file("depthrush.ini"); + mINI::INIStructure ini; + file.read(ini); + std::string& readValue = ini["calibration"]["xGrad"]; + float xGrad = std::stof(readValue); + readValue = ini["calibration"]["xOffset"]; + float xOffset = std::stof(readValue); + readValue = ini["calibration"]["yGrad"]; + float yGrad = std::stof(readValue); + readValue = ini["calibration"]["yOffset"]; + float yOffset = std::stof(readValue); + readValue = ini["calibration"]["zGrad"]; + float zGrad = std::stof(readValue); + readValue = ini["calibration"]["zOffset"]; + float zOffset = std::stof(readValue); + // main loop to read and process skeleton data NUI_SKELETON_FRAME skeletonFrame = { 0 }; - float errorMargin = 0.02; + float errorMargin = 0.03; while (true) { // get the latest skeleton frame hr = NuiSkeletonGetNextFrame(0, &skeletonFrame); @@ -249,24 +267,30 @@ void pollKinect() { //std::cout << "Left Leg: X = " << leftLegPos.x << ", Y = " << leftLegPos.y << ", Z = " << leftLegPos.z << std::endl; //std::cout << "Right Leg: X = " << rightLegPos.x << ", Y = " << rightLegPos.y << ", Z = " << rightLegPos.z << std::endl; - feet[1].event.x = leftLegPos.x; + feet[1].event.x = xGrad * leftLegPos.x + xOffset; feet[1].event.y = 0.5; - feet[2].event.x = rightLegPos.x; + feet[2].event.x = xGrad * rightLegPos.x + xOffset; feet[2].event.y = 0.5; + // Fix feet height + float fixedLeft = leftLegPos.y - (yGrad * leftLegPos.z + yOffset); + float fixedRight = rightLegPos.y - (yGrad * rightLegPos.z + yOffset); + // check for stepping - if (leftLegPos.y > (rightLegPos.y + errorMargin)) { + if (fixedLeft > (fixedRight + errorMargin)) { feet[2].touching = true; feet[1].touching = false; - + // std::cout << "right step\n"; } - else if (rightLegPos.y > (leftLegPos.y + errorMargin)) { + else if (fixedRight > (fixedLeft + errorMargin)) { feet[1].touching = true; feet[2].touching = false; + // std::cout << "left step \n"; } else { feet[1].touching = true; feet[2].touching = true; + // std::cout << "both step\n"; } } diff --git a/depthrush/includes.h b/depthrush/includes.h index 94fb7b9..63d8c55 100644 --- a/depthrush/includes.h +++ b/depthrush/includes.h @@ -9,6 +9,7 @@ #include "kiero/minhook/include/MinHook.h" #include "kiero/injector/injector.hpp" #include "d3d9.h" +#include "ini.h" #include "drs.h" typedef LRESULT(CALLBACK* WNDPROC)(HWND, UINT, WPARAM, LPARAM); diff --git a/depthrush/ini.h b/depthrush/ini.h new file mode 100644 index 0000000..cb0b801 --- /dev/null +++ b/depthrush/ini.h @@ -0,0 +1,789 @@ +/* + * The MIT License (MIT) + * Copyright (c) 2018 Danijel Durakovic + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + * of the Software, and to permit persons to whom the Software is furnished to do + * so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + */ + + /////////////////////////////////////////////////////////////////////////////// + // + // /mINI/ v0.9.14 + // An INI file reader and writer for the modern age. + // + /////////////////////////////////////////////////////////////////////////////// + // + // A tiny utility library for manipulating INI files with a straightforward + // API and a minimal footprint. It conforms to the (somewhat) standard INI + // format - sections and keys are case insensitive and all leading and + // trailing whitespace is ignored. Comments are lines that begin with a + // semicolon. Trailing comments are allowed on section lines. + // + // Files are read on demand, upon which data is kept in memory and the file + // is closed. This utility supports lazy writing, which only writes changes + // and updates to a file and preserves custom formatting and comments. A lazy + // write invoked by a write() call will read the output file, find what + // changes have been made and update the file accordingly. If you only need to + // generate files, use generate() instead. Section and key order is preserved + // on read, write and insert. + // + /////////////////////////////////////////////////////////////////////////////// + // + // /* BASIC USAGE EXAMPLE: */ + // + // /* read from file */ + // mINI::INIFile file("myfile.ini"); + // mINI::INIStructure ini; + // file.read(ini); + // + // /* read value; gets a reference to actual value in the structure. + // if key or section don't exist, a new empty value will be created */ + // std::string& value = ini["section"]["key"]; + // + // /* read value safely; gets a copy of value in the structure. + // does not alter the structure */ + // std::string value = ini.get("section").get("key"); + // + // /* set or update values */ + // ini["section"]["key"] = "value"; + // + // /* set multiple values */ + // ini["section2"].set({ + // {"key1", "value1"}, + // {"key2", "value2"} + // }); + // + // /* write updates back to file, preserving comments and formatting */ + // file.write(ini); + // + // /* or generate a file (overwrites the original) */ + // file.generate(ini); + // + /////////////////////////////////////////////////////////////////////////////// + // + // Long live the INI file!!! + // + /////////////////////////////////////////////////////////////////////////////// + +#ifndef MINI_INI_H_ +#define MINI_INI_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mINI +{ + namespace INIStringUtil + { + const char* const whitespaceDelimiters = " \t\n\r\f\v"; + inline void trim(std::string& str) + { + str.erase(str.find_last_not_of(whitespaceDelimiters) + 1); + str.erase(0, str.find_first_not_of(whitespaceDelimiters)); + } +#ifndef MINI_CASE_SENSITIVE + inline void toLower(std::string& str) + { + std::transform(str.begin(), str.end(), str.begin(), [](const char c) { + return static_cast(std::tolower(c)); + }); + } +#endif + inline void replace(std::string& str, std::string const& a, std::string const& b) + { + if (!a.empty()) + { + std::size_t pos = 0; + while ((pos = str.find(a, pos)) != std::string::npos) + { + str.replace(pos, a.size(), b); + pos += b.size(); + } + } + } +#ifdef _WIN32 + const char* const endl = "\r\n"; +#else + const char* const endl = "\n"; +#endif + } + + template + class INIMap + { + private: + using T_DataIndexMap = std::unordered_map; + using T_DataItem = std::pair; + using T_DataContainer = std::vector; + using T_MultiArgs = typename std::vector>; + + T_DataIndexMap dataIndexMap; + T_DataContainer data; + + inline std::size_t setEmpty(std::string& key) + { + std::size_t index = data.size(); + dataIndexMap[key] = index; + data.emplace_back(key, T()); + return index; + } + + public: + using const_iterator = typename T_DataContainer::const_iterator; + + INIMap() { } + + INIMap(INIMap const& other) + { + std::size_t data_size = other.data.size(); + for (std::size_t i = 0; i < data_size; ++i) + { + auto const& key = other.data[i].first; + auto const& obj = other.data[i].second; + data.emplace_back(key, obj); + } + dataIndexMap = T_DataIndexMap(other.dataIndexMap); + } + + T& operator[](std::string key) + { + INIStringUtil::trim(key); +#ifndef MINI_CASE_SENSITIVE + INIStringUtil::toLower(key); +#endif + auto it = dataIndexMap.find(key); + bool hasIt = (it != dataIndexMap.end()); + std::size_t index = (hasIt) ? it->second : setEmpty(key); + return data[index].second; + } + T get(std::string key) const + { + INIStringUtil::trim(key); +#ifndef MINI_CASE_SENSITIVE + INIStringUtil::toLower(key); +#endif + auto it = dataIndexMap.find(key); + if (it == dataIndexMap.end()) + { + return T(); + } + return T(data[it->second].second); + } + bool has(std::string key) const + { + INIStringUtil::trim(key); +#ifndef MINI_CASE_SENSITIVE + INIStringUtil::toLower(key); +#endif + return (dataIndexMap.count(key) == 1); + } + void set(std::string key, T obj) + { + INIStringUtil::trim(key); +#ifndef MINI_CASE_SENSITIVE + INIStringUtil::toLower(key); +#endif + auto it = dataIndexMap.find(key); + if (it != dataIndexMap.end()) + { + data[it->second].second = obj; + } + else + { + dataIndexMap[key] = data.size(); + data.emplace_back(key, obj); + } + } + void set(T_MultiArgs const& multiArgs) + { + for (auto const& it : multiArgs) + { + auto const& key = it.first; + auto const& obj = it.second; + set(key, obj); + } + } + bool remove(std::string key) + { + INIStringUtil::trim(key); +#ifndef MINI_CASE_SENSITIVE + INIStringUtil::toLower(key); +#endif + auto it = dataIndexMap.find(key); + if (it != dataIndexMap.end()) + { + std::size_t index = it->second; + data.erase(data.begin() + index); + dataIndexMap.erase(it); + for (auto& it2 : dataIndexMap) + { + auto& vi = it2.second; + if (vi > index) + { + vi--; + } + } + return true; + } + return false; + } + void clear() + { + data.clear(); + dataIndexMap.clear(); + } + std::size_t size() const + { + return data.size(); + } + const_iterator begin() const { return data.begin(); } + const_iterator end() const { return data.end(); } + }; + + using INIStructure = INIMap>; + + namespace INIParser + { + using T_ParseValues = std::pair; + + enum class PDataType : char + { + PDATA_NONE, + PDATA_COMMENT, + PDATA_SECTION, + PDATA_KEYVALUE, + PDATA_UNKNOWN + }; + + inline PDataType parseLine(std::string line, T_ParseValues& parseData) + { + parseData.first.clear(); + parseData.second.clear(); + INIStringUtil::trim(line); + if (line.empty()) + { + return PDataType::PDATA_NONE; + } + char firstCharacter = line[0]; + if (firstCharacter == ';') + { + return PDataType::PDATA_COMMENT; + } + if (firstCharacter == '[') + { + auto commentAt = line.find_first_of(';'); + if (commentAt != std::string::npos) + { + line = line.substr(0, commentAt); + } + auto closingBracketAt = line.find_last_of(']'); + if (closingBracketAt != std::string::npos) + { + auto section = line.substr(1, closingBracketAt - 1); + INIStringUtil::trim(section); + parseData.first = section; + return PDataType::PDATA_SECTION; + } + } + auto lineNorm = line; + INIStringUtil::replace(lineNorm, "\\=", " "); + auto equalsAt = lineNorm.find_first_of('='); + if (equalsAt != std::string::npos) + { + auto key = line.substr(0, equalsAt); + INIStringUtil::trim(key); + INIStringUtil::replace(key, "\\=", "="); + auto value = line.substr(equalsAt + 1); + INIStringUtil::trim(value); + parseData.first = key; + parseData.second = value; + return PDataType::PDATA_KEYVALUE; + } + return PDataType::PDATA_UNKNOWN; + } + } + + class INIReader + { + public: + using T_LineData = std::vector; + using T_LineDataPtr = std::shared_ptr; + + bool isBOM = false; + + private: + std::ifstream fileReadStream; + T_LineDataPtr lineData; + + T_LineData readFile() + { + fileReadStream.seekg(0, std::ios::end); + const std::size_t fileSize = static_cast(fileReadStream.tellg()); + fileReadStream.seekg(0, std::ios::beg); + if (fileSize >= 3) { + const char header[3] = { + static_cast(fileReadStream.get()), + static_cast(fileReadStream.get()), + static_cast(fileReadStream.get()) + }; + isBOM = ( + header[0] == static_cast(0xEF) && + header[1] == static_cast(0xBB) && + header[2] == static_cast(0xBF) + ); + } + else { + isBOM = false; + } + std::string fileContents; + fileContents.resize(fileSize); + fileReadStream.seekg(isBOM ? 3 : 0, std::ios::beg); + fileReadStream.read(&fileContents[0], fileSize); + fileReadStream.close(); + T_LineData output; + if (fileSize == 0) + { + return output; + } + std::string buffer; + buffer.reserve(50); + for (std::size_t i = 0; i < fileSize; ++i) + { + char& c = fileContents[i]; + if (c == '\n') + { + output.emplace_back(buffer); + buffer.clear(); + continue; + } + if (c != '\0' && c != '\r') + { + buffer += c; + } + } + output.emplace_back(buffer); + return output; + } + + public: + INIReader(std::string const& filename, bool keepLineData = false) + { + fileReadStream.open(filename, std::ios::in | std::ios::binary); + if (keepLineData) + { + lineData = std::make_shared(); + } + } + ~INIReader() { } + + bool operator>>(INIStructure& data) + { + if (!fileReadStream.is_open()) + { + return false; + } + T_LineData fileLines = readFile(); + std::string section; + bool inSection = false; + INIParser::T_ParseValues parseData; + for (auto const& line : fileLines) + { + auto parseResult = INIParser::parseLine(line, parseData); + if (parseResult == INIParser::PDataType::PDATA_SECTION) + { + inSection = true; + data[section = parseData.first]; + } + else if (inSection && parseResult == INIParser::PDataType::PDATA_KEYVALUE) + { + auto const& key = parseData.first; + auto const& value = parseData.second; + data[section][key] = value; + } + if (lineData && parseResult != INIParser::PDataType::PDATA_UNKNOWN) + { + if (parseResult == INIParser::PDataType::PDATA_KEYVALUE && !inSection) + { + continue; + } + lineData->emplace_back(line); + } + } + return true; + } + T_LineDataPtr getLines() + { + return lineData; + } + }; + + class INIGenerator + { + private: + std::ofstream fileWriteStream; + + public: + bool prettyPrint = false; + + INIGenerator(std::string const& filename) + { + fileWriteStream.open(filename, std::ios::out | std::ios::binary); + } + ~INIGenerator() { } + + bool operator<<(INIStructure const& data) + { + if (!fileWriteStream.is_open()) + { + return false; + } + if (!data.size()) + { + return true; + } + auto it = data.begin(); + for (;;) + { + auto const& section = it->first; + auto const& collection = it->second; + fileWriteStream + << "[" + << section + << "]"; + if (collection.size()) + { + fileWriteStream << INIStringUtil::endl; + auto it2 = collection.begin(); + for (;;) + { + auto key = it2->first; + INIStringUtil::replace(key, "=", "\\="); + auto value = it2->second; + INIStringUtil::trim(value); + fileWriteStream + << key + << ((prettyPrint) ? " = " : "=") + << value; + if (++it2 == collection.end()) + { + break; + } + fileWriteStream << INIStringUtil::endl; + } + } + if (++it == data.end()) + { + break; + } + fileWriteStream << INIStringUtil::endl; + if (prettyPrint) + { + fileWriteStream << INIStringUtil::endl; + } + } + return true; + } + }; + + class INIWriter + { + private: + using T_LineData = std::vector; + using T_LineDataPtr = std::shared_ptr; + + std::string filename; + + T_LineData getLazyOutput(T_LineDataPtr const& lineData, INIStructure& data, INIStructure& original) + { + T_LineData output; + INIParser::T_ParseValues parseData; + std::string sectionCurrent; + bool parsingSection = false; + bool continueToNextSection = false; + bool discardNextEmpty = false; + bool writeNewKeys = false; + std::size_t lastKeyLine = 0; + for (auto line = lineData->begin(); line != lineData->end(); ++line) + { + if (!writeNewKeys) + { + auto parseResult = INIParser::parseLine(*line, parseData); + if (parseResult == INIParser::PDataType::PDATA_SECTION) + { + if (parsingSection) + { + writeNewKeys = true; + parsingSection = false; + --line; + continue; + } + sectionCurrent = parseData.first; + if (data.has(sectionCurrent)) + { + parsingSection = true; + continueToNextSection = false; + discardNextEmpty = false; + output.emplace_back(*line); + lastKeyLine = output.size(); + } + else + { + continueToNextSection = true; + discardNextEmpty = true; + continue; + } + } + else if (parseResult == INIParser::PDataType::PDATA_KEYVALUE) + { + if (continueToNextSection) + { + continue; + } + if (data.has(sectionCurrent)) + { + auto& collection = data[sectionCurrent]; + auto const& key = parseData.first; + auto const& value = parseData.second; + if (collection.has(key)) + { + auto outputValue = collection[key]; + if (value == outputValue) + { + output.emplace_back(*line); + } + else + { + INIStringUtil::trim(outputValue); + auto lineNorm = *line; + INIStringUtil::replace(lineNorm, "\\=", " "); + auto equalsAt = lineNorm.find_first_of('='); + auto valueAt = lineNorm.find_first_not_of( + INIStringUtil::whitespaceDelimiters, + equalsAt + 1 + ); + std::string outputLine = line->substr(0, valueAt); + if (prettyPrint && equalsAt + 1 == valueAt) + { + outputLine += " "; + } + outputLine += outputValue; + output.emplace_back(outputLine); + } + lastKeyLine = output.size(); + } + } + } + else + { + if (discardNextEmpty && line->empty()) + { + discardNextEmpty = false; + } + else if (parseResult != INIParser::PDataType::PDATA_UNKNOWN) + { + output.emplace_back(*line); + } + } + } + if (writeNewKeys || std::next(line) == lineData->end()) + { + T_LineData linesToAdd; + if (data.has(sectionCurrent) && original.has(sectionCurrent)) + { + auto const& collection = data[sectionCurrent]; + auto const& collectionOriginal = original[sectionCurrent]; + for (auto const& it : collection) + { + auto key = it.first; + if (collectionOriginal.has(key)) + { + continue; + } + auto value = it.second; + INIStringUtil::replace(key, "=", "\\="); + INIStringUtil::trim(value); + linesToAdd.emplace_back( + key + ((prettyPrint) ? " = " : "=") + value + ); + } + } + if (!linesToAdd.empty()) + { + output.insert( + output.begin() + lastKeyLine, + linesToAdd.begin(), + linesToAdd.end() + ); + } + if (writeNewKeys) + { + writeNewKeys = false; + --line; + } + } + } + for (auto const& it : data) + { + auto const& section = it.first; + if (original.has(section)) + { + continue; + } + if (prettyPrint && output.size() > 0 && !output.back().empty()) + { + output.emplace_back(); + } + output.emplace_back("[" + section + "]"); + auto const& collection = it.second; + for (auto const& it2 : collection) + { + auto key = it2.first; + auto value = it2.second; + INIStringUtil::replace(key, "=", "\\="); + INIStringUtil::trim(value); + output.emplace_back( + key + ((prettyPrint) ? " = " : "=") + value + ); + } + } + return output; + } + + public: + bool prettyPrint = false; + + INIWriter(std::string const& filename) + : filename(filename) + { + } + ~INIWriter() { } + + bool operator<<(INIStructure& data) + { + struct stat buf; + bool fileExists = (stat(filename.c_str(), &buf) == 0); + if (!fileExists) + { + INIGenerator generator(filename); + generator.prettyPrint = prettyPrint; + return generator << data; + } + INIStructure originalData; + T_LineDataPtr lineData; + bool readSuccess = false; + bool fileIsBOM = false; + { + INIReader reader(filename, true); + if ((readSuccess = reader >> originalData)) + { + lineData = reader.getLines(); + fileIsBOM = reader.isBOM; + } + } + if (!readSuccess) + { + return false; + } + T_LineData output = getLazyOutput(lineData, data, originalData); + std::ofstream fileWriteStream(filename, std::ios::out | std::ios::binary); + if (fileWriteStream.is_open()) + { + if (fileIsBOM) { + const char utf8_BOM[3] = { + static_cast(0xEF), + static_cast(0xBB), + static_cast(0xBF) + }; + fileWriteStream.write(utf8_BOM, 3); + } + if (output.size()) + { + auto line = output.begin(); + for (;;) + { + fileWriteStream << *line; + if (++line == output.end()) + { + break; + } + fileWriteStream << INIStringUtil::endl; + } + } + return true; + } + return false; + } + }; + + class INIFile + { + private: + std::string filename; + + public: + INIFile(std::string const& filename) + : filename(filename) + { } + + ~INIFile() { } + + bool read(INIStructure& data) const + { + if (data.size()) + { + data.clear(); + } + if (filename.empty()) + { + return false; + } + INIReader reader(filename); + return reader >> data; + } + bool generate(INIStructure const& data, bool pretty = false) const + { + if (filename.empty()) + { + return false; + } + INIGenerator generator(filename); + generator.prettyPrint = pretty; + return generator << data; + } + bool write(INIStructure& data, bool pretty = false) const + { + if (filename.empty()) + { + return false; + } + INIWriter writer(filename); + writer.prettyPrint = pretty; + return writer << data; + } + }; +} + +#endif // MINI_INI_H_ \ No newline at end of file diff --git a/depthrushConfig/calibration.cpp b/depthrushConfig/calibration.cpp new file mode 100644 index 0000000..7caaa47 --- /dev/null +++ b/depthrushConfig/calibration.cpp @@ -0,0 +1,114 @@ +#include "includes.h" +Vector4 leftLegPos = { 1.5F, 1.5F, 1.5F, 1.5F }; +Vector4 rightLegPos = { 1.5F, 1.5F, 1.5F, 1.5F }; +bool kinectScanning = true; + +int calibration() { + + // 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; + } + + // start calibration application + std::thread calibrateMenu([] { + std::cout << "Starting calibration... Make sure your kinect is pointed straight ahead at you!\n"; + + // X calibration + std::cout << "Please place your left foot on the left side of the pad for 5 seconds.\n"; + Sleep(5000); + float xMin = leftLegPos.x; + std::cout << "Left side recorded.\n"; + std::cout << "Please place your right foot on the right side of the pad for 5 seconds.\n"; + Sleep(5000); + float xMax = rightLegPos.x; + std::cout << "Right side recorded.\n"; + float xGrad = (1 - 0) / (xMax - xMin); + float xOffset = -(xGrad * xMin); + + // Y and Z calibration + + // Z calibration + std::cout << "Please place your left foot at the front of the pad for 5 seconds.\n"; + Sleep(5000); + float zMin = leftLegPos.z; + float yMin = leftLegPos.y; + std::cout << "Front recorded.\n"; + std::cout << "Please place your left foot at the back of the pad for 5 seconds.\n"; + Sleep(5000); + float zMax = leftLegPos.z; + float yMax = leftLegPos.y; + std::cout << "Back recorded.\n"; + float zGrad = (1 - 0) / (zMax - zMin); + float zOffset = -(zGrad * zMin); + + //Y calibration + float yGrad = (yMax - yMin) / (zMax - zMin); + float yOffset = (yMin - yGrad * zMin); + + + // Clean up and proceed to kinect test + kinectScanning = false; + std::cout << "Saving the following values to depthrush.ini...\n"; + std::cout << std::to_string(xGrad) << " "; + std::cout << std::to_string(xOffset) << " "; + std::cout << std::to_string(yGrad) << " "; + std::cout << std::to_string(yOffset) << " "; + std::cout << std::to_string(zGrad) << " "; + std::cout << std::to_string(zOffset) << "\n"; + + // Save values + mINI::INIFile file("depthrush.ini"); + mINI::INIStructure ini; + ini["calibration"]["xGrad"] = std::to_string(xGrad); + ini["calibration"]["xOffset"] = std::to_string(xOffset); + ini["calibration"]["yGrad"] = std::to_string(yGrad); + ini["calibration"]["yOffset"] = std::to_string(yOffset); + ini["calibration"]["zGrad"] = std::to_string(zGrad); + ini["calibration"]["zOffset"] = std::to_string(zOffset); + file.generate(ini); + std::cout << "Save complete.\n"; + + std::cout << "Proceeding to Kinect Preview..\n"; + kinectTest(xGrad, xOffset, yGrad, yOffset, zGrad, zOffset); + }); + + + calibrateMenu.detach(); + + // main loop to read and process skeleton data + NUI_SKELETON_FRAME skeletonFrame = { 0 }; + while (kinectScanning) { + // 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 + leftLegPos = skeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_ANKLE_LEFT]; + rightLegPos = skeletonFrame.SkeletonData[i].SkeletonPositions[NUI_SKELETON_POSITION_ANKLE_RIGHT]; + } + } + } + + // Clean up and exit + NuiSkeletonTrackingDisable(); + NuiShutdown(); + std::this_thread::sleep_for(std::chrono::hours(1000)); + return 0; +} \ No newline at end of file diff --git a/depthrushConfig/calibration.h b/depthrushConfig/calibration.h new file mode 100644 index 0000000..31423ba --- /dev/null +++ b/depthrushConfig/calibration.h @@ -0,0 +1,2 @@ +#pragma once +int calibration(); \ No newline at end of file diff --git a/depthrushConfig/depthrushConfig.cpp b/depthrushConfig/depthrushConfig.cpp new file mode 100644 index 0000000..15b9474 --- /dev/null +++ b/depthrushConfig/depthrushConfig.cpp @@ -0,0 +1,40 @@ +#include "includes.h" + +int main() +{ + // load config from ini + mINI::INIFile file("depthrush.ini"); + mINI::INIStructure ini; + file.read(ini); + std::string& readValue = ini["calibration"]["xGrad"]; + float xGrad = std::stof(readValue); + readValue = ini["calibration"]["xOffset"]; + float xOffset = std::stof(readValue); + readValue = ini["calibration"]["yGrad"]; + float yGrad = std::stof(readValue); + readValue = ini["calibration"]["yOffset"]; + float yOffset = std::stof(readValue); + readValue = ini["calibration"]["zGrad"]; + float zGrad = std::stof(readValue); + readValue = ini["calibration"]["zOffset"]; + float zOffset = std::stof(readValue); + + int choice = 0; + std::cout << "Depthrush test application\n"; + std::cout << "1. Calibrate Kinect\n"; + std::cout << "2. Preview Kinect\n"; + std::cout << "Enter a choice (1,2): "; + std::cin >> choice; + if (choice == 1) { + calibration(); + return 0; + } + else if (choice == 2) { + kinectTest(xGrad, xOffset, yGrad, yOffset, zGrad, zOffset); + return 0; + } + else { + std::cout << "Invalid option!"; + } + return 0; +} \ No newline at end of file diff --git a/depthrushConfig/depthrushConfig.vcxproj b/depthrushConfig/depthrushConfig.vcxproj new file mode 100644 index 0000000..0116fe4 --- /dev/null +++ b/depthrushConfig/depthrushConfig.vcxproj @@ -0,0 +1,148 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {af89f70a-7f60-4b1c-9532-ce7bbd02ea56} + depthrushConfig + 10.0 + + + + Application + true + v143 + Unicode + + + Application + false + v143 + true + Unicode + + + Application + true + v143 + Unicode + + + Application + false + v143 + true + Unicode + + + + + + + + + + + + + + + + + + + + + C:\Program Files\Microsoft SDKs\Kinect\v1.7\inc;$(IncludePath) + C:\Program Files\Microsoft SDKs\Kinect\v1.7\lib\amd64;$(LibraryPath) + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + Kinect10.lib;%(AdditionalDependencies) + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/depthrushConfig/depthrushConfig.vcxproj.filters b/depthrushConfig/depthrushConfig.vcxproj.filters new file mode 100644 index 0000000..350adad --- /dev/null +++ b/depthrushConfig/depthrushConfig.vcxproj.filters @@ -0,0 +1,42 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Source Files + + + Source Files + + + Source Files + + + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + \ No newline at end of file diff --git a/depthrushConfig/depthrushConfig.vcxproj.user b/depthrushConfig/depthrushConfig.vcxproj.user new file mode 100644 index 0000000..88a5509 --- /dev/null +++ b/depthrushConfig/depthrushConfig.vcxproj.user @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/depthrushConfig/includes.h b/depthrushConfig/includes.h new file mode 100644 index 0000000..554b0d2 --- /dev/null +++ b/depthrushConfig/includes.h @@ -0,0 +1,11 @@ +#pragma once +#include +#include +#include +#include +#include +#include "ini.h" +#include "calibration.h" +#include "kinectTest.h" + + diff --git a/depthrushConfig/ini.h b/depthrushConfig/ini.h new file mode 100644 index 0000000..cb0b801 --- /dev/null +++ b/depthrushConfig/ini.h @@ -0,0 +1,789 @@ +/* + * The MIT License (MIT) + * Copyright (c) 2018 Danijel Durakovic + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + * of the Software, and to permit persons to whom the Software is furnished to do + * so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + */ + + /////////////////////////////////////////////////////////////////////////////// + // + // /mINI/ v0.9.14 + // An INI file reader and writer for the modern age. + // + /////////////////////////////////////////////////////////////////////////////// + // + // A tiny utility library for manipulating INI files with a straightforward + // API and a minimal footprint. It conforms to the (somewhat) standard INI + // format - sections and keys are case insensitive and all leading and + // trailing whitespace is ignored. Comments are lines that begin with a + // semicolon. Trailing comments are allowed on section lines. + // + // Files are read on demand, upon which data is kept in memory and the file + // is closed. This utility supports lazy writing, which only writes changes + // and updates to a file and preserves custom formatting and comments. A lazy + // write invoked by a write() call will read the output file, find what + // changes have been made and update the file accordingly. If you only need to + // generate files, use generate() instead. Section and key order is preserved + // on read, write and insert. + // + /////////////////////////////////////////////////////////////////////////////// + // + // /* BASIC USAGE EXAMPLE: */ + // + // /* read from file */ + // mINI::INIFile file("myfile.ini"); + // mINI::INIStructure ini; + // file.read(ini); + // + // /* read value; gets a reference to actual value in the structure. + // if key or section don't exist, a new empty value will be created */ + // std::string& value = ini["section"]["key"]; + // + // /* read value safely; gets a copy of value in the structure. + // does not alter the structure */ + // std::string value = ini.get("section").get("key"); + // + // /* set or update values */ + // ini["section"]["key"] = "value"; + // + // /* set multiple values */ + // ini["section2"].set({ + // {"key1", "value1"}, + // {"key2", "value2"} + // }); + // + // /* write updates back to file, preserving comments and formatting */ + // file.write(ini); + // + // /* or generate a file (overwrites the original) */ + // file.generate(ini); + // + /////////////////////////////////////////////////////////////////////////////// + // + // Long live the INI file!!! + // + /////////////////////////////////////////////////////////////////////////////// + +#ifndef MINI_INI_H_ +#define MINI_INI_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mINI +{ + namespace INIStringUtil + { + const char* const whitespaceDelimiters = " \t\n\r\f\v"; + inline void trim(std::string& str) + { + str.erase(str.find_last_not_of(whitespaceDelimiters) + 1); + str.erase(0, str.find_first_not_of(whitespaceDelimiters)); + } +#ifndef MINI_CASE_SENSITIVE + inline void toLower(std::string& str) + { + std::transform(str.begin(), str.end(), str.begin(), [](const char c) { + return static_cast(std::tolower(c)); + }); + } +#endif + inline void replace(std::string& str, std::string const& a, std::string const& b) + { + if (!a.empty()) + { + std::size_t pos = 0; + while ((pos = str.find(a, pos)) != std::string::npos) + { + str.replace(pos, a.size(), b); + pos += b.size(); + } + } + } +#ifdef _WIN32 + const char* const endl = "\r\n"; +#else + const char* const endl = "\n"; +#endif + } + + template + class INIMap + { + private: + using T_DataIndexMap = std::unordered_map; + using T_DataItem = std::pair; + using T_DataContainer = std::vector; + using T_MultiArgs = typename std::vector>; + + T_DataIndexMap dataIndexMap; + T_DataContainer data; + + inline std::size_t setEmpty(std::string& key) + { + std::size_t index = data.size(); + dataIndexMap[key] = index; + data.emplace_back(key, T()); + return index; + } + + public: + using const_iterator = typename T_DataContainer::const_iterator; + + INIMap() { } + + INIMap(INIMap const& other) + { + std::size_t data_size = other.data.size(); + for (std::size_t i = 0; i < data_size; ++i) + { + auto const& key = other.data[i].first; + auto const& obj = other.data[i].second; + data.emplace_back(key, obj); + } + dataIndexMap = T_DataIndexMap(other.dataIndexMap); + } + + T& operator[](std::string key) + { + INIStringUtil::trim(key); +#ifndef MINI_CASE_SENSITIVE + INIStringUtil::toLower(key); +#endif + auto it = dataIndexMap.find(key); + bool hasIt = (it != dataIndexMap.end()); + std::size_t index = (hasIt) ? it->second : setEmpty(key); + return data[index].second; + } + T get(std::string key) const + { + INIStringUtil::trim(key); +#ifndef MINI_CASE_SENSITIVE + INIStringUtil::toLower(key); +#endif + auto it = dataIndexMap.find(key); + if (it == dataIndexMap.end()) + { + return T(); + } + return T(data[it->second].second); + } + bool has(std::string key) const + { + INIStringUtil::trim(key); +#ifndef MINI_CASE_SENSITIVE + INIStringUtil::toLower(key); +#endif + return (dataIndexMap.count(key) == 1); + } + void set(std::string key, T obj) + { + INIStringUtil::trim(key); +#ifndef MINI_CASE_SENSITIVE + INIStringUtil::toLower(key); +#endif + auto it = dataIndexMap.find(key); + if (it != dataIndexMap.end()) + { + data[it->second].second = obj; + } + else + { + dataIndexMap[key] = data.size(); + data.emplace_back(key, obj); + } + } + void set(T_MultiArgs const& multiArgs) + { + for (auto const& it : multiArgs) + { + auto const& key = it.first; + auto const& obj = it.second; + set(key, obj); + } + } + bool remove(std::string key) + { + INIStringUtil::trim(key); +#ifndef MINI_CASE_SENSITIVE + INIStringUtil::toLower(key); +#endif + auto it = dataIndexMap.find(key); + if (it != dataIndexMap.end()) + { + std::size_t index = it->second; + data.erase(data.begin() + index); + dataIndexMap.erase(it); + for (auto& it2 : dataIndexMap) + { + auto& vi = it2.second; + if (vi > index) + { + vi--; + } + } + return true; + } + return false; + } + void clear() + { + data.clear(); + dataIndexMap.clear(); + } + std::size_t size() const + { + return data.size(); + } + const_iterator begin() const { return data.begin(); } + const_iterator end() const { return data.end(); } + }; + + using INIStructure = INIMap>; + + namespace INIParser + { + using T_ParseValues = std::pair; + + enum class PDataType : char + { + PDATA_NONE, + PDATA_COMMENT, + PDATA_SECTION, + PDATA_KEYVALUE, + PDATA_UNKNOWN + }; + + inline PDataType parseLine(std::string line, T_ParseValues& parseData) + { + parseData.first.clear(); + parseData.second.clear(); + INIStringUtil::trim(line); + if (line.empty()) + { + return PDataType::PDATA_NONE; + } + char firstCharacter = line[0]; + if (firstCharacter == ';') + { + return PDataType::PDATA_COMMENT; + } + if (firstCharacter == '[') + { + auto commentAt = line.find_first_of(';'); + if (commentAt != std::string::npos) + { + line = line.substr(0, commentAt); + } + auto closingBracketAt = line.find_last_of(']'); + if (closingBracketAt != std::string::npos) + { + auto section = line.substr(1, closingBracketAt - 1); + INIStringUtil::trim(section); + parseData.first = section; + return PDataType::PDATA_SECTION; + } + } + auto lineNorm = line; + INIStringUtil::replace(lineNorm, "\\=", " "); + auto equalsAt = lineNorm.find_first_of('='); + if (equalsAt != std::string::npos) + { + auto key = line.substr(0, equalsAt); + INIStringUtil::trim(key); + INIStringUtil::replace(key, "\\=", "="); + auto value = line.substr(equalsAt + 1); + INIStringUtil::trim(value); + parseData.first = key; + parseData.second = value; + return PDataType::PDATA_KEYVALUE; + } + return PDataType::PDATA_UNKNOWN; + } + } + + class INIReader + { + public: + using T_LineData = std::vector; + using T_LineDataPtr = std::shared_ptr; + + bool isBOM = false; + + private: + std::ifstream fileReadStream; + T_LineDataPtr lineData; + + T_LineData readFile() + { + fileReadStream.seekg(0, std::ios::end); + const std::size_t fileSize = static_cast(fileReadStream.tellg()); + fileReadStream.seekg(0, std::ios::beg); + if (fileSize >= 3) { + const char header[3] = { + static_cast(fileReadStream.get()), + static_cast(fileReadStream.get()), + static_cast(fileReadStream.get()) + }; + isBOM = ( + header[0] == static_cast(0xEF) && + header[1] == static_cast(0xBB) && + header[2] == static_cast(0xBF) + ); + } + else { + isBOM = false; + } + std::string fileContents; + fileContents.resize(fileSize); + fileReadStream.seekg(isBOM ? 3 : 0, std::ios::beg); + fileReadStream.read(&fileContents[0], fileSize); + fileReadStream.close(); + T_LineData output; + if (fileSize == 0) + { + return output; + } + std::string buffer; + buffer.reserve(50); + for (std::size_t i = 0; i < fileSize; ++i) + { + char& c = fileContents[i]; + if (c == '\n') + { + output.emplace_back(buffer); + buffer.clear(); + continue; + } + if (c != '\0' && c != '\r') + { + buffer += c; + } + } + output.emplace_back(buffer); + return output; + } + + public: + INIReader(std::string const& filename, bool keepLineData = false) + { + fileReadStream.open(filename, std::ios::in | std::ios::binary); + if (keepLineData) + { + lineData = std::make_shared(); + } + } + ~INIReader() { } + + bool operator>>(INIStructure& data) + { + if (!fileReadStream.is_open()) + { + return false; + } + T_LineData fileLines = readFile(); + std::string section; + bool inSection = false; + INIParser::T_ParseValues parseData; + for (auto const& line : fileLines) + { + auto parseResult = INIParser::parseLine(line, parseData); + if (parseResult == INIParser::PDataType::PDATA_SECTION) + { + inSection = true; + data[section = parseData.first]; + } + else if (inSection && parseResult == INIParser::PDataType::PDATA_KEYVALUE) + { + auto const& key = parseData.first; + auto const& value = parseData.second; + data[section][key] = value; + } + if (lineData && parseResult != INIParser::PDataType::PDATA_UNKNOWN) + { + if (parseResult == INIParser::PDataType::PDATA_KEYVALUE && !inSection) + { + continue; + } + lineData->emplace_back(line); + } + } + return true; + } + T_LineDataPtr getLines() + { + return lineData; + } + }; + + class INIGenerator + { + private: + std::ofstream fileWriteStream; + + public: + bool prettyPrint = false; + + INIGenerator(std::string const& filename) + { + fileWriteStream.open(filename, std::ios::out | std::ios::binary); + } + ~INIGenerator() { } + + bool operator<<(INIStructure const& data) + { + if (!fileWriteStream.is_open()) + { + return false; + } + if (!data.size()) + { + return true; + } + auto it = data.begin(); + for (;;) + { + auto const& section = it->first; + auto const& collection = it->second; + fileWriteStream + << "[" + << section + << "]"; + if (collection.size()) + { + fileWriteStream << INIStringUtil::endl; + auto it2 = collection.begin(); + for (;;) + { + auto key = it2->first; + INIStringUtil::replace(key, "=", "\\="); + auto value = it2->second; + INIStringUtil::trim(value); + fileWriteStream + << key + << ((prettyPrint) ? " = " : "=") + << value; + if (++it2 == collection.end()) + { + break; + } + fileWriteStream << INIStringUtil::endl; + } + } + if (++it == data.end()) + { + break; + } + fileWriteStream << INIStringUtil::endl; + if (prettyPrint) + { + fileWriteStream << INIStringUtil::endl; + } + } + return true; + } + }; + + class INIWriter + { + private: + using T_LineData = std::vector; + using T_LineDataPtr = std::shared_ptr; + + std::string filename; + + T_LineData getLazyOutput(T_LineDataPtr const& lineData, INIStructure& data, INIStructure& original) + { + T_LineData output; + INIParser::T_ParseValues parseData; + std::string sectionCurrent; + bool parsingSection = false; + bool continueToNextSection = false; + bool discardNextEmpty = false; + bool writeNewKeys = false; + std::size_t lastKeyLine = 0; + for (auto line = lineData->begin(); line != lineData->end(); ++line) + { + if (!writeNewKeys) + { + auto parseResult = INIParser::parseLine(*line, parseData); + if (parseResult == INIParser::PDataType::PDATA_SECTION) + { + if (parsingSection) + { + writeNewKeys = true; + parsingSection = false; + --line; + continue; + } + sectionCurrent = parseData.first; + if (data.has(sectionCurrent)) + { + parsingSection = true; + continueToNextSection = false; + discardNextEmpty = false; + output.emplace_back(*line); + lastKeyLine = output.size(); + } + else + { + continueToNextSection = true; + discardNextEmpty = true; + continue; + } + } + else if (parseResult == INIParser::PDataType::PDATA_KEYVALUE) + { + if (continueToNextSection) + { + continue; + } + if (data.has(sectionCurrent)) + { + auto& collection = data[sectionCurrent]; + auto const& key = parseData.first; + auto const& value = parseData.second; + if (collection.has(key)) + { + auto outputValue = collection[key]; + if (value == outputValue) + { + output.emplace_back(*line); + } + else + { + INIStringUtil::trim(outputValue); + auto lineNorm = *line; + INIStringUtil::replace(lineNorm, "\\=", " "); + auto equalsAt = lineNorm.find_first_of('='); + auto valueAt = lineNorm.find_first_not_of( + INIStringUtil::whitespaceDelimiters, + equalsAt + 1 + ); + std::string outputLine = line->substr(0, valueAt); + if (prettyPrint && equalsAt + 1 == valueAt) + { + outputLine += " "; + } + outputLine += outputValue; + output.emplace_back(outputLine); + } + lastKeyLine = output.size(); + } + } + } + else + { + if (discardNextEmpty && line->empty()) + { + discardNextEmpty = false; + } + else if (parseResult != INIParser::PDataType::PDATA_UNKNOWN) + { + output.emplace_back(*line); + } + } + } + if (writeNewKeys || std::next(line) == lineData->end()) + { + T_LineData linesToAdd; + if (data.has(sectionCurrent) && original.has(sectionCurrent)) + { + auto const& collection = data[sectionCurrent]; + auto const& collectionOriginal = original[sectionCurrent]; + for (auto const& it : collection) + { + auto key = it.first; + if (collectionOriginal.has(key)) + { + continue; + } + auto value = it.second; + INIStringUtil::replace(key, "=", "\\="); + INIStringUtil::trim(value); + linesToAdd.emplace_back( + key + ((prettyPrint) ? " = " : "=") + value + ); + } + } + if (!linesToAdd.empty()) + { + output.insert( + output.begin() + lastKeyLine, + linesToAdd.begin(), + linesToAdd.end() + ); + } + if (writeNewKeys) + { + writeNewKeys = false; + --line; + } + } + } + for (auto const& it : data) + { + auto const& section = it.first; + if (original.has(section)) + { + continue; + } + if (prettyPrint && output.size() > 0 && !output.back().empty()) + { + output.emplace_back(); + } + output.emplace_back("[" + section + "]"); + auto const& collection = it.second; + for (auto const& it2 : collection) + { + auto key = it2.first; + auto value = it2.second; + INIStringUtil::replace(key, "=", "\\="); + INIStringUtil::trim(value); + output.emplace_back( + key + ((prettyPrint) ? " = " : "=") + value + ); + } + } + return output; + } + + public: + bool prettyPrint = false; + + INIWriter(std::string const& filename) + : filename(filename) + { + } + ~INIWriter() { } + + bool operator<<(INIStructure& data) + { + struct stat buf; + bool fileExists = (stat(filename.c_str(), &buf) == 0); + if (!fileExists) + { + INIGenerator generator(filename); + generator.prettyPrint = prettyPrint; + return generator << data; + } + INIStructure originalData; + T_LineDataPtr lineData; + bool readSuccess = false; + bool fileIsBOM = false; + { + INIReader reader(filename, true); + if ((readSuccess = reader >> originalData)) + { + lineData = reader.getLines(); + fileIsBOM = reader.isBOM; + } + } + if (!readSuccess) + { + return false; + } + T_LineData output = getLazyOutput(lineData, data, originalData); + std::ofstream fileWriteStream(filename, std::ios::out | std::ios::binary); + if (fileWriteStream.is_open()) + { + if (fileIsBOM) { + const char utf8_BOM[3] = { + static_cast(0xEF), + static_cast(0xBB), + static_cast(0xBF) + }; + fileWriteStream.write(utf8_BOM, 3); + } + if (output.size()) + { + auto line = output.begin(); + for (;;) + { + fileWriteStream << *line; + if (++line == output.end()) + { + break; + } + fileWriteStream << INIStringUtil::endl; + } + } + return true; + } + return false; + } + }; + + class INIFile + { + private: + std::string filename; + + public: + INIFile(std::string const& filename) + : filename(filename) + { } + + ~INIFile() { } + + bool read(INIStructure& data) const + { + if (data.size()) + { + data.clear(); + } + if (filename.empty()) + { + return false; + } + INIReader reader(filename); + return reader >> data; + } + bool generate(INIStructure const& data, bool pretty = false) const + { + if (filename.empty()) + { + return false; + } + INIGenerator generator(filename); + generator.prettyPrint = pretty; + return generator << data; + } + bool write(INIStructure& data, bool pretty = false) const + { + if (filename.empty()) + { + return false; + } + INIWriter writer(filename); + writer.prettyPrint = pretty; + return writer << data; + } + }; +} + +#endif // MINI_INI_H_ \ No newline at end of file diff --git a/depthrushConfig/kinectTest.cpp b/depthrushConfig/kinectTest.cpp new file mode 100644 index 0000000..7570543 --- /dev/null +++ b/depthrushConfig/kinectTest.cpp @@ -0,0 +1,112 @@ +#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; +} \ No newline at end of file diff --git a/depthrushConfig/kinectTest.h b/depthrushConfig/kinectTest.h new file mode 100644 index 0000000..22da6ab --- /dev/null +++ b/depthrushConfig/kinectTest.h @@ -0,0 +1,2 @@ +#pragma once +int kinectTest(float xGrad, float xOffset, float yGrad, float yOffset, float zGrad, float zOffset); \ No newline at end of file