PSMS: Use controller-provided physics

KinectToVR · Oct 7, 2022 · e41e4be · e41e4be
1 parent 1f96692
commit e41e4be
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 6 deletions.
diff --git a/device_PSMoveService/PSMoveServiceHandler.cpp b/device_PSMoveService/PSMoveServiceHandler.cpp
@@ -10,7 +10,7 @@ Eigen::Vector3d PSMSToEigen(PSMVector3f v)
 
 Eigen::Quaterniond PSMSToEigen(PSMQuatf q)
 {
-	return { q.w, q.x, q.y, q.z };
+	return {q.w, q.x, q.y, q.z};
 }
 
 HRESULT PSMoveServiceHandler::getStatusResult()
@@ -331,7 +331,7 @@ void PSMoveServiceHandler::rebuildControllerList()
 			break;
 		}
 
-		LOG(INFO) << "Controller ID : " << controllerList.controller_id[cntlr_ix] << 
+		LOG(INFO) << "Controller ID : " << controllerList.controller_id[cntlr_ix] <<
 			" is a " << WStringToString(controller_type);
 
 		using namespace std::literals;
@@ -376,10 +376,19 @@ void PSMoveServiceHandler::processKeyInputs()
 
 		// Update joint's position
 		trackedJoints.at(wrapper.controller->ControllerID).
-		              update(PSMSToEigen(controller.Pose.Position),
-		                     wrapper.orientationOffset.inverse() * PSMSToEigen(controller.Pose.Orientation),
-		                     // Recenter (offset)
-		                     ktvr::State_Tracked);
+		              update(
+			              // Pose : [pos, ori]
+			              PSMSToEigen(controller.Pose.Position),
+			              wrapper.orientationOffset.inverse() * PSMSToEigen(controller.Pose.Orientation),
+
+			              // Physics : [v, a, ang_v, ang_a] : [cm]->[m] (1/100)
+			              PSMSToEigen(controller.PhysicsData.LinearVelocityCmPerSec) * 0.01,
+			              PSMSToEigen(controller.PhysicsData.LinearAccelerationCmPerSecSqr) * 0.01,
+						  PSMSToEigen(controller.PhysicsData.AngularVelocityRadPerSec) * 0.01,
+						  PSMSToEigen(controller.PhysicsData.AngularAccelerationRadPerSecSqr) * 0.01,
+
+			              // Recenter (offset)
+			              ktvr::State_Tracked);
 
 		// Optionally signal the joint
 		if (wrapper.flashNow)

diff --git a/device_PSMoveService/PSMoveServiceHandler.h b/device_PSMoveService/PSMoveServiceHandler.h
@@ -33,6 +33,9 @@ class PSMoveServiceHandler : public ktvr::K2TrackingDeviceBase_JointsBasis
 		deviceName = L"PSMove Service";
 		Flags_SettingsSupported = true; // Until the status is OK
 
+		// Use PSMS physics instead of the provided one
+		Flags_OverridesJointPhysics = true;
+
 		// Push the placeholders in
 		for (size_t i = 0; i < PSMOVESERVICE_MAX_CONTROLLER_COUNT; i++)
 			trackedJoints.push_back(ktvr::K2TrackedJoint(L"INVALID " + std::to_wstring(i)));