Work in (static) standing universe instead of raw. (#24)

CMakeLists.txt

@@ -1,10 +1,10 @@
 cmake_minimum_required(VERSION 3.0.0)
 
-if (NOT DEFINED VCPKG_TARGET_TRIPLET)
+#if (NOT DEFINED VCPKG_TARGET_TRIPLET)
     if(WIN32)
         set(VCPKG_TARGET_TRIPLET "x64-windows-static-md")
     endif()
-endif()
+#endif()
 
 # If the toolchain is already defined, do not attempt to find it
 if(NOT DEFINED CMAKE_TOOLCHAIN_FILE)
@@ -65,14 +65,16 @@ find_package(Protobuf CONFIG REQUIRED)
 protobuf_generate_cpp(PROTO_SRC PROTO_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/src/bridge/ProtobufMessages.proto")
 SET_SOURCE_FILES_PROPERTIES(${PROTO_SRC} ${PROTO_INCL} PROPERTIES GENERATED TRUE)
 
+find_package(simdjson CONFIG REQUIRED)
+
 # Project
 file(GLOB_RECURSE HEADERS "${CMAKE_CURRENT_SOURCE_DIR}/src/*.hpp")
 file(GLOB_RECURSE SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp")
 add_library("${PROJECT_NAME}" SHARED "${HEADERS}" "${SOURCES}" ${PROTO_HEADER} ${PROTO_SRC})
 target_include_directories("${PROJECT_NAME}" PUBLIC "${OPENVR_INCLUDE_DIR}")
 target_include_directories("${PROJECT_NAME}" PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/libraries/linalg")
 target_include_directories("${PROJECT_NAME}" PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/src/")
-target_link_libraries("${PROJECT_NAME}" PUBLIC "${OPENVR_LIB}" protobuf::libprotoc protobuf::libprotobuf protobuf::libprotobuf-lite)
+target_link_libraries("${PROJECT_NAME}" PUBLIC "${OPENVR_LIB}" protobuf::libprotoc protobuf::libprotobuf protobuf::libprotobuf-lite simdjson::simdjson)
 set_property(TARGET "${PROJECT_NAME}" PROPERTY CXX_STANDARD 17)
 include_directories(${Protobuf_INCLUDE_DIRS})
 include_directories(${CMAKE_CURRENT_BINARY_DIR})

src/IVRDriver.hpp

@@ -3,10 +3,20 @@
 #include <vector>
 #include <chrono>
 #include <variant>
+#include <optional>
 #include <openvr_driver.h>
 #include "IVRDevice.hpp"
+#include <simdjson.h>
 
 namespace SlimeVRDriver {
+    class UniverseTranslation {
+        public:
+            // TODO: do we want to store this differently?
+            vr::HmdVector3_t translation;
+            float yaw;
+
+            static UniverseTranslation parse(simdjson::ondemand::object &obj);
+    };
 
     typedef std::variant<std::monostate, std::string, int, float, bool> SettingsValue;
 
@@ -63,6 +73,11 @@ namespace SlimeVRDriver {
         /// <returns>OpenVR VRServerDriverHost pointer</returns>
         virtual vr::IVRServerDriverHost* GetDriverHost() = 0;
 
+        /// <summary>
+        /// Gets the current UniverseTranslation
+        /// </summary>
+        virtual std::optional<UniverseTranslation> GetCurrentUniverse() = 0;
+
         /// <summary>
         /// Writes a log message
         /// </summary>

src/TrackerDevice.cpp

@@ -59,6 +59,21 @@ void SlimeVRDriver::TrackerDevice::PositionMessage(messages::Position &position)
     pose.qRotation.y = position.qy();
     pose.qRotation.z = position.qz();
 
+    auto current_universe = GetDriver()->GetCurrentUniverse();
+    if (current_universe.has_value()) {
+        auto trans = current_universe.value();
+
+        // TODO: set this once, somewhere?
+        pose.vecWorldFromDriverTranslation[0] = -trans.translation.v[0];
+        pose.vecWorldFromDriverTranslation[1] = -trans.translation.v[1];
+        pose.vecWorldFromDriverTranslation[2] = -trans.translation.v[2];
+
+        pose.qWorldFromDriverRotation.w = cos(trans.yaw / 2);
+        pose.qWorldFromDriverRotation.x = 0;
+        pose.qWorldFromDriverRotation.y = sin(trans.yaw / 2);
+        pose.qWorldFromDriverRotation.z = 0;
+    }
+
     // Post pose
     GetDriver()->GetDriverHost()->TrackedDevicePoseUpdated(this->device_index_, pose, sizeof(vr::DriverPose_t));
     this->last_pose_ = pose;

src/VRDriver.cpp

@@ -3,6 +3,8 @@
 #include "bridge/bridge.hpp"
 #include "TrackerRole.hpp"
 #include <google/protobuf/arena.h>
+#include <simdjson.h>
+#include "VRPaths_openvr.hpp"
 
 
 vr::EVRInitError SlimeVRDriver::VRDriver::Init(vr::IVRDriverContext* pDriverContext)
@@ -13,9 +15,22 @@ vr::EVRInitError SlimeVRDriver::VRDriver::Init(vr::IVRDriverContext* pDriverCont
     }
 
     Log("Activating SlimeVR Driver...");
+
+    try {
+        auto json = simdjson::padded_string::load(GetVRPathRegistryFilename()); // load VR Path Registry
+        simdjson::ondemand::document doc = json_parser.iterate(json);
+        auto path = std::string { doc.get_object()["config"].at(0).get_string().value() };
+        // Log(path);
+        default_chap_path_ = GetDefaultChaperoneFromConfigPath(path);
+    } catch (simdjson::simdjson_error& e) {
+        std::stringstream ss;
+        ss << "Error getting VR Config path, continuing: " << e.error();
+        Log(ss.str());
+    }
+
     Log("SlimeVR Driver Loaded Successfully");
 
-	return vr::VRInitError_None;
+    return vr::VRInitError_None;
 }
 
 void SlimeVRDriver::VRDriver::Cleanup()
@@ -92,12 +107,52 @@ void SlimeVRDriver::VRDriver::RunFrame()
             Log("Sent HMD hello message");
         }
 
+        uint64_t universe = vr::VRProperties()->GetUint64Property(vr::VRProperties()->TrackedDeviceToPropertyContainer(0), vr::Prop_CurrentUniverseId_Uint64);
+        if (!current_universe.has_value() || current_universe.value().first != universe) {
+            auto res = search_universes(universe);
+            if (res.has_value()) {
+                current_universe.emplace(universe, res.value());
+            } else {
+                Log("Failed to find current universe!");
+            }
+        }
+
         vr::TrackedDevicePose_t hmd_pose[10];
         vr::VRServerDriverHost()->GetRawTrackedDevicePoses(0, hmd_pose, 10);
 
         vr::HmdQuaternion_t q = GetRotation(hmd_pose[0].mDeviceToAbsoluteTracking);
         vr::HmdVector3_t pos = GetPosition(hmd_pose[0].mDeviceToAbsoluteTracking);
 
+        if (current_universe.has_value()) {
+            auto trans = current_universe.value().second;
+            pos.v[0] += trans.translation.v[0];
+            pos.v[1] += trans.translation.v[1];
+            pos.v[2] += trans.translation.v[2];
+
+            // rotate by quaternion w = cos(-trans.yaw / 2), x = 0, y = sin(-trans.yaw / 2), z = 0
+            auto tmp_w = cos(-trans.yaw / 2);
+            auto tmp_y = sin(-trans.yaw / 2);
+            auto new_w = tmp_w * q.w - tmp_y * q.y;
+            auto new_x = tmp_w * q.x + tmp_y * q.z;
+            auto new_y = tmp_w * q.y + tmp_y * q.w;
+            auto new_z = tmp_w * q.z - tmp_y * q.x;
+
+            q.w = new_w;
+            q.x = new_x;
+            q.y = new_y;
+            q.z = new_z;
+
+            // rotate point on the xz plane by -trans.yaw radians
+            // this is equivilant to the quaternion multiplication, after applying the double angle formula.
+            float tmp_sin = sin(-trans.yaw);
+            float tmp_cos = cos(-trans.yaw);
+            auto pos_x = pos.v[0] * tmp_cos + pos.v[2] * tmp_sin;
+            auto pos_z = pos.v[0] * -tmp_sin + pos.v[2] * tmp_cos;
+
+            pos.v[0] = pos_x;
+            pos.v[2] = pos_z;
+        }
+
         messages::Position* hmdPosition = google::protobuf::Arena::CreateMessage<messages::Position>(&arena);
         message->set_allocated_position(hmdPosition);
 
@@ -264,3 +319,73 @@ vr::HmdVector3_t SlimeVRDriver::VRDriver::GetPosition(vr::HmdMatrix34_t &matrix)
 
     return vector;
 }
+
+SlimeVRDriver::UniverseTranslation SlimeVRDriver::UniverseTranslation::parse(simdjson::ondemand::object &obj) {
+    SlimeVRDriver::UniverseTranslation res;
+    int iii = 0;
+    for (auto component: obj["translation"]) {
+        if (iii > 2) {
+            break; // TODO: 4 components in a translation vector? should this be an error?
+        }
+        res.translation.v[iii] = component.get_double();
+        iii += 1;
+    }
+    res.yaw = obj["yaw"].get_double();
+
+    return res;
+}
+
+std::optional<SlimeVRDriver::UniverseTranslation> SlimeVRDriver::VRDriver::search_universe(std::string path, uint64_t target) {
+    try {
+        auto json = simdjson::padded_string::load(path); // load VR Path Registry
+        simdjson::ondemand::document doc = json_parser.iterate(json);
+
+        for (simdjson::ondemand::object uni: doc["universes"]) {
+            // TODO: universeID comes after the translation, would it be faster to unconditionally parse the translation?
+            auto elem = uni["universeID"];
+            uint64_t parsed_universe;
+
+            auto is_integer = elem.is_integer();
+            if (!is_integer.error() && is_integer.value_unsafe()) {
+                parsed_universe = elem.get_uint64();
+            } else {
+                parsed_universe = elem.get_uint64_in_string();
+            }
+
+            if (parsed_universe == target) {
+                return SlimeVRDriver::UniverseTranslation::parse(uni["standing"].get_object().value());
+            }
+        }
+    } catch (simdjson::simdjson_error& e) {
+        std::stringstream ss;
+        ss << "Error getting universes from \"" << path << "\": " << e.error();
+        Log(ss.str());
+        return std::nullopt;
+    }
+
+    return std::nullopt;
+}
+
+std::optional<SlimeVRDriver::UniverseTranslation> SlimeVRDriver::VRDriver::search_universes(uint64_t target) {
+    auto driver_chap_path = vr::VRProperties()->GetStringProperty(vr::VRProperties()->TrackedDeviceToPropertyContainer(0), vr::Prop_DriverProvidedChaperonePath_String);
+    if (driver_chap_path != "") {
+        auto driver_res = search_universe(driver_chap_path, target);
+        if (driver_res.has_value()) {
+            return driver_res.value();
+        }
+    }
+
+    if (default_chap_path_.has_value()) {
+        return search_universe(default_chap_path_.value(), target);
+    }
+
+    return std::nullopt;
+}
+
+std::optional<SlimeVRDriver::UniverseTranslation> SlimeVRDriver::VRDriver::GetCurrentUniverse() {
+    if (current_universe.has_value()) {
+        return current_universe.value().second;
+    } else {
+        return std::nullopt;
+    }
+}

src/VRDriver.hpp

@@ -3,12 +3,15 @@
 
 #include <vector>
 #include <memory>
+#include <optional>
 
 #include <openvr_driver.h>
 
 #include <IVRDriver.hpp>
 #include <IVRDevice.hpp>
 
+#include <simdjson.h>
+
 namespace SlimeVRDriver {
     class VRDriver : public IVRDriver {
     public:
@@ -34,6 +37,8 @@ namespace SlimeVRDriver {
         virtual void LeaveStandby() override;
         virtual ~VRDriver() = default;
 
+        virtual std::optional<UniverseTranslation> GetCurrentUniverse() override;
+
     private:
         std::vector<std::shared_ptr<IVRDevice>> devices_;
         std::vector<vr::VREvent_t> openvr_events_;
@@ -47,5 +52,14 @@ namespace SlimeVRDriver {
         vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t &matrix);
 
         bool sentHmdAddMessage = false;
+
+        simdjson::ondemand::parser json_parser;
+        std::optional<std::string> default_chap_path_ = std::nullopt;
+        //std::map<int, UniverseTranslation> universes;
+
+        std::optional<std::pair<uint64_t, UniverseTranslation>> current_universe = std::nullopt;
+
+        std::optional<UniverseTranslation> search_universe(std::string path, uint64_t target);
+        std::optional<UniverseTranslation> search_universes(uint64_t target);
     };
 };