From 2b8b9ff62ae2de28aeda80c4f855a497692438c3 Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 20:45:33 +0000 Subject: [PATCH 01/13] Fix a bug in the Ipc::Connection class A defaulted move constructor/assignment operator would think that it has unique ownership of the handle it's been given, leading to the possibility of using a handle after it's been closed. This hasn't been causing any issues due to NRVO kicking in in the only use case of the class, but better to fix it now that it's been spotted. --- libraries/ipc/Ipc.cpp | 28 ++++++++++++++++++++++++++-- libraries/ipc/Ipc.hpp | 4 ++-- 2 files changed, 28 insertions(+), 4 deletions(-) diff --git a/libraries/ipc/Ipc.cpp b/libraries/ipc/Ipc.cpp index d2e74c7a..9a95d9a5 100644 --- a/libraries/ipc/Ipc.cpp +++ b/libraries/ipc/Ipc.cpp @@ -20,7 +20,18 @@ namespace Ipc { Connection::~Connection() { - DisconnectNamedPipe(inPipe); + if (inPipe != INVALID_HANDLE_VALUE) { + DisconnectNamedPipe(inPipe); + } + } + + Connection::Connection(Connection &&other) noexcept : inPipe(INVALID_HANDLE_VALUE) { + operator=(std::move(other)); + } + + Connection& Connection::operator=(Connection &&other) noexcept { + std::swap(inPipe, other.inPipe); + return *this; } bool Connection::send(const char *buffer, size_t length) @@ -167,7 +178,18 @@ namespace Ipc { Connection::~Connection() { - ::close(connfd); + if (connfd != -1) { + ::close(connfd); + } + } + + Connection::Connection(Connection&& other) noexcept : connfd(-1) { + operator=(std::move(other)); + } + + Connection& Connection::operator=(Connection&& other) noexcept { + std::swap(connfd, other.connfd); + return *this; } bool Connection::send(const char *buf, size_t len) @@ -252,6 +274,8 @@ namespace Ipc { Connection::Connection() { } Connection::~Connection() { } + Connection::Connection(Connection&&) noexcept = default; + Connection& Connection::operator=(Connection&&) noexcept = default; bool Connection::send(const char *buf, size_t len) { return false; diff --git a/libraries/ipc/Ipc.hpp b/libraries/ipc/Ipc.hpp index ca14440a..3573bc6f 100644 --- a/libraries/ipc/Ipc.hpp +++ b/libraries/ipc/Ipc.hpp @@ -18,8 +18,8 @@ namespace Ipc { Connection& operator=(Connection const &) = delete; // Moving is allowed - Connection(Connection &&) = default; - Connection& operator=(Connection &&) = default; + Connection(Connection &&) noexcept; + Connection& operator=(Connection &&) noexcept; bool send(const char *buffer, size_t length); bool recv(char *buffer, size_t length); From 56b85927c1001d345cb01d1bc761e2470a1a3b54 Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 20:48:36 +0000 Subject: [PATCH 02/13] Remove unnecessary re-declaration of base class methods We're not overriding them here so we don't need to expose them. --- driver_files/src/Driver/IVRDevice.hpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/driver_files/src/Driver/IVRDevice.hpp b/driver_files/src/Driver/IVRDevice.hpp index 79f50d32..c2b59ed5 100644 --- a/driver_files/src/Driver/IVRDevice.hpp +++ b/driver_files/src/Driver/IVRDevice.hpp @@ -50,14 +50,6 @@ namespace ExampleDriver { return out_pose; } - // Inherited via ITrackedDeviceServerDriver - virtual vr::EVRInitError Activate(uint32_t unObjectId) = 0; - virtual void Deactivate() = 0; - virtual void EnterStandby() = 0; - virtual void* GetComponent(const char* pchComponentNameAndVersion) = 0; - virtual void DebugRequest(const char* pchRequest, char* pchResponseBuffer, uint32_t unResponseBufferSize) = 0; - virtual vr::DriverPose_t GetPose() = 0; - ~IVRDevice() = default; }; }; \ No newline at end of file From e003777a7679582e577b9df5d0ec6b338604d331 Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 20:50:32 +0000 Subject: [PATCH 03/13] Add necessary cast to correct type The address of an object and its base class aren't necessarily the same so make it explicit that we're handing out a specific type from a function that returns void*. --- driver_files/src/Driver/IVRDriver.hpp | 2 +- driver_files/src/Native/DriverFactory.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/driver_files/src/Driver/IVRDriver.hpp b/driver_files/src/Driver/IVRDriver.hpp index e9ebc478..7eb3b922 100644 --- a/driver_files/src/Driver/IVRDriver.hpp +++ b/driver_files/src/Driver/IVRDriver.hpp @@ -9,7 +9,7 @@ namespace ExampleDriver { typedef std::variant SettingsValue; - class IVRDriver : protected vr::IServerTrackedDeviceProvider { + class IVRDriver : public vr::IServerTrackedDeviceProvider { public: /// diff --git a/driver_files/src/Native/DriverFactory.cpp b/driver_files/src/Native/DriverFactory.cpp index 02f78545..14e523d9 100644 --- a/driver_files/src/Native/DriverFactory.cpp +++ b/driver_files/src/Native/DriverFactory.cpp @@ -13,7 +13,7 @@ void* HmdDriverFactory(const char* interface_name, int* return_code) { driver = std::make_shared(); } // We always have at least 1 ref to the shared ptr in "driver" so passing out raw pointer is ok - return driver.get(); + return static_cast(driver.get()); } if (return_code) From 69892b38b4d2e99e2832f798367454c8a3ee1330 Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 20:51:30 +0000 Subject: [PATCH 04/13] Always return a value from Key::isPressed() --- driver_files/src/Driver/Key.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/driver_files/src/Driver/Key.cpp b/driver_files/src/Driver/Key.cpp index d2c999fa..259ee081 100644 --- a/driver_files/src/Driver/Key.cpp +++ b/driver_files/src/Driver/Key.cpp @@ -31,8 +31,8 @@ namespace Key { } #else (void)key; - return false; #endif + return false; } }; // namespace Key From 6bcaee6c1c133f7166eb797e6350261be7dd2c62 Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 20:56:34 +0000 Subject: [PATCH 05/13] Remove unused code None of this is referenced anywhere else so may as well clean it up. --- driver_files/src/Driver/TrackerDevice.cpp | 24 ----------------------- driver_files/src/Driver/TrackerDevice.hpp | 9 --------- 2 files changed, 33 deletions(-) diff --git a/driver_files/src/Driver/TrackerDevice.cpp b/driver_files/src/Driver/TrackerDevice.cpp index 1545c441..7542d849 100644 --- a/driver_files/src/Driver/TrackerDevice.cpp +++ b/driver_files/src/Driver/TrackerDevice.cpp @@ -19,8 +19,6 @@ ExampleDriver::TrackerDevice::TrackerDevice(std::string serial, std::string role serial_(serial), role_(role) { - this->last_pose_ = MakeDefaultPose(); - this->isSetup = false; } std::string ExampleDriver::TrackerDevice::GetSerial() @@ -52,28 +50,6 @@ void ExampleDriver::TrackerDevice::Update() if (this->device_index_ == vr::k_unTrackedDeviceIndexInvalid) return; - // Check if this device was asked to be identified - auto events = GetDriver()->GetOpenVREvents(); - for (auto event : events) { - // Note here, event.trackedDeviceIndex does not necissarily equal this->device_index_, not sure why, but the component handle will match so we can just use that instead - //if (event.trackedDeviceIndex == this->device_index_) { - if (event.eventType == vr::EVREventType::VREvent_Input_HapticVibration) { - if (event.data.hapticVibration.componentHandle == this->haptic_component_) { - this->did_vibrate_ = true; - } - } - //} - } - - // Check if we need to keep vibrating - if (this->did_vibrate_) { - this->vibrate_anim_state_ += (GetDriver()->GetLastFrameTime().count() / 1000.f); - if (this->vibrate_anim_state_ > 1.0f) { - this->did_vibrate_ = false; - this->vibrate_anim_state_ = 0.0f; - } - } - // Setup pose for this frame auto pose = this->last_pose_; diff --git a/driver_files/src/Driver/TrackerDevice.hpp b/driver_files/src/Driver/TrackerDevice.hpp index 57406799..16e6a1ac 100644 --- a/driver_files/src/Driver/TrackerDevice.hpp +++ b/driver_files/src/Driver/TrackerDevice.hpp @@ -43,20 +43,11 @@ namespace ExampleDriver { vr::TrackedDeviceIndex_t device_index_ = vr::k_unTrackedDeviceIndexInvalid; std::string serial_; std::string role_; - bool isSetup; std::chrono::milliseconds _pose_timestamp; vr::DriverPose_t last_pose_ = IVRDevice::MakeDefaultPose(); - bool did_vibrate_ = false; - float vibrate_anim_state_ = 0.f; - - vr::VRInputComponentHandle_t haptic_component_ = 0; - - vr::VRInputComponentHandle_t system_click_component_ = 0; - vr::VRInputComponentHandle_t system_touch_component_ = 0; - int max_saved = 10; std::vector> prev_positions; // prev_positions[:][0] je time since now (koliko cajta nazaj se je naredl, torej min-->max) double last_update = 0; From b26f5553d0dbc2d8ef35524a82885fc0b1f31d2e Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 21:24:38 +0000 Subject: [PATCH 06/13] Introduce some types This way it's easier to follow what's going on without the -1's in places. Also move the non-virtual methods to be separate to make it clearer that they're not part of the base classes. --- driver_files/src/Driver/TrackerDevice.cpp | 105 ++++++++++------------ driver_files/src/Driver/TrackerDevice.hpp | 19 ++-- driver_files/src/Driver/VRDriver.cpp | 2 +- 3 files changed, 63 insertions(+), 63 deletions(-) diff --git a/driver_files/src/Driver/TrackerDevice.cpp b/driver_files/src/Driver/TrackerDevice.cpp index 7542d849..1e8e15e8 100644 --- a/driver_files/src/Driver/TrackerDevice.cpp +++ b/driver_files/src/Driver/TrackerDevice.cpp @@ -1,8 +1,9 @@ #include "TrackerDevice.hpp" -void normalizeQuat(double pose[]) -{ +#include +static void normalizeQuat(ExampleDriver::TrackerDevice::PoseInfo& pose) +{ //normalize double mag = sqrt(pose[3] * pose[3] + pose[4] * pose[4] + @@ -37,8 +38,8 @@ void ExampleDriver::TrackerDevice::reinit(int msaved, double mtime, double msmoo msmooth = 0.99; max_saved = msaved; - std::vector> temp(msaved, std::vector(8,-1)); - prev_positions = temp; + prev_positions.clear(); + prev_positions.resize(max_saved); max_time = mtime; smoothing = msmooth; @@ -66,18 +67,13 @@ void ExampleDriver::TrackerDevice::Update() double previous_position[3] = { 0 }; std::copy(std::begin(pose.vecPosition), std::end(pose.vecPosition), std::begin(previous_position)); - double next_pose[7]; + PoseInfo next_pose; if (get_next_pose(0, next_pose) != 0) return; normalizeQuat(next_pose); - bool pose_nan = false; - for (int i = 0; i < 7; i++) - { - if (std::isnan(next_pose[i])) - pose_nan = true; - } + const bool pose_nan = std::any_of(next_pose.begin(), next_pose.end(), [](double d) { return std::isnan(d); }); if (smoothing == 0 || pose_nan) { @@ -139,7 +135,7 @@ void ExampleDriver::TrackerDevice::Log(std::string message) vr::VRDriverLog()->Log(message_endl.c_str()); } -int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, double pred[]) +int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, PoseInfo& next_pose) const { int statuscode = 0; @@ -163,11 +159,11 @@ int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, double pred[ double avg_time2 = 0; for (int i = 0; i < max_saved; i++) { - if (prev_positions[i][0] < 0) + if (prev_positions[i].time < 0) break; curr_saved++; - avg_time += prev_positions[i][0]; - avg_time2 += (prev_positions[i][0] * prev_positions[i][0]); + avg_time += prev_positions[i].time; + avg_time2 += (prev_positions[i].time * prev_positions[i].time); } //Log("saved values: " + std::to_string(curr_saved)); @@ -177,10 +173,7 @@ int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, double pred[ { if (curr_saved > 0) { - for (int i = 1; i < 8; i++) - { - pred[i - 1] = prev_positions[0][i]; - } + next_pose = prev_positions.front().pose; return statuscode; } //printf("Too few values"); @@ -196,21 +189,22 @@ int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, double pred[ double st = 0; for (int j = 0; j < curr_saved; j++) { - st += ((prev_positions[j][0] - avg_time) * (prev_positions[j][0] - avg_time)); + st += ((prev_positions[j].time - avg_time) * (prev_positions[j].time - avg_time)); } st = sqrt(st * (1.0 / curr_saved)); - for (int i = 1; i < 8; i++) + for (int i = 0; i < next_pose.size(); i++) { double avg_val = 0; double avg_val2 = 0; double avg_tval = 0; for (int ii = 0; ii < curr_saved; ii++) { - avg_val += prev_positions[ii][i]; - avg_tval += (prev_positions[ii][0] * prev_positions[ii][i]); - avg_val2 += (prev_positions[ii][i] * prev_positions[ii][i]); + const PrevPose& prev_pose = prev_positions[ii]; + avg_val += prev_pose.pose[i]; + avg_tval += (prev_pose.time * prev_pose.pose[i]); + avg_val2 += (prev_pose.pose[i] * prev_pose.pose[i]); } avg_val /= curr_saved; avg_tval /= curr_saved; @@ -221,7 +215,7 @@ int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, double pred[ double sv = 0; for (int j = 0; j < curr_saved; j++) { - sv += ((prev_positions[j][i] - avg_val) * (prev_positions[j][i] - avg_val)); + sv += ((prev_positions[j].pose[i] - avg_val) * (prev_positions[j].pose[i] - avg_val)); } sv = sqrt(sv * (1.0 / curr_saved)); @@ -238,7 +232,7 @@ int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, double pred[ if (abs(avg_val2 - (avg_val * avg_val)) < 0.00000001) //bloody floating point rounding errors y = avg_val; - pred[i - 1] = y; + next_pose[i] = y; //printf("<<<< %f --> %f\n",y, pred[i-1]); @@ -250,7 +244,7 @@ int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, double pred[ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double c, double w, double x, double y, double z, double time_offset) { - double next_pose[7]; + PoseInfo next_pose; int pose_valid = get_next_pose(time_offset, next_pose); double dot = x * next_pose[4] + y * next_pose[5] + z * next_pose[6] + w * next_pose[3]; @@ -269,14 +263,15 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double double time_since_epoch_seconds = time_since_epoch.count() / 1000.0; double curr_time = time_since_epoch_seconds; this->last_update = curr_time; - prev_positions[0][0] = time_offset; - prev_positions[0][1] = a; - prev_positions[0][2] = b; - prev_positions[0][3] = c; - prev_positions[0][4] = w; - prev_positions[0][5] = x; - prev_positions[0][6] = y; - prev_positions[0][7] = z; + PrevPose& prev_pose = prev_positions.front(); + prev_pose.time = time_offset; + prev_pose.pose[0] = a; + prev_pose.pose[1] = b; + prev_pose.pose[2] = c; + prev_pose.pose[3] = w; + prev_pose.pose[4] = x; + prev_pose.pose[5] = y; + prev_pose.pose[6] = z; return; } @@ -292,12 +287,13 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double double time_since_update = curr_time - this->last_update; this->last_update = curr_time; - for (int i = 0; i < max_saved; i++) + for (PrevPose& prev_pose : prev_positions) { - if (prev_positions[i][0] >= 0) - prev_positions[i][0] += time_since_update; - if (prev_positions[i][0] > max_time) - prev_positions[i][0] = -1; + double& prev_time = prev_pose.time; + if (prev_time >= 0) + prev_time += time_since_update; + if (prev_time > max_time) + prev_time = -1; } double time = time_offset; @@ -325,35 +321,32 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double if (time > max_time) return; - if (prev_positions[max_saved - 1][0] < time && prev_positions[max_saved - 1][0] >= 0) + if (prev_positions[max_saved - 1].time < time && prev_positions[max_saved - 1].time >= 0) return; int i = 0; - while (prev_positions[i][0] < time&& prev_positions[i][0] >= 0) + while (prev_positions[i].time < time && prev_positions[i].time >= 0) i++; for (int j = max_saved - 1; j > i; j--) { - if (prev_positions[j - 1][0] >= 0) + if (prev_positions[j - 1].time >= 0) { - for (int k = 0; k < 8; k++) - { - prev_positions[j][k] = prev_positions[j - 1][k]; - } + prev_positions[j] = prev_positions[j - 1]; } else { - prev_positions[j][0] = -1; + prev_positions[j].time = -1; } } - prev_positions[i][0] = time; - prev_positions[i][1] = a; - prev_positions[i][2] = b; - prev_positions[i][3] = c; - prev_positions[i][4] = w; - prev_positions[i][5] = x; - prev_positions[i][6] = y; - prev_positions[i][7] = z; + prev_positions[i].time = time; + prev_positions[i].pose[0] = a; + prev_positions[i].pose[1] = b; + prev_positions[i].pose[2] = c; + prev_positions[i].pose[3] = w; + prev_positions[i].pose[4] = x; + prev_positions[i].pose[5] = y; + prev_positions[i].pose[6] = z; /* //for debugging Log("------------------------------------------------"); for (int i = 0; i < max_saved; i++) diff --git a/driver_files/src/Driver/TrackerDevice.hpp b/driver_files/src/Driver/TrackerDevice.hpp index 16e6a1ac..30a11dd8 100644 --- a/driver_files/src/Driver/TrackerDevice.hpp +++ b/driver_files/src/Driver/TrackerDevice.hpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -16,6 +17,8 @@ namespace ExampleDriver { class TrackerDevice : public IVRDevice { public: + // [position[x, y, z], rotation[w, x, y, z]] + using PoseInfo = std::array; TrackerDevice(std::string serial, std::string role); ~TrackerDevice() = default; @@ -23,10 +26,6 @@ namespace ExampleDriver { // Inherited via IVRDevice virtual std::string GetSerial() override; virtual void Update() override; - //virtual void UpdatePos(double a, double b, double c, double time, double smoothing); - //virtual void UpdateRot(double qw, double qx, double qy, double qz, double time, double smoothing); - virtual void save_current_pose(double a, double b, double c, double qw, double qx, double qy, double qz, double time); - virtual int get_next_pose(double req_time, double pred[]); virtual vr::TrackedDeviceIndex_t GetDeviceIndex() override; virtual DeviceType GetDeviceType() override; virtual void Log(std::string message); @@ -37,7 +36,10 @@ namespace ExampleDriver { virtual void* GetComponent(const char* pchComponentNameAndVersion) override; virtual void DebugRequest(const char* pchRequest, char* pchResponseBuffer, uint32_t unResponseBufferSize) override; virtual vr::DriverPose_t GetPose() override; - virtual void reinit(int msaved, double mtime, double msmooth); + + void reinit(int msaved, double mtime, double msmooth); + void save_current_pose(double a, double b, double c, double qw, double qx, double qy, double qz, double time); + int get_next_pose(double req_time, PoseInfo& pred) const; private: vr::TrackedDeviceIndex_t device_index_ = vr::k_unTrackedDeviceIndexInvalid; @@ -48,8 +50,13 @@ namespace ExampleDriver { vr::DriverPose_t last_pose_ = IVRDevice::MakeDefaultPose(); + struct PrevPose { + double time = -1; + PoseInfo pose; + }; + int max_saved = 10; - std::vector> prev_positions; // prev_positions[:][0] je time since now (koliko cajta nazaj se je naredl, torej min-->max) + std::vector prev_positions; // koliko cajta nazaj se je naredl, torej min-->max double last_update = 0; double max_time = 1; double smoothing = 0; diff --git a/driver_files/src/Driver/VRDriver.cpp b/driver_files/src/Driver/VRDriver.cpp index f4e835b8..9843acae 100644 --- a/driver_files/src/Driver/VRDriver.cpp +++ b/driver_files/src/Driver/VRDriver.cpp @@ -237,7 +237,7 @@ void ExampleDriver::VRDriver::PipeThread() { s = s + " trackerpose " + std::to_string(idx); - double pose[7]; + TrackerDevice::PoseInfo pose; int statuscode = this->trackers_[idx]->get_next_pose(time_offset, pose); s = s + " " + std::to_string(pose[0]) + From 68d009faad4f73c9b7ecc3ea836579e21848cd6b Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 21:55:31 +0000 Subject: [PATCH 07/13] Initialise _pose_timestamp Also switch to using chrono types to make it clearer which are timestamps and which are durations. --- driver_files/src/Driver/TrackerDevice.cpp | 49 +++++++++-------------- driver_files/src/Driver/TrackerDevice.hpp | 10 +++-- driver_files/src/Driver/VRDriver.cpp | 4 +- 3 files changed, 27 insertions(+), 36 deletions(-) diff --git a/driver_files/src/Driver/TrackerDevice.cpp b/driver_files/src/Driver/TrackerDevice.cpp index 1e8e15e8..67437599 100644 --- a/driver_files/src/Driver/TrackerDevice.cpp +++ b/driver_files/src/Driver/TrackerDevice.cpp @@ -16,10 +16,13 @@ static void normalizeQuat(ExampleDriver::TrackerDevice::PoseInfo& pose) pose[6] /= mag; } -ExampleDriver::TrackerDevice::TrackerDevice(std::string serial, std::string role): +ExampleDriver::TrackerDevice::TrackerDevice(std::string serial, std::string role) : serial_(serial), role_(role) { + const auto now = std::chrono::system_clock::now(); + _pose_timestamp = now; + last_update = now; } std::string ExampleDriver::TrackerDevice::GetSerial() @@ -55,20 +58,18 @@ void ExampleDriver::TrackerDevice::Update() auto pose = this->last_pose_; // Update time delta (for working out velocity) - std::chrono::milliseconds time_since_epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); - double time_since_epoch_seconds = time_since_epoch.count() / 1000.0; - double pose_time_delta_seconds = (time_since_epoch - _pose_timestamp).count() / 1000.0; + const auto time_now = std::chrono::system_clock::now(); + const double pose_time_delta_seconds = std::chrono::duration_cast(time_now - _pose_timestamp).count(); // Update pose timestamp - - _pose_timestamp = time_since_epoch; + _pose_timestamp = time_now; // Copy the previous position data double previous_position[3] = { 0 }; std::copy(std::begin(pose.vecPosition), std::end(pose.vecPosition), std::begin(previous_position)); PoseInfo next_pose; - if (get_next_pose(0, next_pose) != 0) + if (get_next_pose(Seconds(0), next_pose) != 0) return; normalizeQuat(next_pose); @@ -135,16 +136,13 @@ void ExampleDriver::TrackerDevice::Log(std::string message) vr::VRDriverLog()->Log(message_endl.c_str()); } -int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, PoseInfo& next_pose) const +int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& next_pose) const { int statuscode = 0; - std::chrono::milliseconds time_since_epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); - double time_since_epoch_seconds = time_since_epoch.count() / 1000.0; - - double req_time = time_since_epoch_seconds - time_offset; - - double new_time = last_update - req_time; + const auto time_now = std::chrono::system_clock::now(); + const auto req_time = time_now - time_offset; + double new_time = std::chrono::duration_cast(last_update - req_time).count(); if (new_time < -0.2) //limit prediction to max 0.2 second into the future to prevent your feet from being yeeted into oblivion { @@ -242,7 +240,7 @@ int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, PoseInfo& ne //return pred[0], pred[1], pred[2], pred[3], pred[4], pred[5], pred[6]; } -void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double c, double w, double x, double y, double z, double time_offset) +void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double c, double w, double x, double y, double z, Seconds time_offset) { PoseInfo next_pose; int pose_valid = get_next_pose(time_offset, next_pose); @@ -259,12 +257,9 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double if (max_time == 0) { - std::chrono::milliseconds time_since_epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); - double time_since_epoch_seconds = time_since_epoch.count() / 1000.0; - double curr_time = time_since_epoch_seconds; - this->last_update = curr_time; + this->last_update = std::chrono::system_clock::now(); PrevPose& prev_pose = prev_positions.front(); - prev_pose.time = time_offset; + prev_pose.time = time_offset.count(); prev_pose.pose[0] = a; prev_pose.pose[1] = b; prev_pose.pose[2] = c; @@ -277,15 +272,9 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double } //update times - std::chrono::milliseconds time_since_epoch = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); - double time_since_epoch_seconds = time_since_epoch.count() / 1000.0; - - //Log("time since epoch: " + std::to_string(time_since_epoch_seconds)); - //lock_t curr_time = clock(); - //clock_t capture_time = curr_time - (timeOffset*1000); - double curr_time = time_since_epoch_seconds; - double time_since_update = curr_time - this->last_update; - this->last_update = curr_time; + const auto time_now = std::chrono::system_clock::now(); + const double time_since_update = std::chrono::duration_cast(time_now - this->last_update).count(); + this->last_update = time_now; for (PrevPose& prev_pose : prev_positions) { @@ -296,7 +285,7 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double prev_time = -1; } - double time = time_offset; + double time = time_offset.count(); // double offset = (rand() % 100) / 10000.; // time += offset; // printf("%f %f\n", time, offset); diff --git a/driver_files/src/Driver/TrackerDevice.hpp b/driver_files/src/Driver/TrackerDevice.hpp index 30a11dd8..a872a569 100644 --- a/driver_files/src/Driver/TrackerDevice.hpp +++ b/driver_files/src/Driver/TrackerDevice.hpp @@ -20,6 +20,8 @@ namespace ExampleDriver { // [position[x, y, z], rotation[w, x, y, z]] using PoseInfo = std::array; + using Seconds = std::chrono::duration; + TrackerDevice(std::string serial, std::string role); ~TrackerDevice() = default; @@ -38,15 +40,15 @@ namespace ExampleDriver { virtual vr::DriverPose_t GetPose() override; void reinit(int msaved, double mtime, double msmooth); - void save_current_pose(double a, double b, double c, double qw, double qx, double qy, double qz, double time); - int get_next_pose(double req_time, PoseInfo& pred) const; + void save_current_pose(double a, double b, double c, double qw, double qx, double qy, double qz, Seconds time_offset); + int get_next_pose(Seconds time_offset, PoseInfo& pred) const; private: vr::TrackedDeviceIndex_t device_index_ = vr::k_unTrackedDeviceIndexInvalid; std::string serial_; std::string role_; - std::chrono::milliseconds _pose_timestamp; + std::chrono::system_clock::time_point _pose_timestamp; vr::DriverPose_t last_pose_ = IVRDevice::MakeDefaultPose(); @@ -57,7 +59,7 @@ namespace ExampleDriver { int max_saved = 10; std::vector prev_positions; // koliko cajta nazaj se je naredl, torej min-->max - double last_update = 0; + std::chrono::system_clock::time_point last_update; double max_time = 1; double smoothing = 0; diff --git a/driver_files/src/Driver/VRDriver.cpp b/driver_files/src/Driver/VRDriver.cpp index 9843acae..5014d810 100644 --- a/driver_files/src/Driver/VRDriver.cpp +++ b/driver_files/src/Driver/VRDriver.cpp @@ -156,7 +156,7 @@ void ExampleDriver::VRDriver::PipeThread() { if(time < 0) time = -time; - this->trackers_[idx]->save_current_pose(a, b, c, qw, qx, qy, qz, time); + this->trackers_[idx]->save_current_pose(a, b, c, qw, qx, qy, qz, TrackerDevice::Seconds(time)); //this->trackers_[idx]->UpdatePos(a, b, c, time, 1-smoothing); //this->trackers_[idx]->UpdateRot(qw, qx, qy, qz, time, 1-smoothing); @@ -238,7 +238,7 @@ void ExampleDriver::VRDriver::PipeThread() s = s + " trackerpose " + std::to_string(idx); TrackerDevice::PoseInfo pose; - int statuscode = this->trackers_[idx]->get_next_pose(time_offset, pose); + int statuscode = this->trackers_[idx]->get_next_pose(TrackerDevice::Seconds(time_offset), pose); s = s + " " + std::to_string(pose[0]) + " " + std::to_string(pose[1]) + From 4fb40c69dd928993d188264fe4af0094bb46c34f Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 22:03:32 +0000 Subject: [PATCH 08/13] Simplify some Maths This is the same calculation so the results should be the same, and we avoid the division that looks like it was previously causing issues. --- driver_files/src/Driver/TrackerDevice.cpp | 63 ++++------------------- 1 file changed, 11 insertions(+), 52 deletions(-) diff --git a/driver_files/src/Driver/TrackerDevice.cpp b/driver_files/src/Driver/TrackerDevice.cpp index 67437599..eb2929ad 100644 --- a/driver_files/src/Driver/TrackerDevice.cpp +++ b/driver_files/src/Driver/TrackerDevice.cpp @@ -151,7 +151,6 @@ int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& n } int curr_saved = 0; - //double pred[7] = {0}; double avg_time = 0; double avg_time2 = 0; @@ -164,80 +163,40 @@ int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& n avg_time2 += (prev_positions[i].time * prev_positions[i].time); } - //Log("saved values: " + std::to_string(curr_saved)); - - //printf("curr saved %d\n", curr_saved); - if (curr_saved < 4) + if (curr_saved == 0) { - if (curr_saved > 0) - { - next_pose = prev_positions.front().pose; - return statuscode; - } - //printf("Too few values"); - statuscode = -1; - return statuscode; - //return 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + return -1; } - avg_time /= curr_saved; - avg_time2 /= curr_saved; - - //printf("avg time %f\n", avg_time); - - double st = 0; - for (int j = 0; j < curr_saved; j++) + else if (curr_saved < 4) { - st += ((prev_positions[j].time - avg_time) * (prev_positions[j].time - avg_time)); + next_pose = prev_positions.front().pose; + return statuscode; } - st = sqrt(st * (1.0 / curr_saved)); + avg_time /= curr_saved; + avg_time2 /= curr_saved; + const double st = std::sqrt(avg_time2 - avg_time * avg_time); for (int i = 0; i < next_pose.size(); i++) { double avg_val = 0; - double avg_val2 = 0; double avg_tval = 0; for (int ii = 0; ii < curr_saved; ii++) { const PrevPose& prev_pose = prev_positions[ii]; avg_val += prev_pose.pose[i]; avg_tval += (prev_pose.time * prev_pose.pose[i]); - avg_val2 += (prev_pose.pose[i] * prev_pose.pose[i]); } avg_val /= curr_saved; avg_tval /= curr_saved; - avg_val2 /= curr_saved; - //printf("--avg: %f\n", avg_val); - - double sv = 0; - for (int j = 0; j < curr_saved; j++) - { - sv += ((prev_positions[j].pose[i] - avg_val) * (prev_positions[j].pose[i] - avg_val)); - } - sv = sqrt(sv * (1.0 / curr_saved)); - - //printf("----sv: %f\n", sv); - - double rxy = (avg_tval - (avg_val * avg_time)) / sqrt((avg_time2 - (avg_time * avg_time)) * (avg_val2 - (avg_val * avg_val))); - double b = rxy * (sv / st); - double a = avg_val - (b * avg_time); - - //printf("a: %f, b: %f\n", a, b); - - double y = a + b * new_time; - //Log("aha: " + std::to_string(y) + std::to_string(avg_val)); - if (abs(avg_val2 - (avg_val * avg_val)) < 0.00000001) //bloody floating point rounding errors - y = avg_val; + const double m = (avg_tval - (avg_val * avg_time)) / (st * st); + const double y = avg_val + m * (new_time - avg_time); next_pose[i] = y; - //printf("<<<< %f --> %f\n",y, pred[i-1]); - - } - //printf("::: %f\n", pred[0]); + return statuscode; - //return pred[0], pred[1], pred[2], pred[3], pred[4], pred[5], pred[6]; } void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double c, double w, double x, double y, double z, Seconds time_offset) From aa8d94cf7a0fd326ae5118b97cf0be1312677edc Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 22:15:26 +0000 Subject: [PATCH 09/13] Drop max_saved We can infer this from the size of the vector. Also replace some handrolled algorithms with those in . --- driver_files/src/Driver/TrackerDevice.cpp | 50 +++++++++-------------- driver_files/src/Driver/TrackerDevice.hpp | 1 - 2 files changed, 19 insertions(+), 32 deletions(-) diff --git a/driver_files/src/Driver/TrackerDevice.cpp b/driver_files/src/Driver/TrackerDevice.cpp index eb2929ad..060b5104 100644 --- a/driver_files/src/Driver/TrackerDevice.cpp +++ b/driver_files/src/Driver/TrackerDevice.cpp @@ -40,9 +40,8 @@ void ExampleDriver::TrackerDevice::reinit(int msaved, double mtime, double msmoo else if (msmooth > 0.99) msmooth = 0.99; - max_saved = msaved; prev_positions.clear(); - prev_positions.resize(max_saved); + prev_positions.resize(msaved); max_time = mtime; smoothing = msmooth; @@ -154,13 +153,13 @@ int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& n double avg_time = 0; double avg_time2 = 0; - for (int i = 0; i < max_saved; i++) + for (const PrevPose& prev_pose : prev_positions) { - if (prev_positions[i].time < 0) + if (prev_pose.time < 0) break; curr_saved++; - avg_time += prev_positions[i].time; - avg_time2 += (prev_positions[i].time * prev_positions[i].time); + avg_time += prev_pose.time; + avg_time2 += prev_pose.time * prev_pose.time; } if (curr_saved == 0) @@ -251,7 +250,7 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double //Log("Time: " + std::to_string(time)); - double dist = sqrt(pow(next_pose[0] - a, 2) + pow(next_pose[1] - b, 2) + pow(next_pose[2] - c, 2)); + double dist = std::sqrt(std::pow(next_pose[0] - a, 2) + std::pow(next_pose[1] - b, 2) + std::pow(next_pose[2] - c, 2)); if (pose_valid == 0 && dist > 0.5) { Log("Dropped a pose! its error was " + std::to_string(dist)); @@ -259,7 +258,7 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double return; } - dist = sqrt(pow(a, 2) + pow(b, 2) + pow(c, 2)); + dist = std::sqrt(a * a + b * b + c * c); if (dist > 10) { Log("Dropped a pose! Was outside of playspace: " + std::to_string(dist)); @@ -269,32 +268,21 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double if (time > max_time) return; - if (prev_positions[max_saved - 1].time < time && prev_positions[max_saved - 1].time >= 0) + auto first_outdated = std::find_if(prev_positions.begin(), prev_positions.end(), [time](const PrevPose& prev_pose) { return prev_pose.time < 0 || prev_pose.time > time; }); + if (first_outdated == prev_positions.end()) { return; + } - int i = 0; - while (prev_positions[i].time < time && prev_positions[i].time >= 0) - i++; + std::rotate(first_outdated, std::prev(prev_positions.end()), prev_positions.end()); + first_outdated->time = time; + first_outdated->pose[0] = a; + first_outdated->pose[1] = b; + first_outdated->pose[2] = c; + first_outdated->pose[3] = w; + first_outdated->pose[4] = x; + first_outdated->pose[5] = y; + first_outdated->pose[6] = z; - for (int j = max_saved - 1; j > i; j--) - { - if (prev_positions[j - 1].time >= 0) - { - prev_positions[j] = prev_positions[j - 1]; - } - else - { - prev_positions[j].time = -1; - } - } - prev_positions[i].time = time; - prev_positions[i].pose[0] = a; - prev_positions[i].pose[1] = b; - prev_positions[i].pose[2] = c; - prev_positions[i].pose[3] = w; - prev_positions[i].pose[4] = x; - prev_positions[i].pose[5] = y; - prev_positions[i].pose[6] = z; /* //for debugging Log("------------------------------------------------"); for (int i = 0; i < max_saved; i++) diff --git a/driver_files/src/Driver/TrackerDevice.hpp b/driver_files/src/Driver/TrackerDevice.hpp index a872a569..114e3ac2 100644 --- a/driver_files/src/Driver/TrackerDevice.hpp +++ b/driver_files/src/Driver/TrackerDevice.hpp @@ -57,7 +57,6 @@ namespace ExampleDriver { PoseInfo pose; }; - int max_saved = 10; std::vector prev_positions; // koliko cajta nazaj se je naredl, torej min-->max std::chrono::system_clock::time_point last_update; double max_time = 1; From 87e15cfe247c268f40b33a3b57ca118d5ce3a8db Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 22:39:48 +0000 Subject: [PATCH 10/13] Add some thread safety to the trackers The events from the socket come in on one thread but the Update() method (which reads the poses generated by those updates) is called on another thread. --- driver_files/src/Driver/TrackerDevice.cpp | 26 +++++++++++------------ driver_files/src/Driver/TrackerDevice.hpp | 2 ++ 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/driver_files/src/Driver/TrackerDevice.cpp b/driver_files/src/Driver/TrackerDevice.cpp index 060b5104..36adba7a 100644 --- a/driver_files/src/Driver/TrackerDevice.cpp +++ b/driver_files/src/Driver/TrackerDevice.cpp @@ -32,6 +32,8 @@ std::string ExampleDriver::TrackerDevice::GetSerial() void ExampleDriver::TrackerDevice::reinit(int msaved, double mtime, double msmooth) { + std::lock_guard guard(pose_mutex); + if (msaved < 5) //prevent having too few values to calculate linear interpolation, and prevent crash on 0 msaved = 5; @@ -74,20 +76,14 @@ void ExampleDriver::TrackerDevice::Update() normalizeQuat(next_pose); const bool pose_nan = std::any_of(next_pose.begin(), next_pose.end(), [](double d) { return std::isnan(d); }); - - if (smoothing == 0 || pose_nan) - { - pose.vecPosition[0] = next_pose[0]; - pose.vecPosition[1] = next_pose[1]; - pose.vecPosition[2] = next_pose[2]; - - pose.qRotation.w = next_pose[3]; - pose.qRotation.x = next_pose[4]; - pose.qRotation.y = next_pose[5]; - pose.qRotation.z = next_pose[6]; + if (pose_nan) { + return; } - else + { + // Guard around uses of |smoothing| + std::lock_guard guard(pose_mutex); + pose.vecPosition[0] = next_pose[0] * (1 - smoothing) + pose.vecPosition[0] * smoothing; pose.vecPosition[1] = next_pose[1] * (1 - smoothing) + pose.vecPosition[1] * smoothing; pose.vecPosition[2] = next_pose[2] * (1 - smoothing) + pose.vecPosition[2] * smoothing; @@ -97,6 +93,7 @@ void ExampleDriver::TrackerDevice::Update() pose.qRotation.y = next_pose[5] * (1 - smoothing) + pose.qRotation.y * smoothing; pose.qRotation.z = next_pose[6] * (1 - smoothing) + pose.qRotation.z * smoothing; } + //normalize double mag = sqrt(pose.qRotation.w * pose.qRotation.w + pose.qRotation.x * pose.qRotation.x + @@ -137,6 +134,8 @@ void ExampleDriver::TrackerDevice::Log(std::string message) int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& next_pose) const { + std::lock_guard guard(pose_mutex); + int statuscode = 0; const auto time_now = std::chrono::system_clock::now(); @@ -213,6 +212,8 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double w = -w; } + std::lock_guard guard(pose_mutex); + if (max_time == 0) { this->last_update = std::chrono::system_clock::now(); @@ -225,7 +226,6 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double prev_pose.pose[4] = x; prev_pose.pose[5] = y; prev_pose.pose[6] = z; - return; } diff --git a/driver_files/src/Driver/TrackerDevice.hpp b/driver_files/src/Driver/TrackerDevice.hpp index 114e3ac2..37fec92a 100644 --- a/driver_files/src/Driver/TrackerDevice.hpp +++ b/driver_files/src/Driver/TrackerDevice.hpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace ExampleDriver { class TrackerDevice : public IVRDevice { @@ -57,6 +58,7 @@ namespace ExampleDriver { PoseInfo pose; }; + mutable std::mutex pose_mutex; std::vector prev_positions; // koliko cajta nazaj se je naredl, torej min-->max std::chrono::system_clock::time_point last_update; double max_time = 1; From b00fd64fef8cd5983e9523686422ac4a9c543bcb Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Mon, 12 Dec 2022 18:54:46 +0000 Subject: [PATCH 11/13] Add some useful log messages Also remove some commented out code. --- driver_files/src/Driver/TrackerDevice.cpp | 8 ++++++-- driver_files/src/Driver/VRDriver.cpp | 5 +---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/driver_files/src/Driver/TrackerDevice.cpp b/driver_files/src/Driver/TrackerDevice.cpp index 36adba7a..9935fc4c 100644 --- a/driver_files/src/Driver/TrackerDevice.cpp +++ b/driver_files/src/Driver/TrackerDevice.cpp @@ -47,7 +47,7 @@ void ExampleDriver::TrackerDevice::reinit(int msaved, double mtime, double msmoo max_time = mtime; smoothing = msmooth; - //Log("Settings changed! " + std::to_string(msaved) + " " + std::to_string(mtime)); + Log("Settings changed! " + std::to_string(msaved) + ' ' + std::to_string(mtime) + ' ' + std::to_string(msmooth)); } void ExampleDriver::TrackerDevice::Update() @@ -77,6 +77,7 @@ void ExampleDriver::TrackerDevice::Update() const bool pose_nan = std::any_of(next_pose.begin(), next_pose.end(), [](double d) { return std::isnan(d); }); if (pose_nan) { + Log("Not submitting pose! NaNs were seen"); return; } @@ -265,11 +266,14 @@ void ExampleDriver::TrackerDevice::save_current_pose(double a, double b, double return; } - if (time > max_time) + if (time > max_time) { + Log("Dropped a pose! It was too old"); return; + } auto first_outdated = std::find_if(prev_positions.begin(), prev_positions.end(), [time](const PrevPose& prev_pose) { return prev_pose.time < 0 || prev_pose.time > time; }); if (first_outdated == prev_positions.end()) { + Log("Dropped a pose! All previous poses are newer"); return; } diff --git a/driver_files/src/Driver/VRDriver.cpp b/driver_files/src/Driver/VRDriver.cpp index 5014d810..b40e1ae2 100644 --- a/driver_files/src/Driver/VRDriver.cpp +++ b/driver_files/src/Driver/VRDriver.cpp @@ -50,11 +50,8 @@ void ExampleDriver::VRDriver::PipeThread() //we go and read it into our buffer if (ipcConnection.recv(buffer, sizeof(buffer))) { - //MessageBoxA(NULL, "connected2", "Example Driver", MB_OK); //convert our buffer to string - - //MessageBoxA(NULL, buffer, "Example Driver", MB_OK); - + buffer[sizeof(buffer) - 1] = '\0'; std::string rec = buffer; //Log("Received message: " + rec); From 0ab2110a1b7b6c404a8a885afbf29eabd3895e58 Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Tue, 13 Dec 2022 23:22:38 +0000 Subject: [PATCH 12/13] Use the correct member for the trackers/stations list devices_ holds all currently active devices whereas trackers_ and stations_ only hold devices of that type, so bounds check against those instead of the full device list. --- driver_files/src/Driver/VRDriver.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/driver_files/src/Driver/VRDriver.cpp b/driver_files/src/Driver/VRDriver.cpp index b40e1ae2..724e69ef 100644 --- a/driver_files/src/Driver/VRDriver.cpp +++ b/driver_files/src/Driver/VRDriver.cpp @@ -115,7 +115,7 @@ void ExampleDriver::VRDriver::PipeThread() } else if (word == "addstation") { - auto addstation = std::make_shared("AprilCamera" + std::to_string(this->devices_.size())); + auto addstation = std::make_shared("AprilCamera" + std::to_string(this->stations_.size())); this->AddDevice(addstation); this->stations_.push_back(addstation); s = s + " added"; @@ -173,7 +173,7 @@ void ExampleDriver::VRDriver::PipeThread() double a, b, c, time, smoothing; iss >> idx; iss >> a; iss >> b; iss >> c; iss >> time; iss >> smoothing; - if (idx < this->devices_.size()) + if (idx < this->trackers_.size()) { this->trackers_[idx]->UpdatePos(a, b, c, time, smoothing); this->trackers_[idx]->Update(); @@ -191,7 +191,7 @@ void ExampleDriver::VRDriver::PipeThread() double qw, qx, qy, qz, time, smoothing; iss >> qw; iss >> qx; iss >> qy; iss >> qz; iss >> time; iss >> smoothing; - if (idx < this->devices_.size()) + if (idx < this->trackers_.size()) { this->trackers_[idx]->UpdateRot(qw, qx, qy, qz, time, smoothing); this->trackers_[idx]->Update(); @@ -230,7 +230,7 @@ void ExampleDriver::VRDriver::PipeThread() iss >> idx; iss >> time_offset; - if (idx < this->devices_.size()) + if (idx < this->trackers_.size()) { s = s + " trackerpose " + std::to_string(idx); @@ -315,7 +315,7 @@ void ExampleDriver::VRDriver::RunFrame() this->frame_timing_avg_ = this->frame_timing_avg_ * 0.9 + ((double)this->frame_timing_.count()) * 0.1; //MessageBox(NULL, std::to_string(((double)this->frame_timing_.count()) * 0.1).c_str(), "Example Driver", MB_OK); - for (auto& device : this->trackers_) + for (auto& device : this->devices_) device->Update(); } From 551095f703ad6b29ed27ec2c75622c68cee3dff8 Mon Sep 17 00:00:00 2001 From: bobsayshilol Date: Sun, 11 Dec 2022 22:26:53 +0000 Subject: [PATCH 13/13] Return the rate at which the pose is changing This will allow us to feed a rough guesstimate of the velocity to SteamVR which should reduce the jitteriness. --- driver_files/src/Driver/TrackerDevice.cpp | 15 ++++++++++++--- driver_files/src/Driver/TrackerDevice.hpp | 2 +- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/driver_files/src/Driver/TrackerDevice.cpp b/driver_files/src/Driver/TrackerDevice.cpp index 9935fc4c..f0c13ae2 100644 --- a/driver_files/src/Driver/TrackerDevice.cpp +++ b/driver_files/src/Driver/TrackerDevice.cpp @@ -69,8 +69,8 @@ void ExampleDriver::TrackerDevice::Update() double previous_position[3] = { 0 }; std::copy(std::begin(pose.vecPosition), std::end(pose.vecPosition), std::begin(previous_position)); - PoseInfo next_pose; - if (get_next_pose(Seconds(0), next_pose) != 0) + PoseInfo next_pose, pose_rate; + if (get_next_pose(Seconds(0), next_pose, &pose_rate) != 0) return; normalizeQuat(next_pose); @@ -118,6 +118,10 @@ void ExampleDriver::TrackerDevice::Update() pose.poseTimeOffset = 0; + pose.vecVelocity[0] = pose_rate[0]; + pose.vecVelocity[1] = pose_rate[1]; + pose.vecVelocity[2] = pose_rate[2]; + //pose.vecVelocity[0] = (pose.vecPosition[0] - previous_position[0]) / pose_time_delta_seconds; //pose.vecVelocity[1] = (pose.vecPosition[1] - previous_position[1]) / pose_time_delta_seconds; //pose.vecVelocity[2] = (pose.vecPosition[2] - previous_position[2]) / pose_time_delta_seconds; @@ -133,12 +137,16 @@ void ExampleDriver::TrackerDevice::Log(std::string message) vr::VRDriverLog()->Log(message_endl.c_str()); } -int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& next_pose) const +int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& next_pose, PoseInfo* pose_rate_) const { std::lock_guard guard(pose_mutex); int statuscode = 0; + PoseInfo dummy_rate; + PoseInfo& pose_rate = pose_rate_ ? *pose_rate_ : dummy_rate; + pose_rate.fill(0); + const auto time_now = std::chrono::system_clock::now(); const auto req_time = time_now - time_offset; double new_time = std::chrono::duration_cast(last_update - req_time).count(); @@ -193,6 +201,7 @@ int ExampleDriver::TrackerDevice::get_next_pose(Seconds time_offset, PoseInfo& n const double y = avg_val + m * (new_time - avg_time); next_pose[i] = y; + pose_rate[i] = -m; // -ve since |new_time| and |PrevPose::time| increase into the past } return statuscode; diff --git a/driver_files/src/Driver/TrackerDevice.hpp b/driver_files/src/Driver/TrackerDevice.hpp index 37fec92a..5ab3c647 100644 --- a/driver_files/src/Driver/TrackerDevice.hpp +++ b/driver_files/src/Driver/TrackerDevice.hpp @@ -42,7 +42,7 @@ namespace ExampleDriver { void reinit(int msaved, double mtime, double msmooth); void save_current_pose(double a, double b, double c, double qw, double qx, double qy, double qz, Seconds time_offset); - int get_next_pose(Seconds time_offset, PoseInfo& pred) const; + int get_next_pose(Seconds time_offset, PoseInfo& next_pose, PoseInfo* pose_rate = nullptr) const; private: vr::TrackedDeviceIndex_t device_index_ = vr::k_unTrackedDeviceIndexInvalid;