Skip to content

Commit

Permalink
Stub internal tracker physics
Browse files Browse the repository at this point in the history
  • Loading branch information
@KimihikoAkayasaki
KimihikoAkayasaki committed Oct 15, 2022
1 parent f817687 commit 07cd583
Show file tree
Hide file tree
Showing 3 changed files with 39 additions and 90 deletions.
32 changes: 23 additions & 9 deletions Amethyst/K2AppTracker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -280,6 +280,7 @@ namespace k2app
*/
}

// Update orientation filters
void updateOrientationFilters()
{
// ik that's a bunch of normalizations but we really em, weird things happen sometimes
Expand All @@ -293,6 +294,25 @@ 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;

// 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
Expand Down Expand Up @@ -469,8 +489,7 @@ namespace k2app
tracker_base.mutable_pose()->mutable_position()->set_y(_full_position.y());
tracker_base.mutable_pose()->mutable_position()->set_z(_full_position.z());

// If physics are provided by the device
if (m_use_own_physics)
// Physics
{
const auto _full_velocity =
not_calibrated
Expand Down Expand Up @@ -563,8 +582,7 @@ namespace k2app
tracker_base.mutable_pose()->mutable_position()->set_y(_full_position.y());
tracker_base.mutable_pose()->mutable_position()->set_z(_full_position.z());

// If physics are provided by the device
if (m_use_own_physics)
// Physics
{
// Velocity
tracker_base.mutable_pose()->mutable_physics()->mutable_velocity()->set_x(pose_velocity.x());
Expand Down Expand Up @@ -639,11 +657,7 @@ namespace k2app

// Is this tracker enabled?
bool data_isActive = false;

// Does the host device override physics?
// Not saved, computed every loop
bool m_use_own_physics = false;


// Does the managing device request no pos filtering?
bool m_no_position_filtering_requested = false;

Expand Down
26 changes: 16 additions & 10 deletions Amethyst/K2Main.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -779,7 +779,7 @@ namespace k2app::main
vrPlayspaceOrientationQuaternion.inverse() * tracker.pose_orientation;

/*****************************************************************************************/
// Push RAW poses and physics to trackers
// Push RAW poses and physics to trackers, update physics
/*****************************************************************************************/

if (_device.index() == 0)
Expand All @@ -797,18 +797,21 @@ namespace k2app::main
tracker.pose_previousPoseTimestamp = _joint.getPreviousPoseTimestamp();

tracker.pose_position = _joint.getJointPosition();
tracker.m_use_own_physics = _kinect->isPhysicsOverrideEnabled();
tracker.m_no_position_filtering_requested =
_kinect->isPositionFilterBlockingEnabled();

// If the device overrides physics
if (tracker.m_use_own_physics)
if (_kinect->isPhysicsOverrideEnabled())
{
tracker.pose_velocity = _joint.getJointVelocity();
tracker.pose_acceleration = _joint.getJointAcceleration();
tracker.pose_angularVelocity = _joint.getJointAngularVelocity();
tracker.pose_angularAcceleration = _joint.getJointAngularAcceleration();
}
// If not and the tracker is not overriden
else if (!tracker.isPositionOverridden ||
!K2Settings.overrideDeviceGUIDsMap.contains(tracker.overrideGUID))
tracker.updateInternalPhysics();
}
}
else if (_device.index() == 1)
Expand All @@ -825,18 +828,21 @@ namespace k2app::main
tracker.pose_previousPoseTimestamp = _joint.getPreviousPoseTimestamp();

tracker.pose_position = _joint.getJointPosition();
tracker.m_use_own_physics = _joints->isPhysicsOverrideEnabled();
tracker.m_no_position_filtering_requested =
_joints->isPositionFilterBlockingEnabled();

// If the device overrides physics
if (tracker.m_use_own_physics)
if (_joints->isPhysicsOverrideEnabled())
{
tracker.pose_velocity = _joint.getJointVelocity();
tracker.pose_acceleration = _joint.getJointAcceleration();
tracker.pose_angularVelocity = _joint.getJointAngularVelocity();
tracker.pose_angularAcceleration = _joint.getJointAngularAcceleration();
}
// If not and the tracker is not overriden
else if (!tracker.isPositionOverridden ||
!K2Settings.overrideDeviceGUIDsMap.contains(tracker.overrideGUID))
tracker.updateInternalPhysics();
}
}
}
Expand Down Expand Up @@ -1044,7 +1050,7 @@ namespace k2app::main
}

/*****************************************************************************************/
// Push RAW poses and physics to trackers
// Push RAW poses and physics to trackers, update physics
/*****************************************************************************************/

if (_device.index() == 0)
Expand All @@ -1066,18 +1072,18 @@ namespace k2app::main
tracker.pose_previousPoseTimestamp = _joint.getPreviousPoseTimestamp();

tracker.pose_position = _joint.getJointPosition();
tracker.m_use_own_physics = _kinect->isPhysicsOverrideEnabled();
tracker.m_no_position_filtering_requested =
_kinect->isPositionFilterBlockingEnabled();

