void ViewpointCursor::reverseUpdate() {
glm::vec3 d = glm::normalize(cursor3D - userPos);
planePos = userPos + viewDirection*planeDistance;
wcl::Plane p(planePos, viewDirection);
Plane::Intersection i = p.intersect(userPos, d);
if (i.intersects) {
backCursor = glm::vec4(i.intersectionPoint.x, i.intersectionPoint.y, i.intersectionPoint.z, 1);
// rotate back to local...
glm::vec4 vi = glm::affineInverse(getUserTransform()) * backCursor;
cursor2D.x = vi.x;
cursor2D.y = vi.y;
}
}
// publishes TF tree for each tracked user
void publishTransforms (std::string const& frame_id)
{
XnUserID users[15];
XnUInt16 users_count = 15;
g_UserGenerator.GetUsers (users, users_count);
tf::tfMessage tfs;
for (int i = 0; i < users_count; ++i)
{
XnUserID user = users[i];
if (!g_UserGenerator.GetSkeletonCap().IsTracking (user))
continue;
std::stringstream ss;
std::string user_suffix;
ss << "_" << user;
user_suffix = ss.str ();
std::cerr << "publishTransforms: " << __FILE__ << " : " << __LINE__ << std::endl;
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_HEAD, frame_id, "head" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_NECK, frame_id, "neck" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_TORSO, frame_id, "torso" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_LEFT_SHOULDER, frame_id, "left_shoulder" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_LEFT_ELBOW, frame_id, "left_elbow" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_LEFT_HAND, frame_id, "left_hand" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_RIGHT_ELBOW, frame_id, "right_elbow" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_RIGHT_HAND, frame_id, "right_hand" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_LEFT_HIP, frame_id, "left_hip" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_LEFT_KNEE, frame_id, "left_knee" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_LEFT_FOOT, frame_id, "left_foot" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_RIGHT_HIP, frame_id, "right_hip" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_RIGHT_KNEE, frame_id, "right_knee" + user_suffix));
tfs.transforms.push_back (getUserTransform (user, XN_SKEL_RIGHT_FOOT, frame_id, "right_foot" + user_suffix));
}
tf_pub_.publish (tfs);
}
