Made physics optional, removed 'prediction' filters

KinectToVR · Nov 7, 2022 · 84c4786 · 84c4786
1 parent 44af41f
commit 84c4786
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 42 deletions.
diff --git a/Amethyst/JointExpander.h b/Amethyst/JointExpander.h
@@ -522,8 +522,6 @@ namespace winrt::Microsoft::UI::Xaml::Controls
 				L"SettingsPage", L"Filters/Position/LowPass")));
 			_kalman.Content(box_value(k2app::interfacing::LocalizedResourceWString(
 				L"SettingsPage", L"Filters/Position/Kalman")));
-			_predict.Content(box_value(k2app::interfacing::LocalizedResourceWString(
-				L"SettingsPage", L"Filters/Position/Prediction")));
 			_pos_off.Content(box_value(k2app::interfacing::LocalizedResourceWString(
 				L"SettingsPage", L"Filters/Position/Off")));
 

diff --git a/Amethyst/K2AppTracker.h b/Amethyst/K2AppTracker.h
@@ -101,8 +101,6 @@ namespace k2app
 		k2_PositionTrackingFilter_Lowpass,
 		// Extended Kalman
 		k2_PositionTrackingFilter_Kalman,
-		// Hekky^ pose prediction
-		k2_PositionTrackingFilter_Prediction,
 		// Filter Off
 		k2_NoPositionTrackingFilter
 	};
@@ -271,13 +269,6 @@ namespace k2app
 
 			/* Update the pose prediction filter */
 			predictedPosition = pose_position;
-
-			/* Notes for `k2_PositionTrackingFilter_Prediction\\predictedPosition` impl:
-			 *
-			 * - if you need timestamps, you're gonna need to manage them manually (sorry)
-			 * - you may want to create your own filter class and put it in `[repo]/external/vendor/`, like lowpass and kalman filters
-			 * - the tracker class will auto-manage your filter, just output the final position to the `predictedPosition` variable
-			 */
 		}
 
 		// Update orientation filters
@@ -294,31 +285,6 @@ namespace k2app
 			lastSLERPSlowOrientation = pose_orientation.normalized(); // Backup the orientation
 		}
 
-		// Update the internal physics components,
-		// not called if the managing device overrides physics
-		void updateInternalPhysics()
-		{
-			// Timestamps
-			//pose_poseTimestamp;
-			//pose_previousPoseTimestamp;
-
-			// Pose components
-			//pose_position
-			//pose_previousPosition
-			//pose_orientation
-			//pose_previousOrientation
-
-			// Physics components to update
-			//pose_velocity;
-			//pose_angularVelocity;
-			//pose_acceleration;
-			//pose_angularAcceleration;
-
-			// Called after all orientation&pose composes,
-			// MUST YIELD UNCALIBRATED-SPACE COMPONENTS
-			// (which get calibrated at getTrackerBase())
-		}
-
 		// Get filtered data
 		// By default, the saved filter is selected,
 		// and to select it, the filter number must be < 0
@@ -341,8 +307,6 @@ namespace k2app
 				return lowPassPosition;
 			case k2_PositionTrackingFilter_Kalman:
 				return kalmanPosition;
-			case k2_PositionTrackingFilter_Prediction:
-				return predictedPosition;
 			case k2_NoPositionTrackingFilter:
 				return pose_position;
 			}
@@ -510,6 +474,7 @@ namespace k2app
 			tracker_base.mutable_pose()->mutable_position()->set_z(_full_position.z());
 
 			// Physics
