From d2422b348b59476522ab78ce871800b528de0eaa Mon Sep 17 00:00:00 2001 From: Daniel Bisig Date: Tue, 7 Dec 2021 12:15:16 +0000 Subject: [PATCH] first commit --- addons.make | 5 + src/dab_json_helper.cpp | 219 ++++++++++++ src/dab_json_helper.h | 44 +++ src/dab_mocap_data.cpp | 428 ++++++++++++++++++++++++ src/dab_mocap_data.h | 160 +++++++++ src/dab_mocap_data_serialize.cpp | 172 ++++++++++ src/dab_mocap_data_serialize.h | 29 ++ src/dab_mocap_player.cpp | 218 ++++++++++++ src/dab_mocap_player.h | 65 ++++ src/dab_vis_shape.cpp | 335 +++++++++++++++++++ src/dab_vis_shape.h | 86 +++++ src/dab_vis_skeleton.cpp | 307 +++++++++++++++++ src/dab_vis_skeleton.h | 95 ++++++ src/main.cpp | 13 + src/ofApp.cpp | 556 +++++++++++++++++++++++++++++++ src/ofApp.h | 101 ++++++ 16 files changed, 2833 insertions(+) create mode 100644 addons.make create mode 100644 src/dab_json_helper.cpp create mode 100644 src/dab_json_helper.h create mode 100644 src/dab_mocap_data.cpp create mode 100644 src/dab_mocap_data.h create mode 100644 src/dab_mocap_data_serialize.cpp create mode 100644 src/dab_mocap_data_serialize.h create mode 100644 src/dab_mocap_player.cpp create mode 100644 src/dab_mocap_player.h create mode 100644 src/dab_vis_shape.cpp create mode 100644 src/dab_vis_shape.h create mode 100644 src/dab_vis_skeleton.cpp create mode 100644 src/dab_vis_skeleton.h create mode 100644 src/main.cpp create mode 100644 src/ofApp.cpp create mode 100644 src/ofApp.h diff --git a/addons.make b/addons.make new file mode 100644 index 0000000..3b4f3df --- /dev/null +++ b/addons.make @@ -0,0 +1,5 @@ +ofxDabBase +ofxDabMath +ofxDabOsc +ofxImGui +ofxJSON diff --git a/src/dab_json_helper.cpp b/src/dab_json_helper.cpp new file mode 100644 index 0000000..ae540aa --- /dev/null +++ b/src/dab_json_helper.cpp @@ -0,0 +1,219 @@ +/** \file dab_json_helper.cpp +*/ + +#include "dab_json_helper.h" + +using namespace dab; + +const Json::Value& +JsonHelper::getValue(const Json::Value& pValue, const std::string& pValueName) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw dab::Exception("JSON ERROR: Value does not contain value name: " + pValueName, __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueName]; +} + +int +JsonHelper::getInt(const Json::Value& pValue, const std::string& pValueName) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw dab::Exception("JSON ERROR: Value does not contain value name: " + pValueName, __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueName].isInt() == false) throw dab::Exception("JSON ERROR: value name " + pValueName + " is not an integer", __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueName].asInt(); +} + +float +JsonHelper::getFloat(const Json::Value& pValue, const std::string& pValueName) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw dab::Exception("JSON ERROR: Value does not contain value name: " + pValueName, __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueName].isDouble() == false) throw dab::Exception("JSON ERROR: value name " + pValueName + " is not a float", __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueName].asFloat(); +} + +std::string +JsonHelper::getString(const Json::Value& pValue, const std::string& pValueName) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw dab::Exception("JSON ERROR: Value does not contain value name: " + pValueName, __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueName].isString() == false) throw dab::Exception("JSON ERROR: value name " + pValueName + " is not a float", __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueName].asString(); +} + +const Json::Value& +JsonHelper::getValue(const Json::Value& pValue, int pValueIndex) throw (dab::Exception) +{ + if (pValue.size() <= pValueIndex) throw dab::Exception("JSON ERROR: value index " + std::to_string(pValueIndex) + " exceeds size " + std::to_string(pValue.size()), __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueIndex]; +} + +int +JsonHelper::getInt(const Json::Value& pValue, unsigned int pValueIndex) throw (dab::Exception) +{ + if (pValue.size() <= pValueIndex) throw dab::Exception("JSON ERROR: value index " + std::to_string(pValueIndex) + " exceeds size " + std::to_string(pValue.size()), __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueIndex].isInt() == false) throw dab::Exception("JSON ERROR: value at index " + std::to_string(pValueIndex) + " is not an integer", __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueIndex].asInt(); +} + +float +JsonHelper::getFloat(const Json::Value& pValue, unsigned int pValueIndex) throw (dab::Exception) +{ + if (pValue.size() <= pValueIndex) throw dab::Exception("JSON ERROR: value index " + std::to_string(pValueIndex) + " exceeds size " + std::to_string(pValue.size()), __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueIndex].isDouble() == false) throw dab::Exception("JSON ERROR: value at index " + std::to_string(pValueIndex) + " is not a float", __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueIndex].asFloat(); +} + +std::string +JsonHelper::getString(const Json::Value& pValue, unsigned int pValueIndex) throw (dab::Exception) +{ + if (pValue.size() <= pValueIndex) throw dab::Exception("JSON ERROR: value index " + std::to_string(pValueIndex) + " exceeds size " + std::to_string(pValue.size()), __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueIndex].isString() == false) throw dab::Exception("JSON ERROR: value at index " + std::to_string(pValueIndex) + " is not a string", __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueIndex].asString(); +} + + +const Json::Value& +JsonHelper::getValue(const Json::Value& pValue, const std::string& pValueName, int pValueIndex) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw dab::Exception("JSON ERROR: Value does not contain value name: " + pValueName, __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueName].size() <= pValueIndex) throw dab::Exception("JSON ERROR: value index " + std::to_string(pValueIndex) + " exceeds size " + std::to_string(pValue[pValueName].size()) + " of value name: " + pValueName + " ", __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueName][pValueIndex]; +} + +int +JsonHelper::getInt(const Json::Value& pValue, const std::string& pValueName, unsigned int pValueIndex) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw dab::Exception("JSON ERROR: Value does not contain value name: " + pValueName, __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueName].size() <= pValueIndex) throw dab::Exception("JSON ERROR: value index " + std::to_string(pValueIndex) + " exceeds size " + std::to_string(pValue[pValueName].size()) + " of value name: " + pValueName + " ", __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueName][pValueIndex].isInt() == false) throw dab::Exception("JSON ERROR: value name " + pValueName + " at index " + std::to_string(pValueIndex) + " is not an integer", __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueName][pValueIndex].asInt(); +} + +float +JsonHelper::getFloat(const Json::Value& pValue, const std::string& pValueName, unsigned int pValueIndex) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw dab::Exception("JSON ERROR: Value does not contain value name: " + pValueName, __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueName].size() <= pValueIndex) throw dab::Exception("JSON ERROR: value index " + std::to_string(pValueIndex) + " exceeds size " + std::to_string(pValue[pValueName].size()) + " of value name: " + pValueName + " ", __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueName][pValueIndex].isDouble() == false) throw dab::Exception("JSON ERROR: value name " + pValueName + " at index " + std::to_string(pValueIndex) + " is not a float", __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueName][pValueIndex].asFloat(); +} + +std::string +JsonHelper::getString(const Json::Value& pValue, const std::string& pValueName, unsigned int pValueIndex) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw dab::Exception("JSON ERROR: Value does not contain value name: " + pValueName, __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueName].size() <= pValueIndex) throw dab::Exception("JSON ERROR: value index " + std::to_string(pValueIndex) + " exceeds size " + std::to_string(pValue[pValueName].size()) + " of value name: " + pValueName + " ", __FILE__, __FUNCTION__, __LINE__); + if (pValue[pValueName][pValueIndex].isString() == false) throw dab::Exception("JSON ERROR: value name " + pValueName + " at index " + std::to_string(pValueIndex) + " is not a string", __FILE__, __FUNCTION__, __LINE__); + + return pValue[pValueName][pValueIndex].asString(); +} + +void +JsonHelper::getValues(const Json::Value& pValue, const std::string& pValueName, std::vector& pValues) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw Exception("JSON ERROR: no data with name " + pValueName + " found", __FILE__, __FUNCTION__, __LINE__); + + const Json::Value& data = pValue[pValueName]; + int dataSize = data.size(); + + pValues.resize(dataSize); + + for (int d = 0; d < dataSize; ++d) + { + pValues[d] = data[d]; + } +} + +void +JsonHelper::getInts(const Json::Value& pValue, const std::string& pValueName, std::vector& pValues) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw Exception("JSON ERROR: no data with name " + pValueName + " found", __FILE__, __FUNCTION__, __LINE__); + + const Json::Value& data = pValue[pValueName]; + int dataSize = data.size(); + + pValues.resize(dataSize); + + for (int d = 0; d < dataSize; ++d) + { + pValues[d] = data[d].asInt(); + } +} + +void +JsonHelper::getFloats(const Json::Value& pValue, const std::string& pValueName, std::vector& pValues) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw Exception("JSON ERROR: no data with name " + pValueName + " found", __FILE__, __FUNCTION__, __LINE__); + + const Json::Value& data = pValue[pValueName]; + int dataSize = data.size(); + + pValues.resize(dataSize); + + for (int d = 0; d < dataSize; ++d) + { + pValues[d] = data[d].asFloat(); + } +} + +void +JsonHelper::getStrings(const Json::Value& pValue, const std::string& pValueName, std::vector& pValues) throw (dab::Exception) +{ + if (pValue.isMember(pValueName) == false) throw Exception("JSON ERROR: no data with name " + pValueName + " found", __FILE__, __FUNCTION__, __LINE__); + + const Json::Value& data = pValue[pValueName]; + int dataSize = data.size(); + + pValues.resize(dataSize); + + for (int d = 0; d < dataSize; ++d) + { + pValues[d] = data[d].asString(); + } +} + +void +JsonHelper::getInts(const Json::Value& pValue, std::vector& pValues) throw (dab::Exception) +{ + int dataSize = pValue.size(); + + pValues.resize(dataSize); + + for (int d = 0; d < dataSize; ++d) + { + pValues[d] = pValue[d].asInt(); + } +} + +void +JsonHelper::getFloats(const Json::Value& pValue, std::vector& pValues) throw (dab::Exception) +{ + int dataSize = pValue.size(); + + pValues.resize(dataSize); + + for (int d = 0; d < dataSize; ++d) + { + pValues[d] = pValue[d].asFloat(); + } +} + +void +JsonHelper::getStrings(const Json::Value& pValue, std::vector& pValues) throw (dab::Exception) +{ + int dataSize = pValue.size(); + + pValues.resize(dataSize); + + for (int d = 0; d < dataSize; ++d) + { + pValues[d] = pValue[d].asString(); + } +} diff --git a/src/dab_json_helper.h b/src/dab_json_helper.h new file mode 100644 index 0000000..6cf27d5 --- /dev/null +++ b/src/dab_json_helper.h @@ -0,0 +1,44 @@ +/** \file dab_json_helper.h +*/ + +#pragma once + +#include +#include "ofxJSON.h" +#include "dab_singleton.h" +#include "dab_exception.h" + +namespace dab +{ + +class JsonHelper : public Singleton +{ +public: + const Json::Value& getValue(const Json::Value& pValue, const std::string& pValueName) throw (dab::Exception); + int getInt(const Json::Value& pValue, const std::string& pValueName) throw (dab::Exception); + float getFloat(const Json::Value& pValue, const std::string& pValueName) throw (dab::Exception); + std::string getString(const Json::Value& pValue, const std::string& pValueName) throw (dab::Exception); + + const Json::Value& getValue(const Json::Value& pValue, int pValueIndex) throw (dab::Exception); + int getInt(const Json::Value& pValue, unsigned int pValueIndex) throw (dab::Exception); + float getFloat(const Json::Value& pValue, unsigned int pValueIndex) throw (dab::Exception); + std::string getString(const Json::Value& pValue, unsigned int pValueIndex) throw (dab::Exception); + + const Json::Value& getValue(const Json::Value& pValue, const std::string& pValueName, int pValueIndex) throw (dab::Exception); + int getInt(const Json::Value& pValue, const std::string& pValueName, unsigned int pValueIndex) throw (dab::Exception); + float getFloat(const Json::Value& pValue, const std::string& pValueName, unsigned int pValueIndex) throw (dab::Exception); + std::string getString(const Json::Value& pValue, const std::string& pValueName, unsigned int pValueIndex) throw (dab::Exception); + + void getValues(const Json::Value& pValue, const std::string& pValueName, std::vector& pValues) throw (dab::Exception); + void getInts(const Json::Value& pValue, const std::string& pValueName, std::vector& pValues) throw (dab::Exception); + void getFloats(const Json::Value& pValue, const std::string& pValueName, std::vector& pValues) throw (dab::Exception); + void getStrings(const Json::Value& pValue, const std::string& pValueName, std::vector& pValues) throw (dab::Exception); + + void getInts(const Json::Value& pValue, std::vector& pValues) throw (dab::Exception); + void getFloats(const Json::Value& pValue, std::vector& pValues) throw (dab::Exception); + void getStrings(const Json::Value& pValue, std::vector& pValues) throw (dab::Exception); + +protected: +}; + +}; \ No newline at end of file diff --git a/src/dab_mocap_data.cpp b/src/dab_mocap_data.cpp new file mode 100644 index 0000000..2baea7e --- /dev/null +++ b/src/dab_mocap_data.cpp @@ -0,0 +1,428 @@ +/** \file dab_mocap_data.cpp +*/ + +#include "dab_mocap_data.h" +#include "dab_mocap_data_serialize.h" +#include "ofVectorMath.h" + +using namespace dab; + +# pragma mark MocapData implementation + +MocapData::MocapData() + : mName("") +{} + +MocapData::MocapData(const std::string& pName, const std::vector& pDim, const std::vector& pValues) + : mName(pName) + , mDim(pDim) + , mValues(pValues) +{} + +MocapData::MocapData(const MocapData& pMocapData) + : mName(pMocapData.mName) + , mDim(pMocapData.mDim) + , mValues(pMocapData.mValues) +{} + +MocapData::~MocapData() +{} + +MocapData& +MocapData::operator=(const MocapData& pMocapData) +{ + mName = pMocapData.mName; + mDim = pMocapData.mDim; + mValues = pMocapData.mValues; + + return *this; +} + +const std::string& +MocapData::name() const +{ + return mName; +} + +const std::vector& +MocapData::dim() const +{ + return mDim; +} + +const std::vector& +MocapData::values() const +{ + return mValues; +} + +std::vector& +MocapData::values() +{ + return mValues; +} + +MocapData::operator std::string() const +{ + std::stringstream ss; + + ss << "MocapData:\n"; + ss << "name: " << mName << "\n"; + ss << "dim: ["; + for (int vI = 0; vI < mDim.size(); ++vI) ss << " " << mDim[vI]; + ss << " ]\n"; + //ss << "values: ["; + //for (int vI = 0; vI < mValues.size(); ++vI) ss << " " << mValues[vI]; + //ss << " ]\n"; + + return ss.str(); +} + +# pragma mark MocapSkeleton implementation + +MocapSkeleton::MocapSkeleton() +{} + +MocapSkeleton::MocapSkeleton(const std::vector& pJointNames, const std::vector& pJointOffsets, const std::vector& pJointParents) + : mJointNames(pJointNames) + , mJointOffsets(pJointOffsets) + , mJointParents(pJointParents) +{ + computeMetadata(); +} + +MocapSkeleton::MocapSkeleton(const MocapSkeleton& pSkeleton) + : mJointNames(pSkeleton.mJointNames) + , mJointOffsets(pSkeleton.mJointOffsets) + , mJointParents(pSkeleton.mJointParents) + , mJointHasChildren(pSkeleton.mJointHasChildren) + , mJointChildren(pSkeleton.mJointChildren) +{} + +MocapSkeleton::~MocapSkeleton() +{} + +MocapSkeleton& +MocapSkeleton::operator=(const MocapSkeleton& pSkeleton) +{ + mJointNames = pSkeleton.mJointNames; + mJointOffsets = pSkeleton.mJointOffsets; + mJointParents = pSkeleton.mJointParents; + mJointHasChildren = pSkeleton.mJointHasChildren; + mJointChildren = pSkeleton.mJointChildren; + + return *this; +} + +unsigned int +MocapSkeleton::jointCount() const +{ + return mJointNames.size(); +} + +const std::vector< std::string >& +MocapSkeleton::jointNames() const +{ + return mJointNames; +} + +const std::vector& +MocapSkeleton::jointOffsets() const +{ + return mJointOffsets; +} + +const std::vector< int >& +MocapSkeleton::jointParents() const +{ + return mJointParents; +} + +const std::vector< bool >& +MocapSkeleton::jointHasChildren() const +{ + return mJointHasChildren; +} + +const std::vector< std::vector >& +MocapSkeleton::jointChildren() const +{ + return mJointChildren; +} + +//torch::Tensor +//MocapSkeleton::forwardKinematics(torch::Tensor& pRotations, torch::Tensor& pRootPositions) const +//{ +// /* +// Perform forward kinematics using the given trajectory and local rotations. +// Arguments(where N = batch size, L = sequence length, J = number of joints) : +// --rotations : (N, L, J, 4) tensor of unit quaternions describing the local rotations of each joint. +// --root_positions : (N, L, 3) tensor describing the root joint positions. +// */ +// +// TorchQuat& torchquat = TorchQuat::get(); +// +// assert(pRotations.dim() == 4); +// assert(pRotations.size(pRotations.dim() - 1) == 4); +// +// std::vector< torch::Tensor > positionsWorld; +// std::vector< torch::Tensor > rotationsWorld; +// +// torch::Tensor expandedOffsets = mJointOffsets.expand({ pRotations.size(0), pRotations.size(1), mJointOffsets.size(0), mJointOffsets.size(1) }); +// +// /* +// Parallelize along the batch and time dimensions +// loop over joints +// */ +// +// int jointCount = mJointOffsets.size(0); +// for (int jI = 0; jI < jointCount; ++jI) +// { +// if (mJointParents[jI] == -1) +// { +// positionsWorld.push_back(pRootPositions); +// rotationsWorld.push_back(pRotations.index({Slice(None), Slice(None), 0})); +// } +// else +// { +// torch::Tensor pw = torchquat.qrot(rotationsWorld[mJointParents[jI]], expandedOffsets.index({ Slice(None), Slice(None), jI })); +// pw += positionsWorld[mJointParents[jI]]; +// +// positionsWorld.push_back(pw); +// +// if (mJointHasChildren[jI] == true) +// { +// torch::Tensor rw = torchquat.qmul(rotationsWorld[mJointParents[jI]], pRotations.index({Slice(None), Slice(None), jI})); +// rotationsWorld.push_back(rw); +// } +// else +// { +// // This joint is a terminal node -> it would be useless to compute the transformation +// rotationsWorld.push_back(torch::empty({1})); +// } +// } +// } +// +// return torch::stack(positionsWorld, 3).permute({ 0, 1, 3, 2 }); +//} + +MocapSkeleton::operator std::string() const +{ + std::stringstream ss; + + int joint_count = mJointNames.size(); + + ss << "MocapSkeleton:\n"; + ss << "joint names: ["; + for (int jI = 0; jI < joint_count; ++jI) ss << " " << mJointNames[jI]; + ss << " ]\n"; + + ss << "joint offsets: ["; + for (int jI = 0; jI < joint_count; ++jI) + { + ss << " [ "; + for (int dI = 0; dI < 3; ++dI) + { + ss << " " << mJointOffsets[jI*3+dI]; + } + ss << " ] "; + } + ss << " ]\n"; + + ss << "joint parents: ["; + for (int jI = 0; jI < joint_count; ++jI) ss << " " << mJointParents[jI]; + ss << " ]\n"; + + ss << "joint hasChildren: ["; + for (int jI = 0; jI < joint_count; ++jI) ss << " " << mJointHasChildren[jI]; + ss << " ]\n"; + + ss << "joint children: ["; + for (int jI = 0; jI < joint_count; ++jI) + { + ss << "["; + + int childCount = mJointChildren[jI].size(); + + for (int cI = 0; cI < childCount; ++cI) + { + ss << " " << mJointChildren[jI][cI]; + } + + ss << " ]"; + } + ss << "]\n"; + + return ss.str(); +} + +void +MocapSkeleton::computeMetadata() +{ + int joint_count = mJointNames.size(); + + mJointHasChildren.resize(joint_count, false); + mJointChildren.resize(joint_count); + + for (int jI = 0; jI < joint_count; ++jI) + { + if (mJointParents[jI] != -1) + { + mJointHasChildren[jI] = true; + mJointChildren[mJointParents[jI]].push_back(jI); + } + } + + mJointChildren.resize(joint_count); +} + +# pragma mark MocapSubjectData implementation + +MocapSubjectData::MocapSubjectData() +{} + +MocapSubjectData::MocapSubjectData(const std::string& pName, const MocapSkeleton& pSkeleton) + : mName(pName) + , mSkeleton(pSkeleton) +{} + +MocapSubjectData::MocapSubjectData(const MocapSubjectData& pSubjectData) + : mName(pSubjectData.mName) + , mSkeleton(pSubjectData.mSkeleton) + , mData(pSubjectData.mData) +{} + +MocapSubjectData::~MocapSubjectData() +{} + +MocapSubjectData& +MocapSubjectData::operator=(const MocapSubjectData& pSubjectData) +{ + mName = pSubjectData.mName; + mSkeleton = pSubjectData.mSkeleton; + mData = pSubjectData.mData; + + return *this; +} + +const std::string& +MocapSubjectData::name() const +{ + return mName; +} + +const MocapSkeleton& +MocapSubjectData::skeleton() const +{ + return mSkeleton; +} + +std::vector +MocapSubjectData::dataNames() const +{ + std::vector _dataNames; + for (const auto& item : mData) _dataNames.push_back(item.first); + return _dataNames; +} + +const std::map< std::string, MocapData >& +MocapSubjectData::data() const +{ + return mData; +} + +const MocapData& +MocapSubjectData::data(const std::string& pDataName) const throw (dab::Exception) +{ + if (mData.find(pDataName) == mData.end()) throw dab::Exception("MOCAP ERROR: no data with name " + pDataName + " found", __FILE__, __FUNCTION__, __LINE__); + + return mData.at(pDataName); +} + +void +MocapSubjectData::addData(const MocapData& pData) throw (dab::Exception) +{ + const std::string& dataName = pData.name(); + + if (mData.find(dataName) != mData.end()) throw dab::Exception("MOCAP ERROR: data with name " + dataName + " already stored", __FILE__, __FUNCTION__, __LINE__); + + mData[dataName] = pData; +} + +MocapSubjectData::operator std::string() const +{ + std::stringstream ss; + + ss << "MocapSubjectData:\n"; + ss << "name: " << mName << "\n"; + ss << "skeleton: " << mSkeleton << "\n"; + + for (const auto& item : mData) + { + ss << "data name: " << item.first << "\n"; + ss << "data: " << item.second << "\n"; + } + + return ss.str(); +} + +# pragma mark MocapDataset implementation + +MocapDataset::MocapDataset() +{} + +MocapDataset::~MocapDataset() +{} + +void +MocapDataset::load(const std::string& pFilePath) throw (dab::Exception) +{ + try + { + MocapDataSerialize::get().restore(pFilePath, *this); + } + catch (dab::Exception& e) + { + e += dab::Exception("JSON ERROR: failed to load file " + pFilePath, __FILE__, __FUNCTION__, __LINE__); + throw e; + } +} + +std::vector< std::string > +MocapDataset::subjectNames() const +{ + std::vector< std::string > _subjectNames; + for (const auto& item : mSubjects) _subjectNames.push_back(item.first); + return _subjectNames; +} + +MocapSubjectData& +MocapDataset::subjectData(const std::string& pSubjectName) throw (dab::Exception) +{ + if (mSubjects.find(pSubjectName) == mSubjects.end()) throw dab::Exception("MOCAP ERROR: subjec " + pSubjectName + " not found", __FILE__, __FUNCTION__, __LINE__); + + return mSubjects.at(pSubjectName); +} + +const MocapSubjectData& +MocapDataset::subjectData(const std::string& pSubjectName) const throw (dab::Exception) +{ + if (mSubjects.find(pSubjectName) == mSubjects.end()) throw dab::Exception("MOCAP ERROR: subjec " + pSubjectName + " not found", __FILE__, __FUNCTION__, __LINE__); + + return mSubjects.at(pSubjectName); +} + +MocapDataset::operator std::string() const +{ + std::stringstream ss; + + ss << "MocapDataset:\n"; + + for (const auto& item : mSubjects) + { + ss << "data name: " << item.first << "\n"; + ss << "data: " << item.second << "\n"; + } + + return ss.str(); +} \ No newline at end of file diff --git a/src/dab_mocap_data.h b/src/dab_mocap_data.h new file mode 100644 index 0000000..950c1be --- /dev/null +++ b/src/dab_mocap_data.h @@ -0,0 +1,160 @@ +/** \file dab_mocap_data.h +*/ + +#pragma once + +#include +#include +#include "dab_exception.h" + +namespace dab +{ + +class MocapDataSerialize; + +# pragma mark MocapData definition + +class MocapData +{ +public: + friend class MocapDataSerialize; + + MocapData(); + MocapData(const std::string& pName, const std::vector& pDim, const std::vector& pValues); + MocapData(const MocapData& pMocapData); + ~MocapData(); + + MocapData& operator=(const MocapData& pMocapData); + + const std::string& name() const; + const std::vector& dim() const; + + const std::vector& values() const; + std::vector& values(); + + operator std::string() const; + + friend std::ostream& operator << (std::ostream& pOstream, const MocapData& pData) + { + std::string info = pData; + pOstream << info; + return pOstream; + }; + +protected: + std::string mName; + std::vector mDim; + std::vector mValues; +}; + +# pragma mark MocapSkeleton definition + +class MocapSkeleton +{ +public: + friend class MocapDataSerialize; + + MocapSkeleton(); + MocapSkeleton(const std::vector< std::string >& pJointNames, const std::vector& pJointOffsets, const std::vector& pJointParents); + MocapSkeleton(const MocapSkeleton& pSkeleton); + ~MocapSkeleton(); + + MocapSkeleton& operator=(const MocapSkeleton& pSkeleton); + + unsigned int jointCount() const; + const std::vector< std::string >& jointNames() const; + const std::vector& jointOffsets() const; + const std::vector< int >& jointParents() const; + const std::vector< bool >& jointHasChildren() const; + const std::vector< std::vector >& jointChildren() const; + + //torch::Tensor forwardKinematics(torch::Tensor& pRotations, torch::Tensor& pRootPositions) const; + + operator std::string() const; + + friend std::ostream& operator << (std::ostream& pOstream, const MocapSkeleton& pSkeleton) + { + std::string info = pSkeleton; + pOstream << info; + return pOstream; + }; + +protected: + void computeMetadata(); + + std::vector< std::string > mJointNames; + std::vector mJointOffsets; + std::vector< int > mJointParents; + std::vector< bool > mJointHasChildren; + std::vector< std::vector > mJointChildren; + +}; + +# pragma mark MocapSubjectData definition + +class MocapSubjectData +{ +public: + friend class MocapDataSerialize; + + MocapSubjectData(); + MocapSubjectData(const std::string& pName, const MocapSkeleton& pSkeleton); + MocapSubjectData(const MocapSubjectData& pSubjectData); + ~MocapSubjectData(); + + MocapSubjectData& operator=(const MocapSubjectData& pSubjectData); + + const std::string& name() const; + const MocapSkeleton& skeleton() const; + std::vector dataNames() const; + const std::map< std::string, MocapData >& data() const; + const MocapData& data(const std::string& pDataName) const throw (dab::Exception); + + void addData(const MocapData& pData) throw (dab::Exception); + + operator std::string() const; + + friend std::ostream& operator << (std::ostream& pOstream, const MocapSubjectData& pSubjectData) + { + std::string info = pSubjectData; + pOstream << info; + return pOstream; + }; + +protected: + std::string mName; + MocapSkeleton mSkeleton; + std::map< std::string, MocapData > mData; +}; + +# pragma mark MocapDataset definition + +class MocapDataset +{ +public: + friend class MocapDataSerialize; + + MocapDataset(); + ~MocapDataset(); + + void load(const std::string& pFilePath) throw (dab::Exception); + + std::vector< std::string > subjectNames() const; + + MocapSubjectData& subjectData(const std::string& pSubjectName) throw (dab::Exception); + const MocapSubjectData& subjectData(const std::string& pSubjectName) const throw (dab::Exception); + + operator std::string() const; + + friend std::ostream& operator << (std::ostream& pOstream, const MocapDataset& pDataset) + { + std::string info = pDataset; + pOstream << info; + return pOstream; + }; + +protected: + std::map< std::string, MocapSubjectData > mSubjects; +}; + +}; \ No newline at end of file diff --git a/src/dab_mocap_data_serialize.cpp b/src/dab_mocap_data_serialize.cpp new file mode 100644 index 0000000..0856cc0 --- /dev/null +++ b/src/dab_mocap_data_serialize.cpp @@ -0,0 +1,172 @@ +/** \file dab_mocap_data_serialize.cpp +*/ + +#include "dab_mocap_data_serialize.h" +#include "dab_mocap_data.h" +#include "dab_file_io.h" +#include "dab_json_helper.h" + +using namespace dab; + +void +MocapDataSerialize::restore(const std::string& pFileName, MocapDataset& pDataSet) throw (dab::Exception) +{ + try + { + std::string restoreString; + dab::FileIO::get().read(pFileName, restoreString); + + Json::Reader reader; + Json::Value restoreData; + JsonHelper& jsonHelper = JsonHelper::get(); + + bool parsingSuccessful = reader.parse(restoreString, restoreData); + + if (parsingSuccessful == false) throw dab::Exception("FILE ERROR: failed to parse mocap data file " + pFileName, __FILE__, __FUNCTION__, __LINE__); + + unsigned int subjectCount = restoreData.size(); + + for (int sI = 0; sI < subjectCount; ++sI) + { + std::string subjectName = "S" + std::to_string(sI + 1); + + pDataSet.mSubjects[subjectName] = MocapSubjectData(); + pDataSet.mSubjects[subjectName].mName = subjectName; + + Json::Value subjectRestoreData = jsonHelper.getValue(restoreData, subjectName); + restoreSubjectData(subjectRestoreData, pDataSet.mSubjects[subjectName]); + } + + } + catch (dab::Exception& e) + { + e += dab::Exception("JSON ERROR: failed to restore mocap data from file " + pFileName, __FILE__, __FUNCTION__, __LINE__); + throw e; + } +} + +void +MocapDataSerialize::restoreSubjectData(Json::Value& pData, MocapSubjectData& pSubjectData) throw (dab::Exception) +{ + JsonHelper& jsonHelper = JsonHelper::get(); + + try + { + restoreSkeletonData(pData, pSubjectData.mSkeleton); + + std::vector excludeNames = { "names", "offsets", "parents", "children" }; + const std::vector& memberNames = pData.getMemberNames(); + + for (int dI = 0; dI < pData.size(); ++dI) + { + const std::string memberName = memberNames[dI]; + if (std::find(excludeNames.begin(), excludeNames.end(), memberName) != excludeNames.end()) continue; + + pSubjectData.mData[memberName] = MocapData(); + pSubjectData.mData[memberName].mName = memberName; + + restoreMocapData(pData[memberName], pSubjectData.mData[memberName]); + } + } + catch (dab::Exception& e) + { + e += dab::Exception("JSON ERROR: failed to restore subject", __FILE__, __FUNCTION__, __LINE__); + throw e; + } +} + +void +MocapDataSerialize::restoreSkeletonData(Json::Value& pData, MocapSkeleton& pSubjectData) throw (dab::Exception) +{ + JsonHelper& jsonHelper = JsonHelper::get(); + + try + { + std::vector jointNames; + std::vector jointOffsets; + std::vector jointParents; + + jsonHelper.getStrings(pData, "names", jointNames); + jsonHelper.getInts(pData, "parents", jointParents); + + unsigned int jointCount = jointNames.size(); + + Json::Value jointOffsetsRestoreData = jsonHelper.getValue(pData, "offsets"); + for (int jI = 0; jI < jointOffsetsRestoreData.size(); ++jI) + { + std::vector offsets; + Json::Value jointOffsetRestoreData = jsonHelper.getValue(jointOffsetsRestoreData, jI); + + for (int d = 0; d < jointOffsetRestoreData.size(); ++d) + { + jointOffsets.push_back(jsonHelper.getFloat(jointOffsetRestoreData, d)); + } + } + + pSubjectData = MocapSkeleton(jointNames, jointOffsets, jointParents); + } + catch (dab::Exception& e) + { + e += dab::Exception("JSON ERROR: failed to restore skeleton", __FILE__, __FUNCTION__, __LINE__); + throw e; + } +} + +void +MocapDataSerialize::restoreMocapData(Json::Value& pData, MocapData& pMocapData) throw (dab::Exception) +{ + JsonHelper& jsonHelper = JsonHelper::get(); + + try + { + Json::Value* _data = &pData; + std::vector dim; + + int valueCount = 1; + while (_data->size() >= 1) + { + dim.push_back(_data->size()); + valueCount *= _data->size(); + _data = &((*_data)[0]); + } + + int dataDim = dim.size(); + std::vector dataIndices(dataDim, 0); + pMocapData.mDim = dim; + pMocapData.mValues.resize(valueCount); + + int valueIndex = 0; + while (valueIndex < valueCount) + { + _data = &pData; + + for (int dI = 0; dI < dataDim; ++dI) + { + _data = (&(*_data)[dataIndices[dI]]); + } + + pMocapData.mValues[valueIndex++] = _data->asFloat(); + + bool indexIncr = true; + for (int dI = dataDim - 1; dI >= 0; --dI) + { + if (indexIncr == true) + { + dataIndices[dI] += 1; + indexIncr = false; + + if (dataIndices[dI] >= dim[dI]) + { + dataIndices[dI] = 0; + indexIncr = true; + } + } + } + } + } + catch (dab::Exception& e) + { + e += dab::Exception("JSON ERROR: failed to restore mocap data", __FILE__, __FUNCTION__, __LINE__); + throw e; + } +} diff --git a/src/dab_mocap_data_serialize.h b/src/dab_mocap_data_serialize.h new file mode 100644 index 0000000..f551f68 --- /dev/null +++ b/src/dab_mocap_data_serialize.h @@ -0,0 +1,29 @@ +/** \file dab_mocap_data_serialize.h +*/ + +#pragma once + +#include "ofxJSON.h" +#include "dab_singleton.h" +#include "dab_exception.h" + +namespace dab +{ + +class MocapDataset; +class MocapSubjectData; +class MocapSkeleton; +class MocapData; + +class MocapDataSerialize : public Singleton +{ +public: + void restore(const std::string& pFileName, MocapDataset& pDataSet) throw (dab::Exception); + +protected: + void restoreSubjectData(Json::Value& pData, MocapSubjectData& pSubjectData) throw (dab::Exception); + void restoreSkeletonData(Json::Value& pData, MocapSkeleton& pSubjectData) throw (dab::Exception); + void restoreMocapData(Json::Value& pData, MocapData& pMocapData) throw (dab::Exception); +}; + +}; \ No newline at end of file diff --git a/src/dab_mocap_player.cpp b/src/dab_mocap_player.cpp new file mode 100644 index 0000000..0afd0d7 --- /dev/null +++ b/src/dab_mocap_player.cpp @@ -0,0 +1,218 @@ +#include "dab_mocap_player.h" +#include "ofApp.h" + +#pragma mark MocapPlayer implementation + +using namespace dab; + +float MocapPlayer::sFrameRate = 50.0; +bool MocapPlayer::sLoop = true; + +MocapPlayer::MocapPlayer() + : mFrameRate(sFrameRate) + , mLoop(sLoop) + , mPlay(false) + , mPlayStartFrame(0) + , mPlayRate(sFrameRate) +{} + +MocapPlayer::~MocapPlayer() +{} + +void +MocapPlayer::setPositionSequence(const std::vector& pPosDim, const std::vector& pPosValues) +{ + int frameCount = pPosDim[0]; + int jointCount = pPosDim[1]; + int jointDim = pPosDim[2]; + + mPositionSequence = std::vector< std::vector >(frameCount, std::vector(jointCount)); + + for (int fI = 0, dI = 0; fI < frameCount; fI++) + { + for (int jI = 0; jI < jointCount; ++jI, dI += jointDim) + { + //std::cout << "fI " << fI << " jI " << jI << "\n"; + + mPositionSequence[fI][jI] = glm::vec3(pPosValues[dI], pPosValues[dI + 1], pPosValues[dI + 2]); + } + } + + mPlayStopFrame = mPositionSequence.size(); + mPlayStartTime = 0.0; + mPlayStopTime = static_cast(mPlayStopFrame) / mFrameRate; +} + +void +MocapPlayer::setPositionSequence(const std::vector< std::vector >& pPositionSequence) +{ + mPositionSequence = pPositionSequence; + mPlayStopFrame = mPositionSequence.size(); + mPlayStartTime = 0.0; + mPlayStopTime = static_cast(mPlayStopFrame) / mFrameRate; +} + +void +MocapPlayer::setRotationSequence(const std::vector& pRotDim, const std::vector& pRotValues) +{ + int frameCount = pRotDim[0]; + int jointCount = pRotDim[1]; + int jointDim = pRotDim[2]; + + mRotationSequence = std::vector< std::vector >(frameCount, std::vector(jointCount)); + + for (int fI = 0, dI = 0; fI < frameCount; fI++) + { + for (int jI = 0; jI < jointCount; ++jI, dI += jointDim) + { + //std::cout << "fI " << fI << " jI " << jI << "\n"; + + mRotationSequence[fI][jI] = glm::quat(pRotValues[dI], pRotValues[dI + 1], pRotValues[dI + 2], pRotValues[dI + 3]); + } + } + + mPlayStopFrame = mRotationSequence.size(); + mPlayStartTime = 0.0; + mPlayStopTime = static_cast(mPlayStopFrame) / mFrameRate; +} + +bool +MocapPlayer::isPlaying() const +{ + return mPlay; +} + +float +MocapPlayer::playRate() const +{ + return mPlayRate; +} + +float +MocapPlayer::playStartTime() const +{ + return mPlayStartTime; +} + +float +MocapPlayer::playStopTime() const +{ + return mPlayStopTime; +} + +bool +MocapPlayer::loop() const +{ + return mLoop; +} + +float +MocapPlayer::playTime() const +{ + return static_cast(mPlayFrame) / mFrameRate; +} + +void +MocapPlayer::setPlayRate(float pFrameRate) +{ + mPlayRate = pFrameRate; +} + +void +MocapPlayer::setPlayStartTime(float pPlayStartTime) +{ + int _playStartFrame = pPlayStartTime * mPlayRate; + + if (_playStartFrame < 0) _playStartFrame = 0; + if (_playStartFrame > mPlayStopFrame) _playStartFrame = mPlayStopFrame; + + mPlayStartFrame = _playStartFrame; + mPlayStartTime = static_cast(mPlayStartFrame) / mFrameRate; +} + +void +MocapPlayer::setPlayStopTime(float pPlayStopTime) +{ + int _playStopFrame = pPlayStopTime * mPlayRate; + + if (_playStopFrame < mPlayStartFrame) _playStopFrame = mPlayStartFrame; + if (_playStopFrame >= mPositionSequence.size()) _playStopFrame = mPositionSequence.size() - 1; + + mPlayStopFrame = _playStopFrame; + mPlayStopTime = static_cast(mPlayStopFrame) / mFrameRate; +} + +void +MocapPlayer::setLoop(bool pLoop) +{ + mLoop = pLoop; +} + +void +MocapPlayer::start() +{ + mStartTime = static_cast(ofGetElapsedTimeMillis()) / 1000.0; + mPlayFrame = mPlayStartFrame; + mPlay = true; +} + +void +MocapPlayer::stop() +{ + mPlay = false; +} + +const std::vector& +MocapPlayer::currentPosition() const +{ + return mPositionSequence[mPlayFrame]; +} + +const std::vector& +MocapPlayer::currentRotation() const +{ + return mRotationSequence[mPlayFrame]; +} + +void +MocapPlayer::update() +{ + if (mPlay == false) return; + + double currentTime = static_cast(ofGetElapsedTimeMillis()) / 1000.0; + double elapsedTime = currentTime - mStartTime; + int relPlayFrame = mPlayFrame - mPlayStartFrame; + //double nextFrameTime = static_cast(relPlayFrame) / mFrameRate; + double nextFrameTime = static_cast(relPlayFrame) / mPlayRate; + if (elapsedTime > nextFrameTime) + { + mPlayFrame++; + + if (mPlayFrame >= mPlayStopFrame) + { + if (mLoop == true) start(); + else stop(); + } + + // update listeners + int listenerCount = mListeners.size(); + + for (int lI = listenerCount - 1; lI >= 0; --lI) + { + std::shared_ptr listener = mListeners[lI].lock(); + + //std::cout << "lI " << lI << " ptr " << listener << "\n"; + + if (listener != nullptr) + { + mListeners[lI].lock()->notifyUpdate(); + } + } + } +} + +int +MocapPlayer::playFrame() const +{ + return mPlayFrame; +} diff --git a/src/dab_mocap_player.h b/src/dab_mocap_player.h new file mode 100644 index 0000000..b9621de --- /dev/null +++ b/src/dab_mocap_player.h @@ -0,0 +1,65 @@ +#pragma once + +#include +#include "ofVectorMath.h" +#include "ofMaterial.h" +#include "dab_exception.h" +#include "dab_listener.h" + +#pragma mark MocapPlayer definition + +namespace dab +{ + + class MocapPlayer : public dab::UpdateNotifier + { + public: + MocapPlayer(); + ~MocapPlayer(); + + void setPositionSequence(const std::vector& pPosDim, const std::vector& pPosValues); + void setPositionSequence(const std::vector< std::vector >& pPositionSequence); + + void setRotationSequence(const std::vector& pRotDim, const std::vector& pRotValues); + + bool isPlaying() const; + float playRate() const; + float playStartTime() const; + float playStopTime() const; + bool loop() const; + float playTime() const; + + void setPlayRate(float pPlayRate); + void setPlayStartTime(float pPlayStartTime); + void setPlayStopTime(float pPlayStopTime); + void setLoop(bool pLoop); + + void start(); + void stop(); + void update(); + + int playFrame() const; + const std::vector& currentPosition() const; + const std::vector& currentRotation() const; + + protected: + static float sFrameRate; + static bool sLoop; + + std::vector< std::vector > mPositionSequence; + std::vector< std::vector > mRotationSequence; + + float mFrameRate; + bool mPlay; + int mPlayFrame; + int mPlayStartFrame; + int mPlayStopFrame; + double mStartTime; + + float mPlayRate; + float mPlayStartTime; + float mPlayStopTime; + bool mLoop; + }; + +}; \ No newline at end of file diff --git a/src/dab_vis_shape.cpp b/src/dab_vis_shape.cpp new file mode 100644 index 0000000..0388aaa --- /dev/null +++ b/src/dab_vis_shape.cpp @@ -0,0 +1,335 @@ +/** \file dab_vis_shape.cpp +*/ + +#include "dab_vis_shape.h" + +using namespace dab; + +float VisShape::sSphereResolution = 8; +float VisShape::sCylinderResolution = 8; +float VisShape::sConeResolution = 8; + +void +VisShape::test() throw (dab::Exception) +{ + throw dab::Exception("my exception", __FILE__, __FUNCTION__, __LINE__); + +} + +VisShape::VisShape(VisShapeType pShapeType, const std::vector& pShapeProperties) + : mShapeType(pShapeType) + , mShapeProperties(pShapeProperties) + , mMesh(nullptr) + , mVisPrimitive(nullptr) +{ + try + { + init(); + } + catch(dab::Exception& e) + { + std::cout << e << "\n"; + } +} + +VisShape::VisShape(std::shared_ptr pMesh) + : mShapeType(VisMeshType) + , mMesh(pMesh) + , mVisPrimitive(nullptr) +{ + try + { + init(); + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } +} + +VisShape::VisShape(const VisShape& pShape) + : mShapeType(pShape.mShapeType) + , mShapeProperties(pShape.mShapeProperties) + , mMaterial(pShape.mMaterial) + , mMesh(pShape.mMesh) + , mVisPrimitive(nullptr) +{ + try + { + init(); + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } +} + +VisShape::~VisShape() +{} + +VisShape& +VisShape::operator=(const VisShape& pShape) +{ + mShapeType = pShape.mShapeType; + mShapeProperties = pShape.mShapeProperties; + mMaterial = pShape.mMaterial; + mMesh = pShape.mMesh; + mVisPrimitive = std::shared_ptr(nullptr); + + try + { + init(); + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } + + return *this; +} + +void +VisShape::setSphereResolution(float pSphereResolution) +{ + sSphereResolution = pSphereResolution; +} + +void +VisShape::setCylinderResolution(float pCylinderResolution) +{ + sCylinderResolution = pCylinderResolution; +} + +void +VisShape::setConeResolution(float pConeResolution) +{ + sConeResolution = pConeResolution; +} + +bool +VisShape::isSphereShape() const +{ + return mShapeType == VisSphereType; +} + +bool +VisShape::isBoxShape() const +{ + return mShapeType == VisBoxType; +} + +bool +VisShape::isCylinderShape() const +{ + return mShapeType == VisCylinderType; +} + +bool +VisShape::isConeShape() const +{ + return mShapeType == VisConeType; +} + +bool +VisShape::isMeshShape() const +{ + return mShapeType == VisMeshType; +} + +const std::vector& +VisShape::shapeProperties() const +{ + return mShapeProperties; +} + +std::shared_ptr +VisShape::visPrimitive() const +{ + return mVisPrimitive; +} + +ofMaterial& +VisShape::material() +{ + return mMaterial; +} + +void +VisShape::setShapeProperties(const std::vector& pShapeProperties) throw (dab::Exception) +{ + mShapeProperties = pShapeProperties; + + try + { + if (mShapeType == VisSphereType) updateSphereShape(); + else if (mShapeType == VisBoxType) updateBoxShape(); + else if (mShapeType == VisCylinderType) updateCylinderShape(); + else if (mShapeType == VisConeType) updateConeShape(); + } + catch (dab::Exception& e) + { + e += dab::Exception("VIS ERROR: failed to set shape properties", __FILE__, __FUNCTION__, __LINE__); + throw e; + } +} + +void +VisShape::setMaterial(const ofMaterial& pMaterial) +{ + mMaterial = pMaterial; +} + +void +VisShape::setTransform(const glm::vec3& pPosition, bool pGlobal) +{ + if (pGlobal == true) + { + mVisPrimitive->setGlobalPosition(pPosition); + } + else + { + mVisPrimitive->setPosition(pPosition); + } +} + +void +VisShape::setTransform(const glm::quat& pOrientation, bool pGlobal) +{ + if (pGlobal == true) + { + mVisPrimitive->setGlobalOrientation(pOrientation); + } + else + { + mVisPrimitive->setOrientation(pOrientation); + } +} + +void +VisShape::setTransform(const glm::vec3& pPosition, const glm::quat& pOrientation, bool pGlobal) +{ + setTransform(pPosition, pGlobal); + setTransform(pOrientation, pGlobal); +} + +void +VisShape::drawFaces() +{ + mMaterial.begin(); + mVisPrimitive->drawFaces(); + mMaterial.end(); +} + +void +VisShape::drawFaces(const ofMaterial& pMaterial) +{ + pMaterial.begin(); + mVisPrimitive->drawFaces(); + pMaterial.end(); +} + +void +VisShape::drawWireframe() +{ + mMaterial.begin(); + mVisPrimitive->drawWireframe(); + mMaterial.end(); +} + +void +VisShape::init() throw (dab::Exception) +{ + try + { + if(mShapeType == VisSphereType) initSphereShape(); + else if (mShapeType == VisBoxType) initBoxShape(); + else if (mShapeType == VisCylinderType) initCylinderShape(); + else if (mShapeType == VisConeType) initConeShape(); + else if (mShapeType == VisMeshType) initMeshShape(); + } + catch (dab::Exception& e) + { + e += dab::Exception("VIS ERROR: failed to create VisShape", __FILE__, __FUNCTION__, __LINE__); + throw e; + } +} + +void +VisShape::initMeshShape() throw (dab::Exception) +{ + if(mMesh == nullptr) throw dab::Exception("VIS ERROR: body mesh not defined", __FILE__, __FUNCTION__, __LINE__); + + mVisPrimitive = std::make_shared(*mMesh); +} + +void +VisShape::initSphereShape() throw (dab::Exception) +{ + if(mShapeProperties.size() != 1) throw dab::Exception("VIS ERROR: wrong number of shape properties", __FILE__, __FUNCTION__, __LINE__); + + mVisPrimitive = std::make_shared(mShapeProperties[0], sSphereResolution); +} + +void +VisShape::initBoxShape() throw (dab::Exception) +{ + if (mShapeProperties.size() != 3) throw dab::Exception("VIS ERROR: wrong number of box properties", __FILE__, __FUNCTION__, __LINE__); + + mVisPrimitive = std::make_shared(mShapeProperties[0], mShapeProperties[1], mShapeProperties[2], 1, 1, 1); +} + +void +VisShape::initCylinderShape() throw (dab::Exception) +{ + if (mShapeProperties.size() != 2) throw dab::Exception("VIS ERROR: wrong number of cylinder properties", __FILE__, __FUNCTION__, __LINE__); + + mVisPrimitive = std::make_shared(mShapeProperties[0], mShapeProperties[1], sCylinderResolution, 1); +} + +void +VisShape::initConeShape() throw (dab::Exception) +{ + if (mShapeProperties.size() != 2) throw dab::Exception("VIS ERROR: wrong number of cone properties", __FILE__, __FUNCTION__, __LINE__); + + mVisPrimitive = std::make_shared(mShapeProperties[0], mShapeProperties[1], sConeResolution, 1, 1); +} + +void +VisShape::updateSphereShape() throw (dab::Exception) +{ + if (mShapeProperties.size() != 1) throw dab::Exception("VIS ERROR: wrong number of shape properties", __FILE__, __FUNCTION__, __LINE__); + + std::shared_ptr sphereShape = std::static_pointer_cast(mVisPrimitive); + sphereShape->setRadius(mShapeProperties[0]); +} + +void +VisShape::updateBoxShape() throw (dab::Exception) +{ + if (mShapeProperties.size() != 3) throw dab::Exception("VIS ERROR: wrong number of shape properties", __FILE__, __FUNCTION__, __LINE__); + + std::shared_ptr boxShape = std::static_pointer_cast(mVisPrimitive); + boxShape->setWidth(mShapeProperties[0]); + boxShape->setHeight(mShapeProperties[1]); + boxShape->setDepth(mShapeProperties[2]); +} + +void +VisShape::updateCylinderShape() throw (dab::Exception) +{ + if (mShapeProperties.size() != 2) throw dab::Exception("VIS ERROR: wrong number of shape properties", __FILE__, __FUNCTION__, __LINE__); + + std::shared_ptr cylinderShape = std::static_pointer_cast(mVisPrimitive); + cylinderShape->setRadius(mShapeProperties[0]); + cylinderShape->setHeight(mShapeProperties[1]); +} + +void +VisShape::updateConeShape() throw (dab::Exception) +{ + if (mShapeProperties.size() != 2) throw dab::Exception("VIS ERROR: wrong number of shape properties", __FILE__, __FUNCTION__, __LINE__); + + std::shared_ptr coneShape = std::static_pointer_cast(mVisPrimitive); + coneShape->setRadius(mShapeProperties[0]); + coneShape->setHeight(mShapeProperties[1]); +} diff --git a/src/dab_vis_shape.h b/src/dab_vis_shape.h new file mode 100644 index 0000000..3ccaa3f --- /dev/null +++ b/src/dab_vis_shape.h @@ -0,0 +1,86 @@ +/** \file dab_vis_shape.h +*/ + +#pragma once + +#include +#include +#include "of3dPrimitives.h" +#include "ofMesh.h" +#include "ofMaterial.h" +#include "ofVectorMath.h" +#include "dab_exception.h" + +namespace dab +{ + +enum VisShapeType +{ + VisSphereType, + VisBoxType, + VisCylinderType, + VisConeType, + VisMeshType +}; + +class VisShape +{ +public: + void test() throw (dab::Exception); + + VisShape(VisShapeType pShapeType, const std::vector& pShapeProperties); + VisShape(std::shared_ptr pMesh); + VisShape(const VisShape& pShape); + ~VisShape(); + + VisShape& operator=(const VisShape& pShape); + + static void setSphereResolution(float pSphereResolution); + static void setCylinderResolution(float pCylinderResolution); + static void setConeResolution(float pConeResolution); + + bool isSphereShape() const; + bool isBoxShape() const; + bool isCylinderShape() const; + bool isConeShape() const; + bool isMeshShape() const; + + const std::vector& shapeProperties() const; + std::shared_ptr visPrimitive() const; + ofMaterial& material(); + + void setShapeProperties(const std::vector& pShapeProperties) throw (dab::Exception); + void setMaterial(const ofMaterial& pMaterial); + void setTransform(const glm::vec3& pPosition, bool pGlobal = true); + void setTransform(const glm::quat& pOrientation, bool pGlobal = true); + void setTransform(const glm::vec3& pPosition, const glm::quat& pOrientation, bool pGlobal=true); + void drawFaces(); + void drawFaces(const ofMaterial& pMaterial); + void drawWireframe(); + +protected: + static float sSphereResolution; + static float sCylinderResolution; // number of radius segments + static float sConeResolution; // number of radius segments + + VisShapeType mShapeType; + std::vector mShapeProperties; + std::shared_ptr mMesh; + std::shared_ptr mVisPrimitive; + ofMaterial mMaterial; + + void init() throw (dab::Exception); + + void initMeshShape() throw (dab::Exception); + void initSphereShape() throw (dab::Exception); + void initBoxShape() throw (dab::Exception); + void initCylinderShape() throw (dab::Exception); + void initConeShape() throw (dab::Exception); + + void updateSphereShape() throw (dab::Exception); + void updateBoxShape() throw (dab::Exception); + void updateCylinderShape() throw (dab::Exception); + void updateConeShape() throw (dab::Exception); +}; + +}; \ No newline at end of file diff --git a/src/dab_vis_skeleton.cpp b/src/dab_vis_skeleton.cpp new file mode 100644 index 0000000..80d7039 --- /dev/null +++ b/src/dab_vis_skeleton.cpp @@ -0,0 +1,307 @@ +/** \file dab_vis_skeleton.cpp +*/ + +#include "dab_vis_skeleton.h" +#include "dab_vis_shape.h" +#include "dab_math_vec.h" +#include "ofApp.h" +#include + +using namespace dab; + +#pragma mark VisSkeleton definition + +float VisSkeleton::sJointRadius = 1.0; +float VisSkeleton::sEdgeRadius = 0.5; + +VisSkeleton::VisSkeleton(const std::vector< std::vector >& pEdgeConnectivity) + : mEdgeConnectivity(pEdgeConnectivity) + , mRootPosition(0.0, -200.0, 0.0) + , mRootRotation(0.0, 0.0, 0.0) +{ + init(); +} + +VisSkeleton::~VisSkeleton() +{} + +unsigned int +VisSkeleton::jointCount() const +{ + return mJointShapes.size(); +} + +void +VisSkeleton::setJointMaterial(const ofMaterial& pMaterial) +{ + for (auto joint : mJointShapes) joint->setMaterial(pMaterial); +} + +void +VisSkeleton::setEdgeMaterial(const ofMaterial& pMaterial) +{ + for (auto edge : mEdgeShapes) edge->setMaterial(pMaterial); +} + +void +VisSkeleton::setRootPosition(const glm::vec3& pPos) +{ + mRootPosition = pPos; +} + +void +VisSkeleton::setRootRotation(const glm::vec3& pRot) +{ + mRootRotation = pRot; +} + +void +VisSkeleton::update(const std::vector& pJointPositions) throw (dab::Exception) +{ + math::VectorMath& vecMath = math::VectorMath::get(); + + int jointCount = mJointShapes.size(); + int edgeCount = mEdgeShapes.size(); + + if (pJointPositions.size() != jointCount) throw dab::Exception("VIS ERROR: number of joint positions doesn't match number of joints", __FILE__, __FUNCTION__, __LINE__); + + for (int jI = 0; jI < jointCount; ++jI) + { + mJointShapes[jI]->setTransform(pJointPositions[jI], true); + //std::cout << "joint " << jI << " pos " << pJointPositions[jI] << "\n"; + } + + for (int jI = 0, eI = 0; jI < jointCount; ++jI) + { + int jointEdgeCount = mEdgeConnectivity[jI].size(); + + for (int jeI = 0; jeI < jointEdgeCount; ++jeI, ++eI) + { + int jI2 = mEdgeConnectivity[jI][jeI]; + const glm::vec3& jP1 = pJointPositions[jI]; + const glm::vec3& jP2 = pJointPositions[jI2]; + + glm::vec3 edgeVec = jP2 - jP1; + float edgeLength = glm::length(edgeVec); + glm::vec3 edgeDir = glm::normalize(edgeVec); + glm::vec3 refDir(0.0, 1.0, 0.0); + + glm::vec3 edgePos = (jP2 + jP1) * 0.5; + glm::quat edgeRot = vecMath.makeQuaternion(refDir, edgeDir); + + mEdgeShapes[eI]->setShapeProperties( { sEdgeRadius, edgeLength } ); + mEdgeShapes[eI]->setTransform(edgePos, edgeRot, true); + } + } +} + +void +VisSkeleton::draw() +{ + ofSetColor(255, 255, 255); + //ofPushMatrix(); + //ofRotateX(mRootRotation[0]); + //ofRotateY(mRootRotation[1]); + //ofRotateZ(mRootRotation[2]); + //ofTranslate(mRootPosition); + + int jointCount = mJointShapes.size(); + int edgeCount = mEdgeShapes.size(); + + for (int eI = 0; eI < edgeCount; ++eI) + { + mEdgeShapes[eI]->drawFaces(); + } + + for (int jI = 0; jI < jointCount; ++jI) + { + mJointShapes[jI]->drawFaces(); + } + + //ofPopMatrix(); +} + +void +VisSkeleton::init() +{ + // add spheres for each joint + int jointCount = mEdgeConnectivity.size(); + for (int jI = 0; jI < jointCount; ++jI) + { + mJointShapes.push_back(std::shared_ptr(new VisShape(VisSphereType, { sJointRadius }))); + } + + // add cylinders for each edge + for (int jI = 0; jI < jointCount; ++jI) + { + std::cout << "joint " << jI << " children: "; + + int childCount = mEdgeConnectivity[jI].size(); + for (int cI = 0; cI < childCount; ++cI) + { + std::cout << mEdgeConnectivity[jI][cI] << " "; + + float edgeLength = 100.0; // fixed length, the length will be corrected in the update routine + mEdgeShapes.push_back(std::shared_ptr(new VisShape(VisCylinderType, { sEdgeRadius, edgeLength }))); + } + + std::cout << "\n"; + } +} + +#pragma mark VisSkeletonPlayer implementation + +float VisSkeletonPlayer::sFrameRate = 50.0; +bool VisSkeletonPlayer::sLoop = true; + +VisSkeletonPlayer::VisSkeletonPlayer(const std::vector< std::vector >& pEdgeConnectivity) + : mFrameRate(sFrameRate) + , mLoop(sLoop) + , mPlay(false) + , mPlayStartFrame(0) + , mPlayRate(sFrameRate) +{ + init(pEdgeConnectivity); +} + +VisSkeletonPlayer::~VisSkeletonPlayer() +{} + +std::shared_ptr +VisSkeletonPlayer::skeleton() +{ + return mSkeleton; +} + +void +VisSkeletonPlayer::setPositionSequence(const std::vector& pPositionSequence) +{ + int jointCount = mSkeleton->jointCount(); + int frameCount = pPositionSequence.size() / jointCount / 3; + + mPositionSequence = std::vector< std::vector >(frameCount, std::vector(jointCount)); + + for (int fI = 0, dI = 0; fI < frameCount; fI++) + { + for (int jI = 0; jI < jointCount; ++jI, dI += 3) + { + //std::cout << "fI " << fI << " jI " << jI << "\n"; + + mPositionSequence[fI][jI] = glm::vec3(pPositionSequence[dI], pPositionSequence[dI + 1], pPositionSequence[dI + 2]); + } + } + + mPlayStopFrame = mPositionSequence.size(); +} + +void +VisSkeletonPlayer::setPositionSequence(const std::vector< std::vector >& pPositionSequence) +{ + mPositionSequence = pPositionSequence; + mPlayStopFrame = mPositionSequence.size(); +} + +void +VisSkeletonPlayer::setPlayRate(float pFrameRate) +{ + mPlayRate = pFrameRate; +} + +void +VisSkeletonPlayer::setPlayStartTime(int pPlayStartTime) +{ + int _playStartFrame = pPlayStartTime * mPlayRate; + + if (_playStartFrame < 0) _playStartFrame = 0; + if (_playStartFrame > mPlayStopFrame) _playStartFrame = mPlayStopFrame; + + mPlayStartFrame = _playStartFrame; +} + +void +VisSkeletonPlayer::setPlayStopTime(int pPlayStopTime) +{ + int _playStopFrame = pPlayStopTime * mPlayRate; + + if (_playStopFrame < mPlayStartTime) _playStopFrame = mPlayStartTime; + if (_playStopFrame >= mPositionSequence.size()) _playStopFrame = mPositionSequence.size() - 1; + + mPlayStopFrame = _playStopFrame; +} + +void +VisSkeletonPlayer::start() +{ + mPlayStartTime = static_cast(ofGetElapsedTimeMillis()) / 1000.0; + mPlayFrame = mPlayStartFrame; + mPlay = true; +} + +void +VisSkeletonPlayer::stop() +{ + mPlay = false; +} + +const std::vector& +VisSkeletonPlayer::currentPosition() const +{ + return mPositionSequence[mPlayFrame]; +} + +void +VisSkeletonPlayer::update() +{ + if (mPlay == false) return; + + double currentTime = static_cast(ofGetElapsedTimeMillis()) / 1000.0; + double elapsedTime = currentTime - mPlayStartTime; + int relPlayFrame = mPlayFrame - mPlayStartFrame; + double nextFrameTime = static_cast(relPlayFrame) / mFrameRate; + if (elapsedTime > nextFrameTime) + { + mPlayFrame++; + + if (mPlayFrame >= mPlayStopFrame) + { + mPlayFrame = mPlayStartFrame; + } + + try + { + mSkeleton->update(mPositionSequence[mPlayFrame]); + mPlay = true; + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } + + // update listeners + int listenerCount = mListeners.size(); + for (int lI = listenerCount - 1; lI >= 0; --lI) + { + mListeners[lI].lock()->notifyUpdate(); + } + } +} + + +void +VisSkeletonPlayer::draw() +{ + mSkeleton->draw(); +} + +int +VisSkeletonPlayer::playFrame() const +{ + return mPlayFrame; +} + +void +VisSkeletonPlayer::init(const std::vector< std::vector >& pEdgeConnectivity) +{ + mSkeleton = std::make_shared(pEdgeConnectivity); +} + diff --git a/src/dab_vis_skeleton.h b/src/dab_vis_skeleton.h new file mode 100644 index 0000000..225e9e1 --- /dev/null +++ b/src/dab_vis_skeleton.h @@ -0,0 +1,95 @@ +/** \file dab_vis_skeleton.h +*/ + +#pragma once + +#include +#include "ofVectorMath.h" +#include "ofMaterial.h" +#include "dab_exception.h" +#include "dab_listener.h" + +namespace dab +{ + +class VisShape; +class MocapSkeleton; + +#pragma mark VisSkeleton definition + +class VisSkeleton +{ +public: + VisSkeleton(const std::vector< std::vector >& pEdgeConnectivity); + ~VisSkeleton(); + + unsigned int jointCount() const; + + void setJointMaterial(const ofMaterial& pMaterial); + void setEdgeMaterial(const ofMaterial& pMaterial); + void setRootPosition(const glm::vec3& pPos); + void setRootRotation(const glm::vec3& pRot); + + void update(const std::vector& pJointPositions) throw (dab::Exception); + + void draw(); + +protected: + static float sJointRadius; + static float sEdgeRadius; + + void init(); + + glm::vec3 mRootPosition; + glm::vec3 mRootRotation; + + std::vector< std::vector >mEdgeConnectivity; + std::vector< std::shared_ptr > mJointShapes; + std::vector< std::shared_ptr > mEdgeShapes; +}; + +#pragma mark VisSkeletonPlayer definition + +class VisSkeletonPlayer: public dab::UpdateNotifier +{ +public: + VisSkeletonPlayer(const std::vector< std::vector >& pEdgeConnectivity); + ~VisSkeletonPlayer(); + + std::shared_ptr skeleton(); + + void setPositionSequence(const std::vector& pPositionSequence); + void setPositionSequence(const std::vector< std::vector >& pPositionSequence); + void setPlayRate(float pPlayRate); + void setPlayStartTime(int pPlayStartTime); + void setPlayStopTime(int pPlayStopTime); + + void start(); + void stop(); + void update(); + void draw(); + + int playFrame() const; + const std::vector& currentPosition() const; + +protected: + static float sFrameRate; + static bool sLoop; + + void init(const std::vector< std::vector >& pEdgeConnectivity); + + std::shared_ptr mSkeleton; + std::vector< std::vector > mPositionSequence; + float mFrameRate; + bool mPlay; + bool mLoop; + + int mPlayFrame; + int mPlayStartFrame; + int mPlayStopFrame; + double mPlayStartTime; + + float mPlayRate; +}; + +}; \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..e57370b --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,13 @@ +#include "ofMain.h" +#include "ofApp.h" + +//======================================================================== +int main( ){ + ofSetupOpenGL(1024,768,OF_WINDOW); // <-------- setup the GL context + + // this kicks off the running of my app + // can be OF_WINDOW or OF_FULLSCREEN + // pass in width and height too: + ofRunApp(new ofApp()); + +} diff --git a/src/ofApp.cpp b/src/ofApp.cpp new file mode 100644 index 0000000..c3246ab --- /dev/null +++ b/src/ofApp.cpp @@ -0,0 +1,556 @@ +#include "ofApp.h" +#include "ofMathConstants.h" +#include +#include +#include +#include +#include +#include "dab_exception.h" +#include "dab_file_io.h" +#include "dab_json_helper.h" +#include "dab_ringbuffer.h" +#include "dab_vis_skeleton.h" + + +//-------------------------------------------------------------- +void ofApp::setup() +{ + try + { + mSelf = std::shared_ptr(this); + + loadConfig("data/config.json"); + setupMocap(); + loadMocapFile(mMocapFileName); + + setupOsc(); + + setupGraphics(); + + mGui.setup(); + } + catch (std::exception& e) + { + std::cout << e.what() << "\n"; + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } +} + +//-------------------------------------------------------------- +void ofApp::update() +{ + try + { + if(mMocapPlayer != nullptr) mMocapPlayer->update(); + //updateOsc(); + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } + +} + +//-------------------------------------------------------------- +void ofApp::keyPressed(int key){ + +} + +//-------------------------------------------------------------- +void ofApp::keyReleased(int key){ + +} + +//-------------------------------------------------------------- +void ofApp::mouseMoved(int x, int y ){ + +} + +//-------------------------------------------------------------- +void ofApp::mouseDragged(int x, int y, int button) +{ + if (button == 2) + { + if (mMouseDragActive == false) + { + mMouseDragActive = true; + mMouseDragStartPos = glm::vec2(x, y); + } + else + { + glm::vec2 mouseDragVec = glm::vec2(x, y) - mMouseDragStartPos; + mCameraAzumith += mouseDragVec.y / static_cast(ofGetWindowHeight()) * PI * 0.1; + mCameraElevation += mouseDragVec.x / static_cast(ofGetWindowWidth()) * PI * 0.1; + + //std::cout << "drag x " << mouseDragVec.x << " y " << mouseDragVec.y << " az " << mCameraAzumith << " el " << mCameraElevation << "\n"; + + updateCamera(); + } + } + else + { + mMouseDragActive = false; + } +} + +//-------------------------------------------------------------- +void ofApp::mousePressed(int x, int y, int button){ + +} + +//-------------------------------------------------------------- +void ofApp::mouseReleased(int x, int y, int button){ + +} + +//-------------------------------------------------------------- +void ofApp::mouseEntered(int x, int y){ + +} + +//-------------------------------------------------------------- +void ofApp::mouseExited(int x, int y){ + +} + +void ofApp::mouseScrolled(int x, int y, float scrollX, float scrollY) +{ + //std::cout << "scrollX " << scrollX << " scrollY " << scrollY << "\n"; + + mCameraDistance += scrollY * 10.0; + updateCamera(); +} + + +//-------------------------------------------------------------- +void ofApp::windowResized(int w, int h){ + +} + +//-------------------------------------------------------------- +void ofApp::gotMessage(ofMessage msg){ + +} + +//-------------------------------------------------------------- +void ofApp::dragEvent(ofDragInfo dragInfo){ + +} + +//-------------------------------------------------------------- + +void +ofApp::loadConfig(const std::string& pFileName) throw (dab::Exception) +{ + try + { + std::string restoreString; + dab::FileIO::get().read(pFileName, restoreString); + + Json::Reader reader; + Json::Value restoreData; + dab::JsonHelper& jsonHelper = dab::JsonHelper::get(); + + bool parsingSuccessful = reader.parse(restoreString, restoreData); + + if (parsingSuccessful == false) throw dab::Exception("FILE ERROR: failed to parse config data file " + pFileName, __FILE__, __FUNCTION__, __LINE__); + + mMocapFileName = jsonHelper.getString(restoreData, "mocapFileName"); + mOscSendAddress = jsonHelper.getString(restoreData, "oscSendAddress"); + mOscSendPort = jsonHelper.getInt(restoreData, "oscSendPort"); + } + catch (dab::Exception& e) + { + e += dab::Exception("JSON ERROR: failed to restore config from file " + pFileName, __FILE__, __FUNCTION__, __LINE__); + throw e; + } +} + +void +ofApp::setupMocap() +{} + +void +ofApp::loadMocapFile(const std::string& pFilePath) throw (dab::Exception) +{ + try + { + mMocapDataset.load(pFilePath); + + // create visual skeleton player + std::string subjectName = mMocapDataset.subjectNames()[0]; + const dab::MocapSubjectData& subjectData = mMocapDataset.subjectData(subjectName); + const dab::MocapSkeleton& skeleton = subjectData.skeleton(); + + const dab::MocapData& mocapPosData = mMocapDataset.subjectData(subjectName).data("pos_world"); + const std::vector& mocapPosDim = mocapPosData.dim(); + const std::vector& mocapPosValues = mocapPosData.values(); + + const dab::MocapData& mocapRotData = mMocapDataset.subjectData(subjectName).data("rot_world"); + const std::vector& mocapRotDim = mocapRotData.dim(); + const std::vector& mocapRotValues = mocapRotData.values(); + + mMocapPlayer = std::make_shared(); + mMocapPlayer->setPositionSequence(mocapPosDim, mocapPosValues); + mMocapPlayer->setRotationSequence(mocapRotDim, mocapRotValues); + mMocapPlayer->addListener(mSelf); + mMocapPlayer->start(); + } + catch (dab::Exception& e) + { + e += dab::Exception("MOCAP ERROR: failed to load mocap data", __FILE__, __FUNCTION__, __LINE__); + throw e; + } +} + +void +ofApp::updateMocap() +{ + ////std::cout << "ofApp::updateMocap()\n"; + + //int playFrame = mVisSkeletonPlayer->playFrame(); + + ////std::cout << "playFrame " << playFrame << "\n"; + + //if (playFrame == mMocapWindowOffset - 1) // player reached end of playable sequence + //{ + // mVisSkeletonPlayer->setPositionSequence(mPositionSequenceUpdateVector); + + // if (mMLThreadDone == true) + // { + // mMLThreadHandle = std::async(std::launch::async, &ofApp::updateML, this); + // } + //} +} + +void +ofApp::updateMocap(const std::vector& pMocapSample) throw (dab::Exception) +{ + //std::array imuOrient = { pMocapSample[6], pMocapSample[7], pMocapSample[8], pMocapSample[9] }; + + //mMocapLiveRingBuffer->update(imuOrient); + + //for (int sI = 0, vI = 0; sI < mMocapWindowLength; ++sI) + //{ + // const std::array& mocapSample = (*mMocapLiveRingBuffer)[mMocapWindowLength - (sI + 1)]; + + // for (int dI = 0; dI < 4; ++dI, ++vI) + // { + // mMocapLiveValues[vI] = mocapSample[dI]; + // } + //} +} + +void +ofApp::setupOsc() +{ + try + { + mOscReceiver = new dab::OscReceiver("MocapReceiver", 9002); + mOscReceiver->registerOscListener(std::weak_ptr(mSelf)); + mOscReceiver->start(); + + mOscSender = new dab::OscSender("MocapSender", mOscSendAddress, mOscSendPort); + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } +} + +void +ofApp::notify(std::shared_ptr pMessage) +{ + mOscLock.lock(); + + mOscMessageQueue.push_back(pMessage); + if (mOscMessageQueue.size() > mMaxOscMessageQueueLength) mOscMessageQueue.pop_front(); + + mOscLock.unlock(); +} + +void +ofApp::updateOsc() +{ + mOscLock.lock(); + + while (mOscMessageQueue.size() > 0) + { + std::shared_ptr< dab::OscMessage > oscMessage = mOscMessageQueue[0]; + + updateOsc(oscMessage); + + mOscMessageQueue.pop_front(); + } + + mOscLock.unlock(); +} + +void +ofApp::updateOsc(std::shared_ptr pMessage) +{ + try + { + std::string address = pMessage->address(); + + //std::cout << "address " << address << "\n"; + + const std::vector& arguments = pMessage->arguments(); + + if (address.compare("/imu/1") == 0) updateMocap(arguments); + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } +} + +void +ofApp::updateOscSender() throw (dab::Exception) +{ + try + { + // send joint positions + { + const std::vector& currentPos = mMocapPlayer->currentPosition(); + + int posCount = currentPos.size(); + + std::string messageAddress = "/mocap/joint/pos"; + std::shared_ptr message(new dab::OscMessage(messageAddress)); + + for (int pI = 0; pI < posCount; ++pI) + { + message->add(currentPos[pI].x); + message->add(currentPos[pI].y); + message->add(currentPos[pI].z); + } + + mOscSender->send(message); + } + + // send joint rotations + { + const std::vector& currentRot = mMocapPlayer->currentRotation(); + + int rotCount = currentRot.size(); + + std::string messageAddress = "/mocap/joint/rot"; + std::shared_ptr message(new dab::OscMessage(messageAddress)); + + for (int pI = 0; pI < rotCount; ++pI) + { + /* + // send euler + glm::vec3 euler = glm::eulerAngles(currentRot[pI]); + + message->add(euler[0]); + message->add(euler[1]); + message->add(euler[2]); + */ + + // send quaternion + message->add(currentRot[pI][0]); + message->add(currentRot[pI][1]); + message->add(currentRot[pI][2]); + message->add(currentRot[pI][3]); + } + + mOscSender->send(message); + } + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } +} + +void +ofApp::notifyUpdate() +{ + try + { + updateGraphics(); + updateOscSender(); + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } +} + +void +ofApp::updateMocap(const std::vector& pArgs) throw (dab::Exception) +{ + try + { + int argCount = pArgs.size(); + std::vector mocapSample(argCount); + for (int aI = 0; aI < argCount; ++aI) + { + float value = *pArgs[aI]; + mocapSample[aI] = value; + } + + //std::cout << "mocapSample " << mocapSample << "\n"; + + updateMocap(mocapSample); + } + catch (dab::Exception& e) + { + throw e; + } +} + +void +ofApp::setupGraphics() throw (dab::Exception) +{ + try + { + ofDisableArbTex(); // important: otherwise all textures will be of type RECT2D instead of 2D + ofDisableLighting(); + ofEnableDepthTest(); + ofSetVerticalSync(true); + ofBackground(0); + + // create camera + mCamera = std::shared_ptr(new ofCamera()); + updateCamera(); + + // create light source + ofEnableLighting(); + ofSetSmoothLighting(true); + mPointLight = std::make_shared(); + mPointLight->setPointLight(); + + // create visual skeleton + std::string subjectName = mMocapDataset.subjectNames()[0]; + const dab::MocapSubjectData& subjectData = mMocapDataset.subjectData(subjectName); + const dab::MocapSkeleton& skeleton = subjectData.skeleton(); + mVisSkeleton = std::make_shared(skeleton.jointChildren()); + + ofMaterial jointMaterial; + ofMaterial edgeMaterial; + + jointMaterial.setAmbientColor(ofColor::fromHsb(0.2 * 255.0, 0.4 * 255.0, 0.5 * 255.0)); + jointMaterial.setDiffuseColor(ofColor::fromHsb(0.2 * 255.0, 0.4 * 255.0, 0.5 * 255.0)); + jointMaterial.setSpecularColor(ofColor::fromHsb(0.2 * 255.0, 0.4 * 255.0, 0.5 * 255.0)); + jointMaterial.setShininess(100.0); + + edgeMaterial.setAmbientColor(ofColor::fromHsb(0.2 * 255.0, 0.2 * 255.0, 0.5 * 255.0)); + edgeMaterial.setDiffuseColor(ofColor::fromHsb(0.2 * 255.0, 0.2 * 255.0, 0.5 * 255.0)); + edgeMaterial.setSpecularColor(ofColor::fromHsb(0.2 * 255.0, 0.2 * 255.0, 0.5 * 255.0)); + edgeMaterial.setShininess(100.0); + + mVisSkeleton->setJointMaterial(jointMaterial); + mVisSkeleton->setEdgeMaterial(edgeMaterial); + + // configure light settings + mPointLight->setPosition(-100.0, 100.0, 1000); + mPointLight->setAmbientColor(ofColor::fromHsb(0.0 * 255.0, 0.0 * 255.0, 0.13871 * 255.0)); + mPointLight->setAttenuation(0.745602, 0.0, 0.0); + mPointLight->setDiffuseColor(ofColor::fromHsb(0.0 * 255.0, 0.0 * 255.0, 1.0 * 255.0)); + mPointLight->setSpecularColor(ofColor::fromHsb(0.0 * 255.0, 0.0 * 255.0, 1.0 * 255.0)); + } + catch (dab::Exception& e) + { + e += dab::Exception("VIS ERROR: failed to setup graphics", __FILE__, __FUNCTION__, __LINE__); + throw e; + } +} + +void +ofApp::updateGraphics() throw (dab::Exception) +{ + const std::vector& currentPos = mMocapPlayer->currentPosition(); + mVisSkeleton->update(currentPos); + + //mVisSkeleton->setRootRotation(glm::vec3(mCameraAzumith, mCameraElevation, 0.0)); +} + + + + +//-------------------------------------------------------------- +void ofApp::draw() +{ + try + { + displayGraphics(); + + mGui.begin(); + + bool isPlaying = mMocapPlayer->isPlaying(); + float playRate = mMocapPlayer->playRate(); + float playStartTime = mMocapPlayer->playStartTime(); + float playStopTime = mMocapPlayer->playStopTime(); + bool loop = mMocapPlayer->loop(); + float playTime = mMocapPlayer->playTime(); + + if (ImGui::Checkbox("isPlaying", &isPlaying)) + { + if (isPlaying == true) mMocapPlayer->start(); + else mMocapPlayer->stop(); + } + + ImGui::InputFloat("playTime", &playTime); + + if (ImGui::InputFloat("playRate", &playRate)) mMocapPlayer->setPlayRate(playRate); + if (ImGui::InputFloat("startTime", &playStartTime)) mMocapPlayer->setPlayStartTime(playStartTime); + if (ImGui::InputFloat("endTime", &playStopTime)) mMocapPlayer->setPlayStopTime(playStopTime); + if (ImGui::Checkbox("loop", &loop)) mMocapPlayer->setLoop(loop); + + + + //ImGui::SliderInt("iterCount", &mMocapEncodeIterCount, 1, 10); + //ImGui::SliderInt("liveIter", &mMocapLiveIterCount, 0, mMocapEncodeIterCount); + + mGui.end(); + + //const std::vector& jointPos = mMocapPlayer->currentPosition(); + //const glm::vec3 leftHandJointPos = jointPos[11]; + //std::string debugString = std::string("LeftHandPos Z " + std::to_string(leftHandJointPos.z)); + + //ofDrawBitmapString(debugString, 100, 100); + } + catch (dab::Exception& e) + { + std::cout << e << "\n"; + } +} + +void +ofApp::displayGraphics() throw (dab::Exception) +{ + ofSetColor(255.0, 255.0, 255.0); + + ofEnableLighting(); + mPointLight->enable(); + + mCamera->begin(); + mVisSkeleton->draw(); + mCamera->end(); +} + +void +ofApp::updateCamera() +{ + float zOffset = 100.0; + + glm::vec3 cameraPos(0.0, 0.0, 0.0); + cameraPos.x = cos(mCameraAzumith) * sin(mCameraElevation) * mCameraDistance; + cameraPos.y = sin(mCameraAzumith) * sin(mCameraElevation) * mCameraDistance; + cameraPos.z = cos(mCameraElevation) * mCameraDistance; + + cameraPos.z += mCameraVerticalOffset; + + mCamera->setPosition(cameraPos); + mCamera->lookAt(ofVec3f(0.0, 0.0, mCameraVerticalOffset), ofVec3f(0.0, 0.0, 1.0)); + //mCamera->lookAt(ofVec3f(0.0, 200.0, 0.0), ofVec3f(0.0, 1.0, 0.0)); + mCamera->setFov(20.0); + + std::cout << "mCameraAzumith " << mCameraAzumith << " mCameraElevation " << mCameraElevation << " mCameraDistance " << mCameraDistance << "\n"; +} \ No newline at end of file diff --git a/src/ofApp.h b/src/ofApp.h new file mode 100644 index 0000000..a5536fb --- /dev/null +++ b/src/ofApp.h @@ -0,0 +1,101 @@ +#pragma once + +#include "ofMain.h" +#include "ofxImGui.h" +#include "dab_osc_receiver.h" +#include "dab_osc_sender.h" +#include "dab_listener.h" +#include +#include +#include +#include "dab_mocap_data.h" +#include "dab_mocap_player.h" +#include "dab_vis_shape.h" +#include "dab_vis_skeleton.h" + +class ofApp : public ofBaseApp, public dab::OscListener, public dab::UpdateListener +{ + + public: + void setup(); + void update(); + void draw(); + + void keyPressed(int key); + void keyReleased(int key); + void mouseMoved(int x, int y ); + void mouseDragged(int x, int y, int button); + void mousePressed(int x, int y, int button); + void mouseReleased(int x, int y, int button); + void mouseEntered(int x, int y); + void mouseExited(int x, int y); + void mouseScrolled(int x, int y, float scrollX, float scrollY); + void windowResized(int w, int h); + void dragEvent(ofDragInfo dragInfo); + void gotMessage(ofMessage msg); + + void notify(std::shared_ptr pMessage); + void updateOsc(); + void updateOsc(std::shared_ptr pMessage); + + void notifyUpdate(); + + protected: + std::shared_ptr mSelf; + + // config + void loadConfig(const std::string& pFileName) throw (dab::Exception); + + // mocap + void setupMocap(); + void loadMocapFile(const std::string& pFilePath) throw (dab::Exception); + void updateMocap(); + void updateMocap(const std::vector& pMocapSample) throw (dab::Exception); + std::string mMocapFileName; + dab::MocapDataset mMocapDataset; + std::shared_ptr mMocapPlayer; + int mMocapWindowLength; + int mMocapWindowOffset; + int mMocapJointCount; + int mMocapJointDim; + int mMocapLatentDim; + int mMocapReadFrame; + + + // OSC Communication + void setupOsc() throw (dab::Exception); + void updateOscSender() throw (dab::Exception); + std::string mOscSendAddress; + int mOscSendPort; + std::mutex mOscLock; + dab::OscReceiver* mOscReceiver; + dab::OscSender* mOscSender; + unsigned int mMaxOscMessageQueueLength = 1000; + std::deque< std::shared_ptr > mOscMessageQueue; + void updateMocap(const std::vector& pArgs) throw (dab::Exception); + + // visual + void setupGraphics() throw (dab::Exception); + void updateGraphics() throw (dab::Exception); + void displayGraphics() throw (dab::Exception); + std::shared_ptr mCamera; + std::shared_ptr mPointLight; + std::shared_ptr mVisSkeleton; + + // camera + bool mMouseDragActive = false; + glm::vec2 mMouseDragStartPos; + float mCameraVerticalOffset = 90.0; + float mCameraAzumith = PI; + float mCameraElevation = PI / 2.0; + float mCameraDistance = 1000.0; + + float mMocapFPS = 50; + int mSeqIndex = 0; + + void updateCamera(); + + // Gui + ofxImGui::Gui mGui; + +};