diff --git a/src/AngVelTools.h b/src/AngVelTools.h new file mode 100644 index 0000000..3bc99c0 --- /dev/null +++ b/src/AngVelTools.h @@ -0,0 +1,62 @@ +/** @file + @brief Header + + @date 2016 + + @author + Sensics, Inc. + +*/ + +// Copyright 2016 Sensics, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef INCLUDED_AngVelTools_h_GUID_628878EF_DAFC_4486_36BC_6C47BD452AB0 +#define INCLUDED_AngVelTools_h_GUID_628878EF_DAFC_4486_36BC_6C47BD452AB0 + +// Internal Includes +// - none + +// Library/third-party includes +#include +#include + +// Standard includes +#include + +namespace osvr { +namespace vbtracker { + /// use only for derivatives - has factor of 2/0.5 in it! + // inline Eigen::Quaterniond + // angVelVecToIncRot(Eigen::Vector3d const &angVelVec, double dt) { + // return util::ei_quat_exp_map::quat_exp(angVelVec * dt * 0.5).normalized(); + // } +// + /// use only for derivatives - has factor of 2/0.5 in it! + inline Eigen::Vector3d incRotToAngVelVec(Eigen::Quaterniond const &incRot, + double dt) { +#if 0 + if (incRot.w() >= 1. || incRot.vec().isZero(1e-10)) { + return Eigen::Vector3d::Zero(); + } + auto angle = std::acos(incRot.w()); + return incRot.vec().normalized() * angle * 2. / dt; +#else + return util::ei_quat_exp_map::quat_ln(incRot) * 2. / dt; +#endif + } + +} // namespace vbtracker +} // namespace osvr +#endif // INCLUDED_AngVelTools_h_GUID_628878EF_DAFC_4486_36BC_6C47BD452AB0 diff --git a/src/OSVRTrackedDevice.cpp b/src/OSVRTrackedDevice.cpp index b450f24..e335d7a 100644 --- a/src/OSVRTrackedDevice.cpp +++ b/src/OSVRTrackedDevice.cpp @@ -26,29 +26,30 @@ #include "OSVRTrackedDevice.h" #include "Logging.h" -#include "osvr_compiler_detection.h" +#include "AngVelTools.h" +#include "OSVRDisplay.h" +#include "ValveStrCpy.h" #include "make_unique.h" #include "matrix_cast.h" +#include "osvr_compiler_detection.h" #include "osvr_device_properties.h" -#include "ValveStrCpy.h" #include "platform_fixes.h" // strcasecmp -#include "make_unique.h" -#include "OSVRDisplay.h" // OpenVR includes #include // Library/third-party includes +#include #include +#include #include +#include #include #include -#include #include -#include // Standard includes -#include // for std::find +#include // for std::find #include #include #include @@ -143,7 +144,7 @@ vr::EVRInitError OSVRTrackedDevice::Activate(uint32_t object_id) try { renderManagerConfig_.parse(configString); - } catch(const std::exception& e) { + } catch (const std::exception& e) { OSVR_LOG(err) << "OSVRTrackedDevice::Activate(): Exception parsing Render Manager config: " << e.what() << "\n"; } @@ -274,7 +275,7 @@ vr::DistortionCoordinates_t OSVRTrackedDevice::ComputeDistortion(vr::EVREye eye, const auto osvr_eye = static_cast(eye); const auto distortion_parameters = distortionParameters_[osvr_eye]; - const auto in_coords = osvr::renderkit::Float2 {{u, 1.0f - v}}; // flip v-coordinate + const auto in_coords = osvr::renderkit::Float2{{u, 1.0f - v}}; // flip v-coordinate const auto interpolators = (vr::Eye_Left == eye) ? &leftEyeInterpolators_ : &rightEyeInterpolators_; @@ -720,7 +721,7 @@ vr::HmdMatrix34_t OSVRTrackedDevice::GetMatrix34TrackedDeviceProperty(vr::ETrack return default_value; } -uint32_t OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDeviceProperty prop, char* value, uint32_t buffer_size, vr::ETrackedPropertyError *error) +uint32_t OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDeviceProperty prop, char* value, uint32_t buffer_size, vr::ETrackedPropertyError* error) { uint32_t default_value = 0; @@ -746,11 +747,11 @@ uint32_t OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDevicePro return 0; } - // ------------------------------------ - // Private Methods - // ------------------------------------ +// ------------------------------------ +// Private Methods +// ------------------------------------ -std::string OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDeviceProperty prop, vr::ETrackedPropertyError *error) +std::string OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDeviceProperty prop, vr::ETrackedPropertyError* error) { std::string default_value = ""; @@ -837,7 +838,6 @@ std::string OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDevice if (error) *error = vr::TrackedProp_ValueNotProvidedByDevice; return default_value; - } #include "ignore-warning/pop" @@ -848,13 +848,27 @@ std::string OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDevice return default_value; } -void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue*, const OSVR_PoseReport* report) +void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* timeval, const OSVR_PoseReport* report) { if (!userdata) return; auto* self = static_cast(userdata); + // Get angular velocity in correct format for SteamVR + OSVR_TimeValue timeval2; + OSVR_AngularVelocityState state; + OSVR_VelocityState velocitystate; + + osvrGetVelocityState(self->trackerInterface_.get(), &timeval2, &velocitystate); + + double dt = state.dt; + Eigen::Quaterniond poserotation = osvr::util::fromQuat(report->pose.rotation); + Eigen::Quaterniond angvel_incrementalRotation = osvr::util::fromQuat(velocitystate.angularVelocity.incrementalRotation); + angvel_incrementalRotation = poserotation.inverse() * angvel_incrementalRotation * poserotation; + // Convert invcremental rotation to angular velocity + Eigen::Vector3d angularvelocity = osvr::vbtracker::incRotToAngVelVec(angvel_incrementalRotation, dt); + vr::DriverPose_t pose; pose.poseTimeOffset = 0; // close enough @@ -862,7 +876,6 @@ void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* Eigen::Vector3d::Map(pose.vecDriverFromHeadTranslation) = Eigen::Vector3d::Zero(); map(pose.qWorldFromDriverRotation) = Eigen::Quaterniond::Identity(); - map(pose.qDriverFromHeadRotation) = Eigen::Quaterniond::Identity(); // Position @@ -875,10 +888,16 @@ void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* // Orientation map(pose.qRotation) = osvr::util::fromQuat(report->pose.rotation); - // Angular velocity and acceleration are not currently consistently provided - Eigen::Vector3d::Map(pose.vecAngularVelocity) = Eigen::Vector3d::Zero(); + // Acceleration is not currently consistently provided Eigen::Vector3d::Map(pose.vecAngularAcceleration) = Eigen::Vector3d::Zero(); + // If angular velocity is valid, pass that data to SteamVR + if (velocitystate.angularVelocityValid) { + Eigen::Vector3d::Map(pose.vecAngularVelocity) = angularvelocity; + } else { + Eigen::Vector3d::Map(pose.vecAngularVelocity) = Eigen::Vector3d::Zero(); + } + pose.result = vr::TrackingResult_Running_OK; pose.poseIsValid = true; pose.willDriftInYaw = true; @@ -991,7 +1010,7 @@ void OSVRTrackedDevice::configure() display_.position.y = position_y; display_.rotation = rotation; display_.verticalRefreshRate = settings_->getSetting("verticalRefreshRate", getVerticalRefreshRate()); - display_.attachedToDesktop = false; // assuming direct mode + display_.attachedToDesktop = false; // assuming direct mode display_.edidVendorId = settings_->getSetting("edidVendorId", 0xd24e); // SVR display_.edidProductId = settings_->getSetting("edidProductId", 0x1019); @@ -1033,7 +1052,7 @@ void OSVRTrackedDevice::configureDistortionParameters() // Initialize the distortion parameters OSVR_LOG(debug) << "OSVRTrackedDevice::configureDistortionParameters(): Number of eyes: " << displayConfiguration_.getEyes().size() << "."; for (size_t i = 0; i < displayConfiguration_.getEyes().size(); ++i) { - auto distortion = osvr::renderkit::DistortionParameters { displayConfiguration_, i }; + auto distortion = osvr::renderkit::DistortionParameters{displayConfiguration_, i}; distortion.m_desiredTriangles = 200 * 64; OSVR_LOG(debug) << "OSVRTrackedDevice::configureDistortionParameters(): Adding distortion for eye " << i << "."; distortionParameters_.push_back(distortion); @@ -1077,17 +1096,13 @@ osvr::display::ScanOutOrigin OSVRTrackedDevice::parseScanOutOrigin(std::string s // Make the string lowercase std::transform(str.begin(), str.end(), str.begin(), ::tolower); - if ("lower-left" == str || "ll" == str || "lowerleft" == str || "lower left" == str - || "bottom-left" == str || "bl" == str || "bottomleft" == str || "bottom left" == str) { + if ("lower-left" == str || "ll" == str || "lowerleft" == str || "lower left" == str || "bottom-left" == str || "bl" == str || "bottomleft" == str || "bottom left" == str) { return osvr::display::ScanOutOrigin::LowerLeft; - } else if ("lower-right" == str || "lr" == str || "lowerright" == str || "lower right" == str - || "bottom-right" == str || "br" == str || "bottomright" == str || "bottom right" == str) { + } else if ("lower-right" == str || "lr" == str || "lowerright" == str || "lower right" == str || "bottom-right" == str || "br" == str || "bottomright" == str || "bottom right" == str) { return osvr::display::ScanOutOrigin::LowerRight; - } else if ("upper-left" == str || "ul" == str || "upperleft" == str || "upper left" == str - || "top-left" == str || "tl" == str || "topleft" == str || "top left" == str) { + } else if ("upper-left" == str || "ul" == str || "upperleft" == str || "upper left" == str || "top-left" == str || "tl" == str || "topleft" == str || "top left" == str) { return osvr::display::ScanOutOrigin::UpperLeft; - } else if ("upper-right" == str || "ur" == str || "upperright" == str || "upper right" == str - || "top-right" == str || "tr" == str || "topright" == str || "top right" == str) { + } else if ("upper-right" == str || "ur" == str || "upperright" == str || "upper right" == str || "top-right" == str || "tr" == str || "topright" == str || "top right" == str) { return osvr::display::ScanOutOrigin::UpperRight; } else { OSVR_LOG(err) << "The string [" + str + "] could not be parsed as a scan-out origin. Use one of: lower-left, upper-left, lower-right, upper-right."; @@ -1130,16 +1145,15 @@ std::pair OSVRTrackedDevice::rotate(float u, float v, osvr::displa // Rotates normalized coordinates counter-clockwise using R = osvr::display::Rotation; if (R::Zero == rotation) { - return { u, v }; + return {u, v}; } else if (R::Ninety == rotation) { - return { 1.0f - v, u }; + return {1.0f - v, u}; } else if (R::OneEighty == rotation) { - return { 1.0f - u, 1.0f - v }; + return {1.0f - u, 1.0f - v}; } else if (R::TwoSeventy == rotation) { - return { v, 1.0f - u }; + return {v, 1.0f - u}; } else { OSVR_LOG(err) << "Unknown rotation [" << rotation << "] Assuming 0 degrees."; - return { u, v }; + return {u, v}; } } -