void KinectAdapter::calcCenterBody(const Kinect2::BodyFrame& mBodyFrame, const ci::Vec2f& depthSize)
{
auto bodies = mBodyFrame.getBodies();
int bodyIndex = 0;
float closestDistanceTocenter = 100000;
filterBodies.clear();
for (const Kinect2::Body& body : bodies)
{
if (body.isTracked())
{
auto jMap = body.getJointMap();
auto spine = jMap.at(JointType::JointType_SpineBase);
auto state = spine.getTrackingState();
if (state == TrackingState::TrackingState_Tracked)
{
auto position = Vec2f(kinect().getDevice()->mapCameraToDepth(spine.getPosition()));
BodyFilter filter;
filter.position = Vec3f(position.x, position.y, spine.getPosition().z);
filter.index = bodyIndex;
filter.isCentered = false;
filter.distanceZ = float(spine.getPosition().z * 100) / 100;
filter.distanceToCenter = position.distance(depthSize * 0.5);
filter.body = body;
if (filter.distanceToCenter < closestDistanceTocenter)
{
for (size_t i = 0; i < filterBodies.size(); i++)
{
filterBodies[i].isCentered = false;
}
filter.isCentered = true;
centerBody = filter;
closestDistanceTocenter = filter.distanceToCenter;
}
filterBodies.push_back(filter);
}
}
}
}
void RibbonApp::update()
{
// get body data from Kinect
const std::vector<Kinect2::Body> svBodies = mBodyFrame.getBodies();
// if we are not tracking someone find a new one
if (mTrackingID == 0)
{
for (const Kinect2::Body& body : svBodies) {
if (body.isTracked()) {
mTrackingID = body.getId();
mPoints.clear();
// set the first point to the user hand position
const map<JointType, Kinect2::Body::Joint> joints = body.getJointMap();
auto joint = joints.find(JointType::JointType_HandRight);
if (joint == joints.end())
{
break;
}
if (joint->second.getTrackingState() != TrackingState::TrackingState_NotTracked)
{
mHand = joint->second.getPosition();
}
mPoints.push_back(mHand);
break;
}
}
}
// get the hand for the tracked person
bool bTracking = false;
for (const Kinect2::Body& body : svBodies) {
if (body.isTracked() && mTrackingID == body.getId()) {
bTracking = true; // flag that we are still tracking
const map<JointType, Kinect2::Body::Joint> joints = body.getJointMap();
auto joint = joints.find(JointType::JointType_HandRight);
if (joint == joints.end())
{
continue;
}
// may want to add that the hand state is close for example
if (joint->second.getTrackingState() == TrackingState::TrackingState_Tracked) {
Vec3f pos = joint->second.getPosition();
mHand += joint->second.getPosition() * 60.0f;
mHand *= .94f;
mHand.y *= -1.0f;
mHand.z = pos.z;
if (mPoints.size() < 100)
{
mPoints.push_back(mHand);
}
else
{
mPoints.erase(mPoints.cbegin()); // remove first item
}
}
}
}
// no one to track
if (mTrackingID == 0 || !bTracking)
{
mTrackingID = 0;
mHand += Rand::randVec3f() * 60.0;
mHand *= .94;
if (mPoints.size() > 100)
{
// remove first item
mPoints.erase(mPoints.cbegin());
}
else
{
mPoints.push_back(mHand);
}
return;
}
}