Rift S: keep Quick Calibrate from putting the floor at head height

Quick Calibrate places the floor where the headset is, but off-head the
Rift S is in standby and holds its last head pose, so the floor ended up
at head height (#5). The Room Setup guard now keeps the calibration made
while the headset was in use. If the standing origin changes in the same
universe while the headset is in standby, the guard restores that
calibration and explains that Room Setup sets the floor. The unbounded
session universe cannot be committed through IVRChaperoneSetup, so this
restore reloads the saved file.

Each session also re-bases tracking, so offsets saved in universe 3 sent
the floor to the wrong height in later sessions. The session origin is
now reset to the plain origin at startup. The saved room (universe 2) is
unchanged.

Fixes #5

src/xrt/state_trackers/steamvr_drv/ovrd_chaperone_origin.cpp

@@ -42,14 +42,22 @@ bool rift_s_prepare_session_origin(const std::string &path)
     auto universes = cJSON_GetObjectItemCaseSensitive(document.get(), "universes");
     if (!cJSON_IsString(id) || std::strcmp(id->valuestring, "chaperone_info") || !cJSON_IsArray(universes))
         return false;
-    const cJSON *entry;
+    Json origin(cJSON_Parse("{\"universeID\":\"3\",\"seated\":{\"translation\":[0,0,0],\"yaw\":0},\"standing\":{\"translation\":[0,0,0],\"yaw\":0}}"), cJSON_Delete);
+    if (!origin) return false;
+    // Each session re-bases tracking, so offsets saved in universe 3 (for example by
+    // Quick Calibrate) would misplace the floor next time. Reset it to the plain origin.
+    int index = 0;
+    cJSON *entry;
     cJSON_ArrayForEach(entry, universes) {
         auto universe = cJSON_GetObjectItemCaseSensitive(entry, "universeID");
-        if (cJSON_IsString(universe) && std::strcmp(universe->valuestring, "3") == 0)
-            return true;
+        if (cJSON_IsString(universe) && std::strcmp(universe->valuestring, "3") == 0) {
+            if (cJSON_Compare(entry, origin.get(), true)) return true;
+            if (!cJSON_ReplaceItemInArray(universes, index, origin.get())) return false;
+            break;
+        }
+        ++index;
     }
-    Json origin(cJSON_Parse("{\"universeID\":\"3\",\"seated\":{\"translation\":[0,0,0],\"yaw\":0},\"standing\":{\"translation\":[0,0,0],\"yaw\":0}}"), cJSON_Delete);
-    if (!origin || !cJSON_AddItemToArray(universes, origin.get())) return false;
+    if (!entry && !cJSON_AddItemToArray(universes, origin.get())) return false;
     origin.release();
     std::unique_ptr<char, decltype(&cJSON_free)> bytes(cJSON_PrintUnformatted(document.get()), cJSON_free);
     if (!bytes) return false;

src/xrt/state_trackers/steamvr_drv/ovrd_chaperone_origin.hpp

@@ -3,7 +3,7 @@
 #pragma once
 #include <string>
 
-// Add a parseable, unbounded session origin before publishing universe 3.
-// The saved universe and its geometry remain unchanged. Existing entries are preserved.
+// Write a parseable, unbounded session origin before publishing universe 3, replacing any
+// offsets left in universe 3 by an earlier session. Other universes remain unchanged.
 bool rift_s_prepare_session_origin(const std::string &path);
 bool rift_s_prepare_session_origin();

src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp

@@ -370,6 +370,14 @@ public:
 };
 
 
