From 04687352df6bdc3d8249125040f68d165a01fffd Mon Sep 17 00:00:00 2001
From: Moritz Ls <loom@hrz.tu-chemnitz.de>
Date: Fri, 16 Feb 2018 15:43:53 +0100
Subject: [PATCH] Add x and z offset to recenter playspace during floor fix.

---
 src/tabcontrollers/FixFloorTabController.cpp | 18 +++++++++++-------
 src/tabcontrollers/FixFloorTabController.h   |  6 +++++-
 2 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/src/tabcontrollers/FixFloorTabController.cpp b/src/tabcontrollers/FixFloorTabController.cpp
index 4cb2664..5d48eef 100644
--- a/src/tabcontrollers/FixFloorTabController.cpp
+++ b/src/tabcontrollers/FixFloorTabController.cpp
@@ -62,7 +62,9 @@ void FixFloorTabController::eventLoopTick(vr::TrackedDevicePose_t* devicePoses)
 				}
 
 				auto& m = devicePoses[referenceController].mDeviceToAbsoluteTracking.m;
+				tempOffsetX = (double)m[0][3];
 				tempOffsetY = (double)m[1][3];
+				tempOffsetZ = (double)m[2][3];
 				/*
 				| Intrinsic y-x'-z" rotation matrix:
 				| cr*cy+sp*sr*sy | cr*sp*sy-cy*sr | cp*sy |
@@ -96,12 +98,14 @@ void FixFloorTabController::eventLoopTick(vr::TrackedDevicePose_t* devicePoses)
 
 			if (measurementCount >= 25) {
 				if (std::abs(tempRoll) <= M_PI_2) {
-					floorOffset = tempOffsetY - controllerUpOffsetCorrection;
+					floorOffsetY = tempOffsetY - controllerUpOffsetCorrection;
 				} else {
-					floorOffset = tempOffsetY - controllerDownOffsetCorrection;
+					floorOffsetY = tempOffsetY - controllerDownOffsetCorrection;
 				}
-				LOG(INFO) << "Fix Floor: Floor Offset = " << floorOffset;
-				parent->AddOffsetToUniverseCenter(vr::TrackingUniverseStanding, 1, floorOffset, false);
+				LOG(INFO) << "Fix Floor: Floor Offset = [" << floorOffsetX << ", " << floorOffsetY << ", " << floorOffsetZ << "]";
+				parent->AddOffsetToUniverseCenter(vr::TrackingUniverseStanding, 0, floorOffsetX, false);
+				parent->AddOffsetToUniverseCenter(vr::TrackingUniverseStanding, 1, floorOffsetY, false);
+				parent->AddOffsetToUniverseCenter(vr::TrackingUniverseStanding, 2, floorOffsetZ, false);
 				statusMessage = "Fixing ... OK";
 				statusMessageTimeout = 1.0;
 				emit statusMessageSignal();
@@ -144,9 +148,9 @@ void FixFloorTabController::fixFloorClicked() {
 }
 
 void FixFloorTabController::undoFixFloorClicked() {
-	parent->AddOffsetToUniverseCenter(vr::TrackingUniverseStanding, 1, -floorOffset, false);
-	LOG(INFO) << "Fix Floor: Undo Floor Offset = " << -floorOffset;
-	floorOffset = 0.0f;
+	parent->AddOffsetToUniverseCenter(vr::TrackingUniverseStanding, 1, -floorOffsetY, false);
+	LOG(INFO) << "Fix Floor: Undo Floor Offset = " << -floorOffsetY;
+	floorOffsetY = 0.0f;
 	statusMessage = "Undo ... OK";
 	statusMessageTimeout = 1.0;
 	emit statusMessageSignal();
diff --git a/src/tabcontrollers/FixFloorTabController.h b/src/tabcontrollers/FixFloorTabController.h
index a5323ac..1f7e358 100644
--- a/src/tabcontrollers/FixFloorTabController.h
+++ b/src/tabcontrollers/FixFloorTabController.h
@@ -25,9 +25,13 @@ private:
 	int state = 0; // 0 .. idle, 1 .. measurement in progress
 	vr::TrackedDeviceIndex_t referenceController = vr::k_unTrackedDeviceIndexInvalid;
 	unsigned measurementCount = 0;
+	double tempOffsetX = 0.0;
 	double tempOffsetY = 0.0;
+	double tempOffsetZ = 0.0;
 	double tempRoll = 0.0;
-	float floorOffset = 0.0f;
+	float floorOffsetX = 0.0f;
+	float floorOffsetY = 0.0f;
+	float floorOffsetZ = 0.0f;
 	QString statusMessage = "";
 	float statusMessageTimeout = 0.0f;
 	bool m_canUndo = false;