+			if (overridePhysics)
 			{
 				const auto _full_velocity =
 					not_calibrated
@@ -598,6 +563,7 @@ namespace k2app
 			tracker_base.mutable_pose()->mutable_position()->set_z(_full_position.z());
 
 			// Physics
+			if (overridePhysics)
 			{
 				// Velocity
 				tracker_base.mutable_pose()->mutable_physics()->mutable_velocity()->set_x(pose_velocity.x());
@@ -667,6 +633,7 @@ namespace k2app
 
 		// Is this tracker enabled?
 		bool data_isActive = false;
+		bool overridePhysics = false;
 
 		// Does the managing device request no pos filtering?
 		bool m_no_position_filtering_requested = false;
@@ -696,6 +663,8 @@ namespace k2app
 				CEREAL_NVP(overrideJointID),
 				CEREAL_NVP(overrideGUID),
 
+				CEREAL_NVP(overridePhysics),
+
 				CEREAL_NVP(pose_position),
 				CEREAL_NVP(pose_orientation),
 				CEREAL_NVP(pose_previousPosition),

diff --git a/Amethyst/K2Main.h b/Amethyst/K2Main.h
@@ -841,6 +841,7 @@ namespace k2app::main
 					// If the device overrides physics
 					if (_kinect->isPhysicsOverrideEnabled())
 					{
+						tracker.overridePhysics = true; // Mark as requested
 						tracker.pose_velocity = _joint.getJointVelocity();
 						tracker.pose_acceleration = _joint.getJointAcceleration();
 						tracker.pose_angularVelocity = _joint.getJointAngularVelocity();
@@ -849,7 +850,7 @@ namespace k2app::main
 					// If not and the tracker is not overriden
 					else if (!tracker.isPositionOverridden ||
 						!K2Settings.overrideDeviceGUIDsMap.contains(tracker.overrideGUID))
-						tracker.updateInternalPhysics();
+						tracker.overridePhysics = false;
 				}
 			}
 			else if (_device.index() == 1)
@@ -874,6 +875,7 @@ namespace k2app::main
 					// If the device overrides physics
 					if (_joints->isPhysicsOverrideEnabled())
 					{
+						tracker.overridePhysics = true; // Mark as requested
 						tracker.pose_velocity = _joint.getJointVelocity();
 						tracker.pose_acceleration = _joint.getJointAcceleration();
 						tracker.pose_angularVelocity = _joint.getJointAngularVelocity();
@@ -882,7 +884,7 @@ namespace k2app::main
 					// If not and the tracker is not overriden
 					else if (!tracker.isPositionOverridden ||
 						!K2Settings.overrideDeviceGUIDsMap.contains(tracker.overrideGUID))
-						tracker.updateInternalPhysics();
+						tracker.overridePhysics = false;
 				}
 			}
 		}
@@ -1145,12 +1147,13 @@ namespace k2app::main
 							// If the device overrides physics
 							if (_kinect->isPhysicsOverrideEnabled())
 							{
+								tracker.overridePhysics = true; // Mark as requested
 								tracker.pose_velocity = _joint.getJointVelocity();
 								tracker.pose_acceleration = _joint.getJointAcceleration();
 								tracker.pose_angularVelocity = _joint.getJointAngularVelocity();
 								tracker.pose_angularAcceleration = _joint.getJointAngularAcceleration();
 							}
-							else tracker.updateInternalPhysics();
+							else tracker.overridePhysics = false;
 						}
 				}
 				else if (_device.index() == 1)
@@ -1177,12 +1180,13 @@ namespace k2app::main
 							// If the device overrides physics
 							if (_joints->isPhysicsOverrideEnabled())
 							{
+								tracker.overridePhysics = true; // Mark as requested
 								tracker.pose_velocity = _joint.getJointVelocity();
 								tracker.pose_acceleration = _joint.getJointAcceleration();
 								tracker.pose_angularVelocity = _joint.getJointAngularVelocity();
 								tracker.pose_angularAcceleration = _joint.getJointAngularAcceleration();
 							}
-							else tracker.updateInternalPhysics();
+							else tracker.overridePhysics = false;
 						}
 				}
 			}

diff --git a/driver_Amethyst/K2Tracker.cpp b/driver_Amethyst/K2Tracker.cpp
@@ -119,6 +119,29 @@ void K2Tracker::set_pose(const ktvr::K2TrackerPose& pose)
 			_pose.vecAngularAcceleration[2] = pose.physics().angularacceleration().z();
 		}
 
+		else
+		{
+			// Velocity
+			_pose.vecVelocity[0] = 0.;
+			_pose.vecVelocity[1] = 0.;
+			_pose.vecVelocity[2] = 0.;
+
+			// Acceleration
+			_pose.vecAcceleration[0] = 0.;
+			_pose.vecAcceleration[1] = 0.;
+			_pose.vecAcceleration[2] = 0.;
+
+			// Angular Velocity
+			_pose.vecAngularVelocity[0] = 0.;
+			_pose.vecAngularVelocity[1] = 0.;
+			_pose.vecAngularVelocity[2] = 0.;
+
+			// Angular Acceleration
+			_pose.vecAngularAcceleration[0] = 0.;
+			_pose.vecAngularAcceleration[1] = 0.;
+			_pose.vecAngularAcceleration[2] = 0.;
+		}
+
 		// Automatically update the tracker when finished
 		update(); // called from this
 	}