// If the device overrides physics
if (tracker.m_use_own_physics)
if (_kinect->isPhysicsOverrideEnabled())
{
tracker.pose_velocity = _joint.getJointVelocity();
tracker.pose_acceleration = _joint.getJointAcceleration();
tracker.pose_angularVelocity = _joint.getJointAngularVelocity();
tracker.pose_angularAcceleration = _joint.getJointAngularAcceleration();
}
else tracker.updateInternalPhysics();
}
}
else if (_device.index() == 1)
Expand All @@ -1096,18 +1102,18 @@ namespace k2app::main
tracker.pose_previousPoseTimestamp = _joint.getPreviousPoseTimestamp();

tracker.pose_position = _joint.getJointPosition();
tracker.m_use_own_physics = _joints->isPhysicsOverrideEnabled();
tracker.m_no_position_filtering_requested =
_joints->isPositionFilterBlockingEnabled();

// If the device overrides physics
if (tracker.m_use_own_physics)
if (_joints->isPhysicsOverrideEnabled())
{
tracker.pose_velocity = _joint.getJointVelocity();
tracker.pose_acceleration = _joint.getJointAcceleration();
tracker.pose_angularVelocity = _joint.getJointAngularVelocity();
tracker.pose_angularAcceleration = _joint.getJointAngularAcceleration();
}
else tracker.updateInternalPhysics();
}
}
}
Expand Down
71 changes: 0 additions & 71 deletions driver_Amethyst/K2Tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,12 +76,6 @@ void K2Tracker::update()
}
}

// The previous frame cached pose
Eigen::Quaterniond m_previous_orientation;
Eigen::Vector3d m_previous_position,
m_previous_pos_velocity,
m_previous_rot_velocity;

void K2Tracker::set_pose(const ktvr::K2TrackerPose& pose)
{
try
Expand Down Expand Up @@ -121,72 +115,7 @@ void K2Tracker::set_pose(const ktvr::K2TrackerPose& pose)
_pose.vecAngularAcceleration[0] = pose.physics().angularacceleration().x();
_pose.vecAngularAcceleration[1] = pose.physics().angularacceleration().y();
_pose.vecAngularAcceleration[2] = pose.physics().angularacceleration().z();

// Backup Velocity
m_previous_pos_velocity = {
_pose.vecVelocity[0],
_pose.vecVelocity[1],
_pose.vecVelocity[2]
};
m_previous_rot_velocity = {
_pose.vecAngularVelocity[0],
_pose.vecAngularVelocity[1],
_pose.vecAngularVelocity[2]
};
}
else
{
// Calculate Velocity
const auto _pos_velocity =
EigenUtils::lerp(
Eigen::Vector3d(
EigenUtils::p_cast_type<Eigen::Vector3d>(_pose) - m_previous_position /
static_cast<double>(pose.posetimestamp() - pose.previousposetimestamp()) * 0.000001),
m_previous_pos_velocity, .5f);

const auto _rot_velocity =
EigenUtils::lerp(
Eigen::Vector3d(
EigenUtils::QuatToEulers(m_previous_orientation.inverse() *
EigenUtils::p_cast_type<Eigen::Quaterniond>(_pose)) /
static_cast<double>(pose.posetimestamp() - pose.previousposetimestamp()) * 0.000001),
m_previous_rot_velocity, .5f);

// Calculate Acceleration
const auto _pos_acceleration =
_pos_velocity - m_previous_pos_velocity /
static_cast<double>(pose.posetimestamp() - pose.previousposetimestamp()) * 0.000001;

const auto _rot_acceleration =
_rot_velocity - m_previous_rot_velocity /
static_cast<double>(pose.posetimestamp() - pose.previousposetimestamp()) * 0.000001;

// Set the Velocity
_pose.vecVelocity[0] = _pos_velocity.x();
_pose.vecVelocity[1] = _pos_velocity.y();
_pose.vecVelocity[2] = _pos_velocity.z();

_pose.vecAngularVelocity[0] = _rot_velocity.x();
_pose.vecAngularVelocity[1] = _rot_velocity.y();
_pose.vecAngularVelocity[2] = _rot_velocity.z();

// Set the Acceleration
_pose.vecAcceleration[0] = _pos_acceleration.x();
_pose.vecAcceleration[1] = _pos_acceleration.y();
_pose.vecAcceleration[2] = _pos_acceleration.z();

_pose.vecAngularAcceleration[0] = _rot_acceleration.x();
_pose.vecAngularAcceleration[1] = _rot_acceleration.y();
_pose.vecAngularAcceleration[2] = _rot_acceleration.z();

// Backup Velocity
m_previous_pos_velocity = _pos_velocity;
m_previous_rot_velocity = _rot_velocity;
}

// Backup poses
m_previous_position = EigenUtils::p_cast_type<Eigen::Vector3d>(_pose);
m_previous_orientation = EigenUtils::p_cast_type<Eigen::Quaterniond>(_pose);

// Automatically update the tracker when finished
update(); // called from this
Expand Down

0 comments on commit 07cd583

Please sign in to comment.