+static bool same_transform(const vr::HmdMatrix34_t &a, const vr::HmdMatrix34_t &b)
+{
+	for (int row = 0; row < 3; ++row)
+		for (int column = 0; column < 4; ++column)
+			if (std::fabs(a.m[row][column] - b.m[row][column]) > 1e-4f) return false;
+	return true;
+}
+
 struct Snapshot
 {
 	std::string file;
@@ -387,11 +395,14 @@ static std::optional<Snapshot> snapshot(vr::IVRChaperoneSetup &setup, const fs::
 	return saved;
 }
 
-static bool restore(vr::IVRChaperoneSetup &setup, const fs::path &path, const Snapshot &saved)
+// The unbounded Rift S session universe has no perimeter, which CommitWorkingCopy rejects;
+// require_commit=false restores it from the file alone.
+static bool restore(vr::IVRChaperoneSetup &setup, const fs::path &path, const Snapshot &saved, bool require_commit = true)
 {
 	setup.RevertWorkingCopy();
-	if (!setup.ImportFromBufferToWorking(saved.exported.data(), 0) ||
-	    !setup.CommitWorkingCopy(vr::EChaperoneConfigFile_Live)) return false;
+	bool committed = setup.ImportFromBufferToWorking(saved.exported.data(), 0) &&
+	                 setup.CommitWorkingCopy(vr::EChaperoneConfigFile_Live);
+	if (!committed && require_commit) return false;
 	// Commit updates the server's live calibration. Restore the complete file too,
 	// including other universes and formatting that OpenVR's export does not retain.
 	auto temporary = path;
@@ -528,6 +539,21 @@ int main(int argc, char **argv)
 	auto next_notification_attempt = std::chrono::steady_clock::time_point{};
 	DisplayRecovery display_recovery(log_path);
 	auto next_display_check = std::chrono::steady_clock::time_point{};
+	// Quick Calibrate puts the floor where the headset is. Off-head the Rift S is in standby
+	// and holds its last head pose, so a calibration made then puts the floor at head height.
+	// Keep the last calibration made while worn and restore it over such a change.
+	const bool guard_standby_calibration = is_rift_s && setup_available;
+	std::optional<Snapshot> worn_calibration;
+	vr::HmdMatrix34_t worn_standing{};
+	uint64_t worn_universe = 0;
+	auto remember_calibration = [&] {
+		worn_calibration = snapshot(*setup, path);
+		worn_standing = system->GetRawZeroPoseToStandingAbsoluteTrackingPose();
+		worn_universe = system->GetUint64TrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd,
+		                                                       vr::Prop_CurrentUniverseId_Uint64);
+	};
+	if (guard_standby_calibration) remember_calibration();
+	bool calibration_changed = false;
 	while (running && !exited(parent)) {
 		// Test before consuming Accept's events, including a compositor that exits immediately before Accept.
 		bool recent_exit = std::chrono::steady_clock::now() <= exit_deadline;
@@ -599,9 +625,37 @@ int main(int argc, char **argv)
 			    event.eventType == vr::VREvent_RoomViewShown || event.eventType == vr::VREvent_RoomViewHidden ||
 			    event.eventType == vr::VREvent_DashboardActivated || event.eventType == vr::VREvent_DashboardDeactivated))
 				log_coordinates(*system, *setup, event.eventType, step);
+			if (guard_standby_calibration && (event.eventType == vr::VREvent_ChaperoneUniverseHasChanged ||
+			    event.eventType == vr::VREvent_ChaperoneDataHasChanged ||
+			    event.eventType == vr::VREvent_StandingZeroPoseReset))
+				calibration_changed = true;
 			if (event.eventType == vr::VREvent_Quit) { display_recovery.note_quit(); running = false; }
 		}
 		if (!running) break;
+		if (calibration_changed) {
+			calibration_changed = false;
+			bool standby = system->GetTrackedDeviceActivityLevel(vr::k_unTrackedDeviceIndex_Hmd) ==
+			               vr::k_EDeviceActivityLevel_Standby;
+			auto universe = system->GetUint64TrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd,
+			                                                       vr::Prop_CurrentUniverseId_Uint64);
+			if (standby && step == 0 && worn_calibration && universe == worn_universe &&
+			    !same_transform(system->GetRawZeroPoseToStandingAbsoluteTrackingPose(), worn_standing)) {
+				// SteamVR applies the reload asynchronously; its change event re-checks the result.
+				bool ok = restore(*setup, path, *worn_calibration, false);
+				fprintf(stderr, "monado-roomsetup-guard: standby calibration reverted=%s\n", ok ? "ok" : "FAILED");
+				if (notifications && overlay != vr::k_ulOverlayHandleInvalid) {
+					vr::VRNotificationId id;
+					notifications->CreateNotification(overlay, 0, vr::EVRNotificationType_Transient,
+					    ok ? "Quick Calibrate needs a tracked headset, and the Rift S stops tracking off your head. "
+					         "The previous floor and boundary were kept. Use Room Setup to set the floor."
+					       : "Quick Calibrate ran while the headset was off. Restoring the previous calibration failed. "
+					         "Run Room Setup before playing.",
+					    vr::EVRNotificationStyle_Application, nullptr, &id);
+				}
+			} else if (!standby || universe != worn_universe) {
+				remember_calibration();
+			}
+		}
 		if (world_setup && world_cancel_deadline != std::chrono::steady_clock::time_point{} &&
 		    std::chrono::steady_clock::now() >= world_cancel_deadline) {
 			world_command("cancel");

tests/tests_steamvr_roomsetup_guard.cpp

@@ -228,6 +228,13 @@ TEST_CASE("Native session origin has no geometry and preserves the saved room")
     CHECK(cJSON_GetObjectItemCaseSensitive(origin, "collision_bounds") == nullptr);
     REQUIRE(rift_s_prepare_session_origin(fixture.path.string()));
     CHECK(read_file(fixture.path) == bytes);
+    // A Quick Calibrate offset from an earlier session must not survive the re-based origin.
+    fixture.write(R"({"jsonid":"chaperone_info","version":5,"universes":[)" + saved +
+                  R"(,{"universeID":"3","standing":{"translation":[0,-1.6,0],"yaw":0.3}}]})");
+    REQUIRE(rift_s_prepare_session_origin(fixture.path.string()));
+    auto calibrated = read_file(fixture.path);
+    CHECK(calibrated.find(saved) != std::string::npos);
+    CHECK(calibrated == bytes);
     fixture.write("broken json");
     CHECK_FALSE(rift_s_prepare_session_origin(fixture.path.string()));
     CHECK(read_file(fixture.path) == "broken json");