mirror of
https://thingvellir.net/git/overte
synced 2025-03-27 23:52:03 +01:00
Shoulder puck calibration work in progress
* AnimInverseKinematics: debug draw for secondary targets * AnimInverseKienmatics: better clean up of ik target debug draw * GeometryUtil: added findPlaneFromPoints() * ViveControllerManager: external dependency on eigen * ViveControllerManager: record history of left/right hand controllers * ViveControllerManager: use history to determine user shoulder location for better calibration * ViveControllerManager: pass defaultToReferenceMat by const ref to calibrate functions. * CMake: added external depenency to eigen linear algebra library.
This commit is contained in:
Anthony J. Thibault
parent
c85e187c61
commit
9f6641ed10
10 changed files with 252 additions and 56 deletions
20
cmake/externals/eigen/CMakeLists.txt
vendored
Normal file
20
cmake/externals/eigen/CMakeLists.txt
vendored
Normal file
|
|
@ -0,0 +1,20 @@
|
|||
set(EXTERNAL_NAME eigen)
|
||||
|
||||
include(ExternalProject)
|
||||
ExternalProject_Add(
|
||||
${EXTERNAL_NAME}
|
||||
URL http://bitbucket.org/eigen/eigen/get/3.3.4.zip
|
||||
URL_MD5 e337acc279874bc6a56da4d973a723fb
|
||||
CONFIGURE_COMMAND ""
|
||||
BUILD_COMMAND ""
|
||||
INSTALL_COMMAND ""
|
||||
LOG_DOWNLOAD 1
|
||||
)
|
||||
|
||||
# Hide this external target (for ide users)
|
||||
set_target_properties(${EXTERNAL_NAME} PROPERTIES FOLDER "hidden/externals")
|
||||
|
||||
ExternalProject_Get_Property(${EXTERNAL_NAME} INSTALL_DIR)
|
||||
|
||||
string(TOUPPER ${EXTERNAL_NAME} EXTERNAL_NAME_UPPER)
|
||||
set(${EXTERNAL_NAME_UPPER}_INCLUDE_DIRS ${INSTALL_DIR} CACHE PATH "List of eigen include directories")
|
||||
26
cmake/modules/FindEigen.cmake
Normal file
26
cmake/modules/FindEigen.cmake
Normal file
|
|
@ -0,0 +1,26 @@
|
|||
#
|
||||
# FindEigen.cmake
|
||||
#
|
||||
# Try to find Eigen include path.
|
||||
# Once done this will define
|
||||
#
|
||||
# EIGEN_INCLUDE_DIRS
|
||||
#
|
||||
# Created on 7/14/2017 by Anthony Thibault
|
||||
# Copyright 2017 High Fidelity, Inc.
|
||||
#
|
||||
# Distributed under the Apache License, Version 2.0.
|
||||
# See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
|
||||
#
|
||||
|
||||
# setup hints for Eigen search
|
||||
include("${MACRO_DIR}/HifiLibrarySearchHints.cmake")
|
||||
hifi_library_search_hints("eigen")
|
||||
|
||||
# locate dir
|
||||
string(CONCAT EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS} "/src/eigen")
|
||||
|
||||
include(FindPackageHandleStandardArgs)
|
||||
find_package_handle_standard_args(EIGEN DEFAULT_MSG EIGEN_INCLUDE_DIRS)
|
||||
|
||||
mark_as_advanced(EIGEN_INCLUDE_DIRS EIGEN_SEARCH_DIRS)
|
||||
|
|
@ -61,24 +61,6 @@
|
|||
"weight": 1.0,
|
||||
"flexCoefficients": [1]
|
||||
},
|
||||
{
|
||||
"jointName": "LeftArm",
|
||||
"positionVar": "leftArmPosition",
|
||||
"rotationVar": "leftArmRotation",
|
||||
"typeVar": "leftArmType",
|
||||
"weightVar": "leftArmWeight",
|
||||
"weight": 0.75,
|
||||
"flexCoefficients": [1.0, 0.35, 0.2, 0.1, 0.05, 0.0, 0.0, 0.0]
|
||||
},
|
||||
{
|
||||
"jointName": "RightArm",
|
||||
"positionVar": "rightArmPosition",
|
||||
"rotationVar": "rightArmRotation",
|
||||
"typeVar": "rightArmType",
|
||||
"weightVar": "rightArmWeight",
|
||||
"weight": 0.75,
|
||||
"flexCoefficients": [1.0, 0.35, 0.2, 0.1, 0.05, 0.0, 0.0, 0.0]
|
||||
},
|
||||
{
|
||||
"jointName": "RightHand",
|
||||
"positionVar": "rightHandPosition",
|
||||
|
|
|
|||
|
|
@ -4811,10 +4811,10 @@ void Application::update(float deltaTime) {
|
|||
controller::Action::RIGHT_TOE_BASE
|
||||
};
|
||||
|
||||
// copy controller poses from userInputMapper to myAvatar.
|
||||
glm::mat4 myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition());
|
||||
glm::mat4 worldToSensorMatrix = glm::inverse(myAvatar->getSensorToWorldMatrix());
|
||||
glm::mat4 avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix;
|
||||
|
||||
for (auto& action : avatarControllerActions) {
|
||||
controller::Pose pose = userInputMapper->getPoseState(action);
|
||||
myAvatar->setControllerPoseInSensorFrame(action, pose.transform(avatarToSensorMatrix));
|
||||
|
|
|
|||
|
|
@ -23,6 +23,8 @@
|
|||
#include "CubicHermiteSpline.h"
|
||||
#include "AnimUtil.h"
|
||||
|
||||
static const int MAX_TARGET_MARKERS = 30;
|
||||
|
||||
static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointChainInfos, size_t numJointChainInfos,
|
||||
int indexA, int indexB,
|
||||
AnimInverseKinematics::JointChainInfo** jointChainInfoA,
|
||||
|
|
@ -94,6 +96,12 @@ AnimInverseKinematics::~AnimInverseKinematics() {
|
|||
_rotationAccumulators.clear();
|
||||
_translationAccumulators.clear();
|
||||
_targetVarVec.clear();
|
||||
|
||||
// remove markers
|
||||
for (int i = 0; i < MAX_TARGET_MARKERS; i++) {
|
||||
QString name = QString("ikTarget%1").arg(i);
|
||||
DebugDraw::getInstance().removeMyAvatarMarker(name);
|
||||
}
|
||||
}
|
||||
|
||||
void AnimInverseKinematics::loadDefaultPoses(const AnimPoseVec& poses) {
|
||||
|
|
@ -898,19 +906,30 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
|
|||
// debug render ik targets
|
||||
if (context.getEnableDebugDrawIKTargets()) {
|
||||
const vec4 WHITE(1.0f);
|
||||
const vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f);
|
||||
glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3());
|
||||
int targetNum = 0;
|
||||
|
||||
for (auto& target : targets) {
|
||||
glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation());
|
||||
glm::mat4 avatarTargetMat = rigToAvatarMat * context.getGeometryToRigMatrix() * geomTargetMat;
|
||||
|
||||
QString name = QString("ikTarget%1").arg(target.getIndex());
|
||||
QString name = QString("ikTarget%1").arg(targetNum);
|
||||
DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE);
|
||||
targetNum++;
|
||||
}
|
||||
|
||||
// draw secondary ik targets
|
||||
for (auto& iter : _secondaryTargetsInRigFrame) {
|
||||
glm::mat4 avatarTargetMat = rigToAvatarMat * (glm::mat4)iter.second;
|
||||
QString name = QString("ikTarget%1").arg(targetNum);
|
||||
DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), GREEN);
|
||||
targetNum++;
|
||||
}
|
||||
} else if (context.getEnableDebugDrawIKTargets() != _previousEnableDebugIKTargets) {
|
||||
// remove markers if they were added last frame.
|
||||
for (auto& target : targets) {
|
||||
QString name = QString("ikTarget%1").arg(target.getIndex());
|
||||
for (int i = 0; i < MAX_TARGET_MARKERS; i++) {
|
||||
QString name = QString("ikTarget%1").arg(i);
|
||||
DebugDraw::getInstance().removeMyAvatarMarker(name);
|
||||
}
|
||||
}
|
||||
|
|
@ -1744,12 +1763,16 @@ void AnimInverseKinematics::setSecondaryTargets(const AnimContext& context) {
|
|||
AnimPose rigToGeometryPose = AnimPose(glm::inverse(context.getGeometryToRigMatrix()));
|
||||
for (auto& iter : _secondaryTargetsInRigFrame) {
|
||||
AnimPose absPose = rigToGeometryPose * iter.second;
|
||||
absPose.scale() = glm::vec3(1.0f);
|
||||
AnimPose parentAbsPose;
|
||||
int parentIndex = _skeleton->getParentIndex(iter.first);
|
||||
if (parentIndex >= 0) {
|
||||
parentAbsPose = _skeleton->getAbsolutePose(parentIndex, _relativePoses);
|
||||
}
|
||||
// AJT: for now ignore translation on secondary poses.
|
||||
glm::vec3 origTrans = _relativePoses[iter.first].trans();
|
||||
_relativePoses[iter.first] = parentAbsPose.inverse() * absPose;
|
||||
_relativePoses[iter.first].trans() = origTrans;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -605,3 +605,55 @@ float coneSphereAngle(const glm::vec3& coneCenter, const glm::vec3& coneDirectio
|
|||
|
||||
return glm::max(0.0f, theta - phi);
|
||||
}
|
||||
|
||||
// given a set of points, compute a best fit plane that passes as close as possible through all the points.
|
||||
// http://www.ilikebigbits.com/blog/2015/3/2/plane-from-points
|
||||
bool findPlaneFromPoints(const glm::vec3* points, size_t numPoints, glm::vec3& planeNormalOut, glm::vec3& pointOnPlaneOut) {
|
||||
if (numPoints < 3) {
|
||||
return false;
|
||||
}
|
||||
glm::vec3 sum;
|
||||
for (size_t i = 0; i < numPoints; i++) {
|
||||
sum += points[i];
|
||||
}
|
||||
glm::vec3 centroid = sum * (1.0f / (float)numPoints);
|
||||
float xx = 0.0f, xy = 0.0f, xz = 0.0f;
|
||||
float yy = 0.0f, yz = 0.0f, zz = 0.0f;
|
||||
|
||||
for (size_t i = 0; i < numPoints; i++) {
|
||||
glm::vec3 r = points[i] - centroid;
|
||||
xx += r.x * r.x;
|
||||
xy += r.x * r.y;
|
||||
xz += r.x * r.z;
|
||||
yy += r.y * r.y;
|
||||
yz += r.y * r.z;
|
||||
zz += r.z * r.z;
|
||||
}
|
||||
|
||||
float det_x = yy * zz - yz * yz;
|
||||
float det_y = xx * zz - xz * xz;
|
||||
float det_z = xx * yy - xy * xy;
|
||||
float det_max = std::max(std::max(det_x, det_y), det_z);
|
||||
|
||||
if (det_max == 0.0f) {
|
||||
return false; // The points don't span a plane
|
||||
}
|
||||
|
||||
glm::vec3 dir;
|
||||
if (det_max == det_x) {
|
||||
float a = (xz * yz - xy * zz) / det_x;
|
||||
float b = (xy * yz - xz * yy) / det_x;
|
||||
dir = glm::vec3(1.0f, a, b);
|
||||
} else if (det_max == det_y) {
|
||||
float a = (yz * xz - xy * zz) / det_y;
|
||||
float b = (xy * xz - yz * xx) / det_y;
|
||||
dir = glm::vec3(a, 1.0f, b);
|
||||
} else {
|
||||
float a = (yz * xy - xz * yy) / det_z;
|
||||
float b = (xz * xy - yz * xx) / det_z;
|
||||
dir = glm::vec3(a, b, 1.0f);
|
||||
}
|
||||
pointOnPlaneOut = centroid;
|
||||
planeNormalOut = glm::normalize(dir);
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -163,5 +163,7 @@ private:
|
|||
static void copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& lengthB, glm::vec2* vertexArrayB);
|
||||
};
|
||||
|
||||
// given a set of points, compute a best fit plane that passes as close as possible through all the points.
|
||||
bool findPlaneFromPoints(const glm::vec3* points, size_t numPoints, glm::vec3& planeNormalOut, glm::vec3& pointOnPlaneOut);
|
||||
|
||||
#endif // hifi_GeometryUtil_h
|
||||
|
|
|
|||
|
|
@ -18,8 +18,14 @@ if (WIN32)
|
|||
include_hifi_library_headers(octree)
|
||||
|
||||
add_dependency_external_projects(OpenVR)
|
||||
add_dependency_external_projects(eigen)
|
||||
|
||||
find_package(OpenVR REQUIRED)
|
||||
target_include_directories(${TARGET_NAME} PRIVATE ${OPENVR_INCLUDE_DIRS})
|
||||
target_link_libraries(${TARGET_NAME} ${OPENVR_LIBRARIES})
|
||||
target_link_libraries(${TARGET_NAME} Winmm.lib)
|
||||
|
||||
# header only library
|
||||
find_package(eigen REQUIRED)
|
||||
target_include_directories(${TARGET_NAME} PRIVATE ${EIGEN_INCLUDE_DIRS})
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@
|
|||
#include <Plugins/InputConfiguration.h>
|
||||
#include <controllers/StandardControls.h>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
extern PoseData _nextSimPoseData;
|
||||
|
||||
|
|
@ -282,7 +283,13 @@ void ViveControllerManager::pluginUpdate(float deltaTime, const controller::Inpu
|
|||
}
|
||||
}
|
||||
|
||||
ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) : controller::InputDevice("Vive"), _system(system) {
|
||||
static const size_t CONTROLLER_HISTORY_SIZE = 90 * 3;
|
||||
|
||||
ViveControllerManager::InputDevice::InputDevice(vr::IVRSystem*& system) :
|
||||
controller::InputDevice("Vive"),
|
||||
_system(system),
|
||||
_leftControllerHistory(CONTROLLER_HISTORY_SIZE),
|
||||
_rightControllerHistory(CONTROLLER_HISTORY_SIZE) {
|
||||
|
||||
_configStringMap[Config::None] = QString("None");
|
||||
_configStringMap[Config::Feet] = QString("Feet");
|
||||
|
|
@ -534,7 +541,7 @@ void ViveControllerManager::InputDevice::calibrate(const controller::InputCalibr
|
|||
}
|
||||
}
|
||||
|
||||
bool ViveControllerManager::InputDevice::configureHands(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
bool ViveControllerManager::InputDevice::configureHands(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksXPosition);
|
||||
int puckCount = (int)_validTrackedObjects.size();
|
||||
if (_handConfig == HandConfig::Pucks && puckCount >= MIN_PUCK_COUNT) {
|
||||
|
|
@ -569,7 +576,7 @@ bool ViveControllerManager::InputDevice::configureHands(glm::mat4& defaultToRefe
|
|||
return false;
|
||||
}
|
||||
|
||||
bool ViveControllerManager::InputDevice::configureHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
bool ViveControllerManager::InputDevice::configureHead(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition);
|
||||
int puckCount = (int)_validTrackedObjects.size();
|
||||
if (_headConfig == HeadConfig::Puck && puckCount >= MIN_HEAD) {
|
||||
|
|
@ -583,7 +590,7 @@ bool ViveControllerManager::InputDevice::configureHead(glm::mat4& defaultToRefer
|
|||
return false;
|
||||
}
|
||||
|
||||
bool ViveControllerManager::InputDevice::configureBody(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
bool ViveControllerManager::InputDevice::configureBody(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
std::sort(_validTrackedObjects.begin(), _validTrackedObjects.end(), sortPucksYPosition);
|
||||
int puckCount = (int)_validTrackedObjects.size();
|
||||
glm::vec3 headXAxis = getReferenceHeadXAxis(defaultToReferenceMat, inputCalibration.defaultHeadMat);
|
||||
|
|
@ -637,8 +644,17 @@ void ViveControllerManager::InputDevice::updateCalibratedLimbs() {
|
|||
_poseStateMap[controller::RIGHT_FOOT] = addOffsetToPuckPose(controller::RIGHT_FOOT);
|
||||
_poseStateMap[controller::HIPS] = addOffsetToPuckPose(controller::HIPS);
|
||||
_poseStateMap[controller::SPINE2] = addOffsetToPuckPose(controller::SPINE2);
|
||||
_poseStateMap[controller::RIGHT_ARM] = addOffsetToPuckPose(controller::RIGHT_ARM);
|
||||
_poseStateMap[controller::LEFT_ARM] = addOffsetToPuckPose(controller::LEFT_ARM);
|
||||
controller::Pose rightArmPose = addOffsetToPuckPose(controller::RIGHT_ARM);
|
||||
_poseStateMap[controller::RIGHT_ARM] = rightArmPose;
|
||||
if (rightArmPose.isValid()) {
|
||||
// AJT: TODO: compute rotation for RIGHT_SHOULDER AS WELL.
|
||||
}
|
||||
|
||||
controller::Pose leftArmPose = addOffsetToPuckPose(controller::LEFT_ARM);
|
||||
_poseStateMap[controller::LEFT_ARM] = leftArmPose;
|
||||
if (leftArmPose.isValid()) {
|
||||
// AJT: TODO: compute rotation for LEFT_SHOULDER AS WELL.
|
||||
}
|
||||
|
||||
if (_overrideHead) {
|
||||
_poseStateMap[controller::HEAD] = addOffsetToPuckPose(controller::HEAD);
|
||||
|
|
@ -899,6 +915,16 @@ void ViveControllerManager::InputDevice::handlePoseEvent(float deltaTime, const
|
|||
// transform into avatar frame
|
||||
glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;
|
||||
_poseStateMap[isLeftHand ? controller::LEFT_HAND : controller::RIGHT_HAND] = pose.transform(controllerToAvatar);
|
||||
|
||||
// AJT: TODO ONLY DO THIS IF CALIBRATING!
|
||||
// record controller history
|
||||
if (isLeftHand) {
|
||||
_leftControllerHistory[_leftControllerHistoryCursor] = pose.translation;
|
||||
_leftControllerHistoryCursor = (_leftControllerHistoryCursor + 1) % CONTROLLER_HISTORY_SIZE;
|
||||
} else {
|
||||
_rightControllerHistory[_rightControllerHistoryCursor] = pose.translation;
|
||||
_rightControllerHistoryCursor = (_rightControllerHistoryCursor + 1) % CONTROLLER_HISTORY_SIZE;
|
||||
}
|
||||
}
|
||||
|
||||
bool ViveControllerManager::InputDevice::triggerHapticPulse(float strength, float duration, controller::Hand hand) {
|
||||
|
|
@ -950,7 +976,7 @@ void ViveControllerManager::InputDevice::hapticsHelper(float deltaTime, bool lef
|
|||
}
|
||||
}
|
||||
|
||||
void ViveControllerManager::InputDevice::calibrateLeftHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
|
||||
void ViveControllerManager::InputDevice::calibrateLeftHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
|
||||
controller::Pose& handPose = handPair.second;
|
||||
glm::mat4 handPoseAvatarMat = createMatFromQuatAndPos(handPose.getRotation(), handPose.getTranslation());
|
||||
glm::vec3 handPoseTranslation = extractTranslation(handPoseAvatarMat);
|
||||
|
|
@ -981,7 +1007,7 @@ void ViveControllerManager::InputDevice::calibrateLeftHand(glm::mat4& defaultToR
|
|||
_pucksPostOffset[handPair.first] = offsetMat;
|
||||
}
|
||||
|
||||
void ViveControllerManager::InputDevice::calibrateRightHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
|
||||
void ViveControllerManager::InputDevice::calibrateRightHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair) {
|
||||
controller::Pose& handPose = handPair.second;
|
||||
glm::mat4 handPoseAvatarMat = createMatFromQuatAndPos(handPose.getRotation(), handPose.getTranslation());
|
||||
glm::vec3 handPoseTranslation = extractTranslation(handPoseAvatarMat);
|
||||
|
|
@ -1013,7 +1039,7 @@ void ViveControllerManager::InputDevice::calibrateRightHand(glm::mat4& defaultTo
|
|||
}
|
||||
|
||||
|
||||
void ViveControllerManager::InputDevice::calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
void ViveControllerManager::InputDevice::calibrateFeet(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
glm::vec3 headXAxis = getReferenceHeadXAxis(defaultToReferenceMat, inputCalibration.defaultHeadMat);
|
||||
glm::vec3 headPosition = getReferenceHeadPosition(defaultToReferenceMat, inputCalibration.defaultHeadMat);
|
||||
auto& firstFoot = _validTrackedObjects[FIRST_FOOT];
|
||||
|
|
@ -1030,7 +1056,7 @@ void ViveControllerManager::InputDevice::calibrateFeet(glm::mat4& defaultToRefer
|
|||
}
|
||||
}
|
||||
|
||||
void ViveControllerManager::InputDevice::calibrateFoot(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot){
|
||||
void ViveControllerManager::InputDevice::calibrateFoot(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot){
|
||||
controller::Pose footPose = footPair.second;
|
||||
glm::mat4 puckPoseAvatarMat = createMatFromQuatAndPos(footPose.getRotation(), footPose.getTranslation());
|
||||
glm::mat4 defaultFoot = isLeftFoot ? inputCalibration.defaultLeftFoot : inputCalibration.defaultRightFoot;
|
||||
|
|
@ -1052,17 +1078,75 @@ void ViveControllerManager::InputDevice::calibrateFoot(glm::mat4& defaultToRefer
|
|||
}
|
||||
}
|
||||
|
||||
void ViveControllerManager::InputDevice::calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
void ViveControllerManager::InputDevice::calibrateHips(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
_jointToPuckMap[controller::HIPS] = _validTrackedObjects[HIP].first;
|
||||
_pucksPostOffset[_validTrackedObjects[HIP].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultHips, _validTrackedObjects[HIP].second);
|
||||
}
|
||||
|
||||
void ViveControllerManager::InputDevice::calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
void ViveControllerManager::InputDevice::calibrateChest(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
_jointToPuckMap[controller::SPINE2] = _validTrackedObjects[CHEST].first;
|
||||
_pucksPostOffset[_validTrackedObjects[CHEST].first] = computeOffset(defaultToReferenceMat, inputCalibration.defaultSpine2, _validTrackedObjects[CHEST].second);
|
||||
}
|
||||
|
||||
void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration,
|
||||
// assuming the user has kept his arms straight to his sides and has rotated his hand controllers
|
||||
// up and down laterally about his shoulders. This will attempt to locate the users actual shoulders
|
||||
// by calculating a best fit circle around the points, in the plane of the head's forward normal.
|
||||
static glm::vec3 computeUserShoulderPositionFromHistory(const glm::mat4& headMat, const std::vector<glm::vec3>& history) {
|
||||
glm::mat4 invHeadMat = glm::inverse(headMat);
|
||||
|
||||
// AJT: TODO: we could perhaps reject the approximation if the radius is larger or smaller then we expect.
|
||||
// AJT: TODO: abort if history is too small or invalid...
|
||||
|
||||
// Take equation for a circle: (x - h)^2 + (y - k)^2 = r^2
|
||||
// And put it into the linear form: A * x = b
|
||||
//
|
||||
// where A = | (2 * x0) (2 * y0) 1 |
|
||||
// | (2 * x1) (2 * y1) 1 |
|
||||
// | . . . |
|
||||
// | (2 * xn) (2 * yn) 1 |
|
||||
//
|
||||
// and b = | x0^2 + y0^2 |
|
||||
// | x1^2 + y1^2 |
|
||||
// | . |
|
||||
// | xn^2 + yn^2 |
|
||||
//
|
||||
// and x = | h |
|
||||
// | k |
|
||||
// | r^2 - h^2 - k^2 |
|
||||
//
|
||||
|
||||
// Build aMat and bMat
|
||||
Eigen::MatrixXf aMat(CONTROLLER_HISTORY_SIZE, 3);
|
||||
Eigen::MatrixXf bMat(CONTROLLER_HISTORY_SIZE, 1);
|
||||
for (int r = 0; r < CONTROLLER_HISTORY_SIZE; r++) {
|
||||
// transform history[r] into local head coordinates
|
||||
glm::vec3 p = transformPoint(invHeadMat, history[r]);
|
||||
|
||||
// update the aMat with the appropriate 2d history values
|
||||
aMat(r, 0) = 2.0f * p.x;
|
||||
aMat(r, 1) = 2.0f * p.y;
|
||||
aMat(r, 2) = 1.0f;
|
||||
|
||||
// update the bMat with the appropriate 2d history values
|
||||
bMat(r, 0) = p.x * p.x + p.y * p.y;
|
||||
}
|
||||
|
||||
// Then use least squares approximation to solve for the best x.
|
||||
// http://math.mit.edu/~gs/linearalgebra/ila0403.pdf
|
||||
Eigen::MatrixXf aTransMat = aMat.transpose();
|
||||
Eigen::MatrixXf xMat = (aTransMat * aMat).inverse() * aTransMat * bMat;
|
||||
|
||||
// extract the 2d center of the circle from the xMat
|
||||
glm::vec3 center(xMat(0, 0), xMat(1, 0), 0.0f);
|
||||
|
||||
// we don't need the radius, but it's included here for completeness.
|
||||
// float radius = center.x * center.x + center.y * center.y - xMat(2, 0);
|
||||
|
||||
// transform center back into sensor coordinates
|
||||
return transformPoint(headMat, center);
|
||||
}
|
||||
|
||||
void ViveControllerManager::InputDevice::calibrateShoulders(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration,
|
||||
int firstShoulderIndex, int secondShoulderIndex) {
|
||||
const PuckPosePair& firstShoulder = _validTrackedObjects[firstShoulderIndex];
|
||||
const PuckPosePair& secondShoulder = _validTrackedObjects[secondShoulderIndex];
|
||||
|
|
@ -1071,18 +1155,18 @@ void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultTo
|
|||
|
||||
glm::mat4 refLeftArm = defaultToReferenceMat * inputCalibration.defaultLeftArm;
|
||||
glm::mat4 refRightArm = defaultToReferenceMat * inputCalibration.defaultRightArm;
|
||||
|
||||
glm::mat4 userRefLeftArm = refLeftArm;
|
||||
glm::mat4 userRefRightArm = refRightArm;
|
||||
const float PUCK_TO_USER_REF_ARM_Y_OFFSET = -0.06f;
|
||||
|
||||
glm::mat4 headMat = defaultToReferenceMat * inputCalibration.defaultHeadMat;
|
||||
userRefLeftArm[3] = glm::vec4(computeUserShoulderPositionFromHistory(headMat, _leftControllerHistory), 1.0f);
|
||||
userRefRightArm[3] = glm::vec4(computeUserShoulderPositionFromHistory(headMat, _rightControllerHistory), 1.0f);
|
||||
|
||||
if (firstShoulderPose.translation.x < secondShoulderPose.translation.x) {
|
||||
_jointToPuckMap[controller::LEFT_ARM] = firstShoulder.first;
|
||||
_jointToPuckMap[controller::RIGHT_ARM] = secondShoulder.first;
|
||||
|
||||
// move the userRefArm to the same height as the puck.
|
||||
userRefLeftArm[3][1] = firstShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET;
|
||||
userRefRightArm[3][1] = secondShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET;
|
||||
|
||||
// compute the post offset from the userRefArm
|
||||
_pucksPostOffset[firstShoulder.first] = computeOffset(Matrices::IDENTITY, userRefLeftArm, firstShoulderPose);
|
||||
_pucksPostOffset[secondShoulder.first] = computeOffset(Matrices::IDENTITY, userRefRightArm, secondShoulderPose);
|
||||
|
|
@ -1095,10 +1179,6 @@ void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultTo
|
|||
_jointToPuckMap[controller::LEFT_ARM] = secondShoulder.first;
|
||||
_jointToPuckMap[controller::RIGHT_ARM] = firstShoulder.first;
|
||||
|
||||
// move the userRefArm to the same height as the puck
|
||||
userRefLeftArm[3][1] = secondShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET;
|
||||
userRefRightArm[3][1] = firstShoulderPose.translation.y + PUCK_TO_USER_REF_ARM_Y_OFFSET;
|
||||
|
||||
// compute the post offset from the userRefArm
|
||||
_pucksPostOffset[secondShoulder.first] = computeOffset(Matrices::IDENTITY, userRefLeftArm, secondShoulderPose);
|
||||
_pucksPostOffset[firstShoulder.first] = computeOffset(Matrices::IDENTITY, userRefRightArm, firstShoulderPose);
|
||||
|
|
@ -1110,7 +1190,7 @@ void ViveControllerManager::InputDevice::calibrateShoulders(glm::mat4& defaultTo
|
|||
}
|
||||
}
|
||||
|
||||
void ViveControllerManager::InputDevice::calibrateHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
void ViveControllerManager::InputDevice::calibrateHead(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration) {
|
||||
size_t headIndex = _validTrackedObjects.size() - 1;
|
||||
const PuckPosePair& head = _validTrackedObjects[headIndex];
|
||||
_jointToPuckMap[controller::HEAD] = head.first;
|
||||
|
|
|
|||
|
|
@ -93,18 +93,18 @@ private:
|
|||
void partitionTouchpad(int sButton, int xAxis, int yAxis, int centerPsuedoButton, int xPseudoButton, int yPseudoButton);
|
||||
void printDeviceTrackingResultChange(uint32_t deviceIndex);
|
||||
void setConfigFromString(const QString& value);
|
||||
bool configureHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
bool configureHands(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
bool configureBody(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
void calibrateLeftHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair);
|
||||
void calibrateRightHand(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair);
|
||||
void calibrateFeet(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
void calibrateFoot(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot);
|
||||
void calibrateHips(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
void calibrateChest(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
void calibrateShoulders(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration,
|
||||
bool configureHead(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
bool configureHands(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
bool configureBody(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
void calibrateLeftHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair);
|
||||
void calibrateRightHand(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& handPair);
|
||||
void calibrateFeet(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
void calibrateFoot(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration, PuckPosePair& footPair, bool isLeftFoot);
|
||||
void calibrateHips(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
void calibrateChest(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
void calibrateShoulders(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration,
|
||||
int firstShoulderIndex, int secondShoulderIndex);
|
||||
void calibrateHead(glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
void calibrateHead(const glm::mat4& defaultToReferenceMat, const controller::InputCalibrationData& inputCalibration);
|
||||
void calibrateFromHandController(const controller::InputCalibrationData& inputCalibrationData);
|
||||
void calibrateFromUI(const controller::InputCalibrationData& inputCalibrationData);
|
||||
void emitCalibrationStatus();
|
||||
|
|
@ -190,6 +190,11 @@ private:
|
|||
bool _overrideHands { false };
|
||||
mutable std::recursive_mutex _lock;
|
||||
|
||||
std::vector<glm::vec3> _leftControllerHistory;
|
||||
size_t _leftControllerHistoryCursor { 0 };
|
||||
std::vector<glm::vec3> _rightControllerHistory;
|
||||
size_t _rightControllerHistoryCursor { 0 };
|
||||
|
||||
QString configToString(Config config);
|
||||
friend class ViveControllerManager;
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in a new issue