From 84260192fb812f47bf134f221f17a5e3da8e72c5 Mon Sep 17 00:00:00 2001 From: Anthony Thibault Date: Fri, 25 Dec 2015 09:57:50 -0800 Subject: [PATCH] Set up controller poses to match hifi standard skeleton Neuron plugin in fills in the gap (thumb1) with identity. Updated neuronAvatar script to work with new controller pose names. --- examples/controllers/neuron/neuronAvatar.js | 97 +----- .../src/controllers/StandardControls.h | 20 +- plugins/hifiNeuron/src/NeuronPlugin.cpp | 324 +++++++++++------- plugins/hifiNeuron/src/NeuronPlugin.h | 10 +- 4 files changed, 245 insertions(+), 206 deletions(-) diff --git a/examples/controllers/neuron/neuronAvatar.js b/examples/controllers/neuron/neuronAvatar.js index fae46330b1..cc53987f05 100644 --- a/examples/controllers/neuron/neuronAvatar.js +++ b/examples/controllers/neuron/neuronAvatar.js @@ -1,66 +1,3 @@ -// maps controller joint names to avatar joint names -var JOINT_NAME_MAP = { - Hips: "Hips", - RightUpLeg: "RightUpLeg", - RightLeg: "RightLeg", - RightFoot: "RightFoot", - LeftUpLeg: "LeftUpLeg", - LeftLeg: "LeftLeg", - LeftFoot: "LeftFoot", - Spine: "Spine", - Spine1: "Spine1", - Spine2: "Spine2", - Spine3: "Spine3", - Neck: "Neck", - Head: "Head", - RightShoulder: "RightShoulder", - RightArm: "RightArm", - RightForeArm: "RightForeArm", - RightHand: "RightHand", - RightHandThumb1: "RightHandThumb2", - RightHandThumb2: "RightHandThumb3", - RightHandThumb3: "RightHandThumb4", - RightInHandIndex: "RightHandIndex1", - RightHandIndex1: "RightHandIndex2", - RightHandIndex2: "RightHandIndex3", - RightHandIndex3: "RightHandIndex4", - RightInHandMiddle: "RightHandMiddle1", - RightHandMiddle1: "RightHandMiddle2", - RightHandMiddle2: "RightHandMiddle3", - RightHandMiddle3: "RightHandMiddle4", - RightInHandRing: "RightHandRing1", - RightHandRing1: "RightHandRing2", - RightHandRing2: "RightHandRing3", - RightHandRing3: "RightHandRing4", - RightInHandPinky: "RightHandPinky1", - RightHandPinky1: "RightHandPinky2", - RightHandPinky2: "RightHandPinky3", - RightHandPinky3: "RightHandPinky4", - LeftShoulder: "LeftShoulder", - LeftArm: "LeftArm", - LeftForeArm: "LeftForeArm", - LeftHand: "LeftHand", - LeftHandThumb1: "LeftHandThumb2", - LeftHandThumb2: "LeftHandThumb3", - LeftHandThumb3: "LeftHandThumb4", - LeftInHandIndex: "LeftHandIndex1", - LeftHandIndex1: "LeftHandIndex2", - LeftHandIndex2: "LeftHandIndex3", - LeftHandIndex3: "LeftHandIndex4", - LeftInHandMiddle: "LeftHandMiddle1", - LeftHandMiddle1: "LeftHandMiddle2", - LeftHandMiddle2: "LeftHandMiddle3", - LeftHandMiddle3: "LeftHandMiddle4", - LeftInHandRing: "LeftHandRing1", - LeftHandRing1: "LeftHandRing2", - LeftHandRing2: "LeftHandRing3", - LeftHandRing3: "LeftHandRing4", - LeftInHandPinky: "LeftHandPinky1", - LeftHandPinky1: "LeftHandPinky2", - LeftHandPinky2: "LeftHandPinky3", - LeftHandPinky3: "LeftHandPinky4" -}; - var JOINT_PARENT_MAP = { Hips: "", RightUpLeg: "Hips", @@ -170,10 +107,11 @@ NeuronAvatar.prototype.activate = function () { // build absDefaultPoseMap this._absDefaultRotMap = {}; - var keys = Object.keys(JOINT_NAME_MAP); + this._absDefaultRotMap[""] = {x: 0, y: 0, z: 0, w: 1}; + var keys = Object.keys(JOINT_PARENT_MAP); var i, l = keys.length; for (i = 0; i < l; i++) { - var jointName = JOINT_NAME_MAP[keys[i]]; + var jointName = keys[i]; var j = MyAvatar.getJointIndex(jointName); var parentJointName = JOINT_PARENT_MAP[jointName]; if (parentJointName === "") { @@ -194,24 +132,27 @@ NeuronAvatar.prototype.deactivate = function () { }; NeuronAvatar.prototype.update = function (deltaTime) { - var keys = Object.keys(JOINT_NAME_MAP); + var keys = Object.keys(JOINT_PARENT_MAP); var i, l = keys.length; var absDefaultRot = {}; + var jointName, channel, pose, parentJointName, j, parentDefaultAbsRot; for (i = 0; i < l; i++) { - var channel = Controller.Hardware.Neuron[keys[i]]; + var jointName = keys[i]; + var channel = Controller.Hardware.Neuron[jointName]; if (channel) { - var pose = Controller.getPoseValue(channel); - var jointName = JOINT_NAME_MAP[keys[i]]; - var parentJointName = JOINT_PARENT_MAP[jointName]; - var j = MyAvatar.getJointIndex(jointName); - var defaultAbsRot = this._absDefaultRotMap[jointName]; - var parentDefaultAbsRot; - if (parentJointName === "") { - parentDefaultAbsRot = {x: 0, y: 0, z: 0, w: 1}; - } else { - parentDefaultAbsRot = this._absDefaultRotMap[parentJointName]; - } + pose = Controller.getPoseValue(channel); + parentJointName = JOINT_PARENT_MAP[jointName]; + j = MyAvatar.getJointIndex(jointName); + defaultAbsRot = this._absDefaultRotMap[jointName]; + parentDefaultAbsRot = this._absDefaultRotMap[parentJointName]; + + // Rotations from the neuron controller are in world orientation but are delta's from the default pose. + // So first we build the absolute rotation of the default pose (local into world). + // Then apply the rotation from the controller, in world space. + // Then we transform back into joint local by multiplying by the inverse of the parents absolute rotation. MyAvatar.setJointRotation(j, Quat.multiply(Quat.inverse(parentDefaultAbsRot), Quat.multiply(pose.rotation, defaultAbsRot))); + + // TODO: //MyAvatar.setJointTranslation(j, Vec3.multiply(100.0, pose.translation)); } } diff --git a/libraries/controllers/src/controllers/StandardControls.h b/libraries/controllers/src/controllers/StandardControls.h index 4294713238..2b0613321e 100644 --- a/libraries/controllers/src/controllers/StandardControls.h +++ b/libraries/controllers/src/controllers/StandardControls.h @@ -108,22 +108,23 @@ namespace controller { RIGHT_HAND_THUMB1, RIGHT_HAND_THUMB2, RIGHT_HAND_THUMB3, - RIGHT_IN_HAND_INDEX, + RIGHT_HAND_THUMB4, RIGHT_HAND_INDEX1, RIGHT_HAND_INDEX2, RIGHT_HAND_INDEX3, - RIGHT_IN_HAND_MIDDLE, + RIGHT_HAND_INDEX4, RIGHT_HAND_MIDDLE1, RIGHT_HAND_MIDDLE2, RIGHT_HAND_MIDDLE3, - RIGHT_IN_HANDRING, + RIGHT_HAND_MIDDLE4, RIGHT_HAND_RING1, RIGHT_HAND_RING2, RIGHT_HAND_RING3, - RIGHT_IN_HAND_PINKY, + RIGHT_HAND_RING4, RIGHT_HAND_PINKY1, RIGHT_HAND_PINKY2, RIGHT_HAND_PINKY3, + RIGHT_HAND_PINKY4, LEFT_SHOULDER, LEFT_ARM, LEFT_FORE_ARM, @@ -131,28 +132,29 @@ namespace controller { LEFT_HAND_THUMB1, LEFT_HAND_THUMB2, LEFT_HAND_THUMB3, - LEFT_IN_HAND_INDEX, + LEFT_HAND_THUMB4, LEFT_HAND_INDEX1, LEFT_HAND_INDEX2, LEFT_HAND_INDEX3, - LEFT_IN_HAND_MIDDLE, + LEFT_HAND_INDEX4, LEFT_HAND_MIDDLE1, LEFT_HAND_MIDDLE2, LEFT_HAND_MIDDLE3, - LEFT_IN_HAND_RING, + LEFT_HAND_MIDDLE4, LEFT_HAND_RING1, LEFT_HAND_RING2, LEFT_HAND_RING3, - LEFT_IN_HAND_PINKY, + LEFT_HAND_RING4, LEFT_HAND_PINKY1, LEFT_HAND_PINKY2, LEFT_HAND_PINKY3, + LEFT_HAND_PINKY4, NUM_STANDARD_POSES }; enum StandardCounts { TRIGGERS = 2, ANALOG_STICKS = 2, - POSES = 2, // FIXME 3? if we want to expose the head? + POSES = NUM_STANDARD_POSES }; } diff --git a/plugins/hifiNeuron/src/NeuronPlugin.cpp b/plugins/hifiNeuron/src/NeuronPlugin.cpp index ae438c10e0..4edda64c22 100644 --- a/plugins/hifiNeuron/src/NeuronPlugin.cpp +++ b/plugins/hifiNeuron/src/NeuronPlugin.cpp @@ -29,8 +29,10 @@ Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") const QString NeuronPlugin::NAME = "Neuron"; const QString NeuronPlugin::NEURON_ID_STRING = "Perception Neuron"; -// This matches controller::StandardPoseChannel -enum JointIndex { +// indices of joints of the Neuron standard skeleton. +// This is 'almost' the same as the High Fidelity standard skeleton. +// It is missing a thumb joint. +enum NeuronJointIndex { Hips = 0, RightUpLeg, RightLeg, @@ -93,12 +95,160 @@ enum JointIndex { Size }; -bool NeuronPlugin::isSupported() const { - // Because it's a client/server network architecture, we can't tell - // if the neuron is actually connected until we connect to the server. - return true; +// Almost a direct mapping except for LEFT_HAND_THUMB1 and RIGHT_HAND_THUMB1, +// which are not present in the Neuron standard skeleton. +static controller::StandardPoseChannel neuronJointIndexToPoseIndexMap[NeuronJointIndex::Size] = { + controller::HIPS, + controller::RIGHT_UP_LEG, + controller::RIGHT_LEG, + controller::RIGHT_FOOT, + controller::LEFT_UP_LEG, + controller::LEFT_LEG, + controller::LEFT_FOOT, + controller::SPINE, + controller::SPINE1, + controller::SPINE2, + controller::SPINE3, + controller::NECK, + controller::HEAD, + controller::RIGHT_SHOULDER, + controller::RIGHT_ARM, + controller::RIGHT_FORE_ARM, + controller::RIGHT_HAND, + controller::RIGHT_HAND_THUMB2, + controller::RIGHT_HAND_THUMB3, + controller::RIGHT_HAND_THUMB4, + controller::RIGHT_HAND_INDEX1, + controller::RIGHT_HAND_INDEX2, + controller::RIGHT_HAND_INDEX3, + controller::RIGHT_HAND_INDEX4, + controller::RIGHT_HAND_MIDDLE1, + controller::RIGHT_HAND_MIDDLE2, + controller::RIGHT_HAND_MIDDLE3, + controller::RIGHT_HAND_MIDDLE4, + controller::RIGHT_HAND_RING1, + controller::RIGHT_HAND_RING2, + controller::RIGHT_HAND_RING3, + controller::RIGHT_HAND_RING4, + controller::RIGHT_HAND_PINKY1, + controller::RIGHT_HAND_PINKY2, + controller::RIGHT_HAND_PINKY3, + controller::RIGHT_HAND_PINKY4, + controller::LEFT_SHOULDER, + controller::LEFT_ARM, + controller::LEFT_FORE_ARM, + controller::LEFT_HAND, + controller::LEFT_HAND_THUMB2, + controller::LEFT_HAND_THUMB3, + controller::LEFT_HAND_THUMB4, + controller::LEFT_HAND_INDEX1, + controller::LEFT_HAND_INDEX2, + controller::LEFT_HAND_INDEX3, + controller::LEFT_HAND_INDEX4, + controller::LEFT_HAND_MIDDLE1, + controller::LEFT_HAND_MIDDLE2, + controller::LEFT_HAND_MIDDLE3, + controller::LEFT_HAND_MIDDLE4, + controller::LEFT_HAND_RING1, + controller::LEFT_HAND_RING2, + controller::LEFT_HAND_RING3, + controller::LEFT_HAND_RING4, + controller::LEFT_HAND_PINKY1, + controller::LEFT_HAND_PINKY2, + controller::LEFT_HAND_PINKY3, + controller::LEFT_HAND_PINKY4 +}; + +static controller::StandardPoseChannel neuronJointIndexToPoseIndex(NeuronJointIndex i) { + assert(i >= 0 && i < NeuronJointIndex::Size); + if (i >= 0 && i < NeuronJointIndex::Size) { + return neuronJointIndexToPoseIndexMap[i]; + } else { + return (controller::StandardPoseChannel)0; // not sure what to do here, but don't crash! + } } +static const char* controllerJointName(controller::StandardPoseChannel i) { + switch (i) { + case controller::HIPS: return "Hips"; + case controller::RIGHT_UP_LEG: return "RightUpLeg"; + case controller::RIGHT_LEG: return "RightLeg"; + case controller::RIGHT_FOOT: return "RightFoot"; + case controller::LEFT_UP_LEG: return "LeftUpLeg"; + case controller::LEFT_LEG: return "LeftLeg"; + case controller::LEFT_FOOT: return "LeftFoot"; + case controller::SPINE: return "Spine"; + case controller::SPINE1: return "Spine1"; + case controller::SPINE2: return "Spine2"; + case controller::SPINE3: return "Spine3"; + case controller::NECK: return "Neck"; + case controller::HEAD: return "Head"; + case controller::RIGHT_SHOULDER: return "RightShoulder"; + case controller::RIGHT_ARM: return "RightArm"; + case controller::RIGHT_FORE_ARM: return "RightForeArm"; + case controller::RIGHT_HAND: return "RightHand"; + case controller::RIGHT_HAND_THUMB1: return "RightHandThumb1"; + case controller::RIGHT_HAND_THUMB2: return "RightHandThumb2"; + case controller::RIGHT_HAND_THUMB3: return "RightHandThumb3"; + case controller::RIGHT_HAND_THUMB4: return "RightHandThumb4"; + case controller::RIGHT_HAND_INDEX1: return "RightHandIndex1"; + case controller::RIGHT_HAND_INDEX2: return "RightHandIndex2"; + case controller::RIGHT_HAND_INDEX3: return "RightHandIndex3"; + case controller::RIGHT_HAND_INDEX4: return "RightHandIndex4"; + case controller::RIGHT_HAND_MIDDLE1: return "RightHandMiddle1"; + case controller::RIGHT_HAND_MIDDLE2: return "RightHandMiddle2"; + case controller::RIGHT_HAND_MIDDLE3: return "RightHandMiddle3"; + case controller::RIGHT_HAND_MIDDLE4: return "RightHandMiddle4"; + case controller::RIGHT_HAND_RING1: return "RightHandRing1"; + case controller::RIGHT_HAND_RING2: return "RightHandRing2"; + case controller::RIGHT_HAND_RING3: return "RightHandRing3"; + case controller::RIGHT_HAND_RING4: return "RightHandRing4"; + case controller::RIGHT_HAND_PINKY1: return "RightHandPinky1"; + case controller::RIGHT_HAND_PINKY2: return "RightHandPinky2"; + case controller::RIGHT_HAND_PINKY3: return "RightHandPinky3"; + case controller::RIGHT_HAND_PINKY4: return "RightHandPinky4"; + case controller::LEFT_SHOULDER: return "LeftShoulder"; + case controller::LEFT_ARM: return "LeftArm"; + case controller::LEFT_FORE_ARM: return "LeftForeArm"; + case controller::LEFT_HAND: return "LeftHand"; + case controller::LEFT_HAND_THUMB1: return "LeftHandThumb1"; + case controller::LEFT_HAND_THUMB2: return "LeftHandThumb2"; + case controller::LEFT_HAND_THUMB3: return "LeftHandThumb3"; + case controller::LEFT_HAND_THUMB4: return "LeftHandThumb4"; + case controller::LEFT_HAND_INDEX1: return "LeftHandIndex1"; + case controller::LEFT_HAND_INDEX2: return "LeftHandIndex2"; + case controller::LEFT_HAND_INDEX3: return "LeftHandIndex3"; + case controller::LEFT_HAND_INDEX4: return "LeftHandIndex4"; + case controller::LEFT_HAND_MIDDLE1: return "LeftHandMiddle1"; + case controller::LEFT_HAND_MIDDLE2: return "LeftHandMiddle2"; + case controller::LEFT_HAND_MIDDLE3: return "LeftHandMiddle3"; + case controller::LEFT_HAND_MIDDLE4: return "LeftHandMiddle4"; + case controller::LEFT_HAND_RING1: return "LeftHandRing1"; + case controller::LEFT_HAND_RING2: return "LeftHandRing2"; + case controller::LEFT_HAND_RING3: return "LeftHandRing3"; + case controller::LEFT_HAND_RING4: return "LeftHandRing4"; + case controller::LEFT_HAND_PINKY1: return "LeftHandPinky1"; + case controller::LEFT_HAND_PINKY2: return "LeftHandPinky2"; + case controller::LEFT_HAND_PINKY3: return "LeftHandPinky3"; + case controller::LEFT_HAND_PINKY4: return "LeftHandPinky4"; + default: return "???"; + } +} + +// convert between YXZ neuron euler angles in degrees to quaternion +// this is the default setting in the Axis Neuron server. +static quat eulerToQuat(vec3 euler) { + // euler.x and euler.y are swaped, WTF. + glm::vec3 e = glm::vec3(euler.y, euler.x, euler.z) * RADIANS_PER_DEGREE; + return (glm::angleAxis(e.y, Vectors::UNIT_Y) * + glm::angleAxis(e.x, Vectors::UNIT_X) * + glm::angleAxis(e.z, Vectors::UNIT_Z)); +} + +// +// neuronDataReader SDK callback functions +// + // NOTE: must be thread-safe void FrameDataReceivedCallback(void* context, SOCKET_REF sender, BvhDataHeaderEx* header, float* data) { @@ -133,7 +283,28 @@ void FrameDataReceivedCallback(void* context, SOCKET_REF sender, BvhDataHeaderEx // copy the data memcpy(&(neuronPlugin->_joints[0]), data, sizeof(NeuronPlugin::NeuronJoint) * NUM_JOINTS); + } else { + // enter mutex + std::lock_guard guard(neuronPlugin->_jointsMutex); + + // + // Data is 3 floats per joint: 3 rotation euler angles (degrees) + // + + // resize vector if necessary + const size_t NUM_FLOATS_PER_JOINT = 3; + const size_t NUM_JOINTS = header->DataCount / NUM_FLOATS_PER_JOINT; + if (neuronPlugin->_joints.size() != NUM_JOINTS) { + neuronPlugin->_joints.resize(NUM_JOINTS, { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } }); + } + + assert(sizeof(NeuronPlugin::NeuronJoint) == (NUM_FLOATS_PER_JOINT * sizeof(float))); + + for (int i = 0; i < NUM_JOINTS; i++) { + // TODO: should probably just copy over default positions?! + memcpy(&(neuronPlugin->_joints[i].euler), data, sizeof(float) * NUM_FLOATS_PER_JOINT); + } qCCritical(inputplugins) << "NeruonPlugin: format not supported"; @@ -148,6 +319,9 @@ void FrameDataReceivedCallback(void* context, SOCKET_REF sender, BvhDataHeaderEx } } +// I can't even get the SDK to send me a callback. +// BRCommandFetchAvatarDataFromServer & BRRegisterAutoSyncParmeter [sic] don't seem to work. +// So this is totally untested. // NOTE: must be thread-safe static void CommandDataReceivedCallback(void* context, SOCKET_REF sender, CommandPack* pack, void* data) { @@ -196,9 +370,20 @@ static void CommandDataReceivedCallback(void* context, SOCKET_REF sender, Comman // NOTE: must be thread-safe static void SocketStatusChangedCallback(void* context, SOCKET_REF sender, SocketStatus status, char* message) { + // just dump to log, later we might want to pop up a connection lost dialog or attempt to reconnect. qCDebug(inputplugins) << "NeuronPlugin: socket status = " << message; } +// +// NeuronPlugin +// + +bool NeuronPlugin::isSupported() const { + // Because it's a client/server network architecture, we can't tell + // if the neuron is actually connected until we connect to the server. + return true; +} + void NeuronPlugin::activate() { InputPlugin::activate(); @@ -206,28 +391,26 @@ void NeuronPlugin::activate() { auto userInputMapper = DependencyManager::get(); userInputMapper->registerDevice(_inputDevice); - qCDebug(inputplugins) << "NeuronPlugin::activate"; - // register c-style callbacks BRRegisterFrameDataCallback((void*)this, FrameDataReceivedCallback); BRRegisterCommandDataCallback((void*)this, CommandDataReceivedCallback); BRRegisterSocketStatusCallback((void*)this, SocketStatusChangedCallback); - // TODO: pull these from prefs! + // TODO: Pull these from prefs dialog? + // localhost is fine for now. _serverAddress = "localhost"; - _serverPort = 7001; + _serverPort = 7001; // default port for TCP Axis Neuron server. + _socketRef = BRConnectTo((char*)_serverAddress.c_str(), _serverPort); if (!_socketRef) { // error - qCCritical(inputplugins) << "NeuronPlugin: error connecting to " << _serverAddress.c_str() << ":" << _serverPort << "error = " << BRGetLastErrorMessage(); + qCCritical(inputplugins) << "NeuronPlugin: error connecting to " << _serverAddress.c_str() << ":" << _serverPort << ", error = " << BRGetLastErrorMessage(); } else { qCDebug(inputplugins) << "NeuronPlugin: success connecting to " << _serverAddress.c_str() << ":" << _serverPort; } } void NeuronPlugin::deactivate() { - qCDebug(inputplugins) << "NeuronPlugin::deactivate"; - // unregister from userInputMapper if (_inputDevice->_deviceID != controller::Input::INVALID_DEVICE) { auto userInputMapper = DependencyManager::get(); @@ -241,126 +424,35 @@ void NeuronPlugin::deactivate() { InputPlugin::deactivate(); } -// convert between euler in degrees to quaternion -static quat eulerToQuat(vec3 euler) { - // euler.x and euler.y are swaped (thanks NOMICOM!) - glm::vec3 e = glm::vec3(euler.y, euler.x, euler.z) * RADIANS_PER_DEGREE; - return (glm::angleAxis(e.y, Vectors::UNIT_Y) * glm::angleAxis(e.x, Vectors::UNIT_X) * glm::angleAxis(e.z, Vectors::UNIT_Z)); -} - void NeuronPlugin::pluginUpdate(float deltaTime, bool jointsCaptured) { - std::vector joints; - // copy the shared data { + // lock and copy std::lock_guard guard(_jointsMutex); joints = _joints; } - - /* - DebugDraw::getInstance().addMyAvatarMarker("LEFT_FOOT", - eulerToQuat(joints[6].euler), - joints[6].pos / 100.0f, - glm::vec4(1)); - */ _inputDevice->update(deltaTime, joints, _prevJoints); _prevJoints = joints; } void NeuronPlugin::saveSettings() const { InputPlugin::saveSettings(); - // TODO: - qCDebug(inputplugins) << "NeuronPlugin::saveSettings"; } void NeuronPlugin::loadSettings() { InputPlugin::loadSettings(); - // TODO: - qCDebug(inputplugins) << "NeuronPlugin::loadSettings"; } // // InputDevice // -static controller::StandardPoseChannel neuronJointIndexToPoseIndex(JointIndex i) { - // Currently they are the same. - // but that won't always be the case... - return (controller::StandardPoseChannel)i; -} - -static const char* neuronJointName(JointIndex i) { - switch (i) { - case Hips: return "Hips"; - case RightUpLeg: return "RightUpLeg"; - case RightLeg: return "RightLeg"; - case RightFoot: return "RightFoot"; - case LeftUpLeg: return "LeftUpLeg"; - case LeftLeg: return "LeftLeg"; - case LeftFoot: return "LeftFoot"; - case Spine: return "Spine"; - case Spine1: return "Spine1"; - case Spine2: return "Spine2"; - case Spine3: return "Spine3"; - case Neck: return "Neck"; - case Head: return "Head"; - case RightShoulder: return "RightShoulder"; - case RightArm: return "RightArm"; - case RightForeArm: return "RightForeArm"; - case RightHand: return "RightHand"; - case RightHandThumb1: return "RightHandThumb1"; - case RightHandThumb2: return "RightHandThumb2"; - case RightHandThumb3: return "RightHandThumb3"; - case RightInHandIndex: return "RightInHandIndex"; - case RightHandIndex1: return "RightHandIndex1"; - case RightHandIndex2: return "RightHandIndex2"; - case RightHandIndex3: return "RightHandIndex3"; - case RightInHandMiddle: return "RightInHandMiddle"; - case RightHandMiddle1: return "RightHandMiddle1"; - case RightHandMiddle2: return "RightHandMiddle2"; - case RightHandMiddle3: return "RightHandMiddle3"; - case RightInHandRing: return "RightInHandRing"; - case RightHandRing1: return "RightHandRing1"; - case RightHandRing2: return "RightHandRing2"; - case RightHandRing3: return "RightHandRing3"; - case RightInHandPinky: return "RightInHandPinky"; - case RightHandPinky1: return "RightHandPinky1"; - case RightHandPinky2: return "RightHandPinky2"; - case RightHandPinky3: return "RightHandPinky3"; - case LeftShoulder: return "LeftShoulder"; - case LeftArm: return "LeftArm"; - case LeftForeArm: return "LeftForeArm"; - case LeftHand: return "LeftHand"; - case LeftHandThumb1: return "LeftHandThumb1"; - case LeftHandThumb2: return "LeftHandThumb2"; - case LeftHandThumb3: return "LeftHandThumb3"; - case LeftInHandIndex: return "LeftInHandIndex"; - case LeftHandIndex1: return "LeftHandIndex1"; - case LeftHandIndex2: return "LeftHandIndex2"; - case LeftHandIndex3: return "LeftHandIndex3"; - case LeftInHandMiddle: return "LeftInHandMiddle"; - case LeftHandMiddle1: return "LeftHandMiddle1"; - case LeftHandMiddle2: return "LeftHandMiddle2"; - case LeftHandMiddle3: return "LeftHandMiddle3"; - case LeftInHandRing: return "LeftInHandRing"; - case LeftHandRing1: return "LeftHandRing1"; - case LeftHandRing2: return "LeftHandRing2"; - case LeftHandRing3: return "LeftHandRing3"; - case LeftInHandPinky: return "LeftInHandPinky"; - case LeftHandPinky1: return "LeftHandPinky1"; - case LeftHandPinky2: return "LeftHandPinky2"; - case LeftHandPinky3: return "LeftHandPinky3"; - default: return "???"; - } -} - controller::Input::NamedVector NeuronPlugin::InputDevice::getAvailableInputs() const { - // TODO: static controller::Input::NamedVector availableInputs; - if (availableInputs.size() == 0) { - for (int i = 0; i < JointIndex::Size; i++) { - availableInputs.push_back(makePair(neuronJointIndexToPoseIndex((JointIndex)i), neuronJointName((JointIndex)i))); + for (int i = 0; i < controller::NUM_STANDARD_POSES; i++) { + auto channel = static_cast(i); + availableInputs.push_back(makePair(channel, controllerJointName(channel))); } }; return availableInputs; @@ -373,21 +465,21 @@ QString NeuronPlugin::InputDevice::getDefaultMappingConfig() const { void NeuronPlugin::InputDevice::update(float deltaTime, const std::vector& joints, const std::vector& prevJoints) { for (int i = 0; i < joints.size(); i++) { - int poseIndex = neuronJointIndexToPoseIndex((JointIndex)i); glm::vec3 linearVel, angularVel; glm::vec3 pos = (joints[i].pos * METERS_PER_CENTIMETER); glm::quat rot = eulerToQuat(joints[i].euler); if (i < prevJoints.size()) { - linearVel = (pos - (prevJoints[i].pos * METERS_PER_CENTIMETER)) / deltaTime; - // quat log imag part points along the axis of rotation, and it's length will be the half angle. + linearVel = (pos - (prevJoints[i].pos * METERS_PER_CENTIMETER)) / deltaTime; // m/s + // quat log imaginary part points along the axis of rotation, with length of one half the angle of rotation. glm::quat d = glm::log(rot * glm::inverse(eulerToQuat(prevJoints[i].euler))); - angularVel = glm::vec3(d.x, d.y, d.z) / (0.5f * deltaTime); + angularVel = glm::vec3(d.x, d.y, d.z) / (0.5f * deltaTime); // radians/s } + int poseIndex = neuronJointIndexToPoseIndex((NeuronJointIndex)i); _poseStateMap[poseIndex] = controller::Pose(pos, rot, linearVel, angularVel); } -} -void NeuronPlugin::InputDevice::focusOutEvent() { - // TODO: - qCDebug(inputplugins) << "NeuronPlugin::InputDevice::focusOutEvent"; + // fill in missing thumb joints with identity. + // TODO: need a standard displacement + _poseStateMap[controller::RIGHT_HAND_THUMB1] = controller::Pose(glm::vec3(), glm::quat(), glm::vec3(), glm::vec3()); + _poseStateMap[controller::LEFT_HAND_THUMB1] = controller::Pose(glm::vec3(), glm::quat(), glm::vec3(), glm::vec3()); } diff --git a/plugins/hifiNeuron/src/NeuronPlugin.h b/plugins/hifiNeuron/src/NeuronPlugin.h index f787838ce2..c85a5dd383 100644 --- a/plugins/hifiNeuron/src/NeuronPlugin.h +++ b/plugins/hifiNeuron/src/NeuronPlugin.h @@ -57,7 +57,7 @@ protected: virtual controller::Input::NamedVector getAvailableInputs() const override; virtual QString getDefaultMappingConfig() const override; virtual void update(float deltaTime, bool jointsCaptured) override {}; - virtual void focusOutEvent() override; + virtual void focusOutEvent() override {}; void update(float deltaTime, const std::vector& joints, const std::vector& prevJoints); }; @@ -71,9 +71,13 @@ protected: int _serverPort; void* _socketRef; - std::vector _joints; - std::mutex _jointsMutex; // used to guard access to _joints + // used to guard multi-threaded access to _joints + std::mutex _jointsMutex; + // copy of data directly from the NeuronDataReader SDK + std::vector _joints; + + // one frame old copy of _joints, used to caluclate angular and linear velocity. std::vector _prevJoints; };