bool KSensor::getBodyData()
{
    QTextStream forLog(m_log);
    // Get frame
    IBodyFrame* frame = NULL;
    hr = m_reader->AcquireLatestFrame(&frame);
    if (FAILED(hr)) {
        forLog << "Failed to acquire latest frame. hr = " << hr << endl;
        return false;
    } else {
        forLog << "Got frame. ";
        INT64 relTime = 0;
        hr = frame->get_RelativeTime(&relTime);

        IBody* bodies[BODY_COUNT] = { 0 };
        if (FAILED(hr)) {
            forLog << "Could not get relative time. hr = " << hr << endl;
        } else {
            forLog << "RelativeTime=" << relTime;
            hr = frame->GetAndRefreshBodyData(_countof(bodies), bodies);
        }

        if (FAILED(hr)) {
            forLog << "Could not get and refresh body data. hr = " << hr << endl;
        } else {
            processBodyFrameData(bodies);
        }

        for (int i = 0; i < _countof(bodies); ++i) {
            safeRelease(&bodies[i]);
        }
        safeRelease(&frame);
    }
    return true;
}

void KSensor::processBodyFrameData(IBody** bodies)
{
    QTextStream furLog(m_log);
    bool discardFrame = true;
    BOOLEAN isTracked;
    Joint joints[JointType_Count];
    JointOrientation orientations[JointType_Count];
    for (int i = 0; i < BODY_COUNT; i++) {
        bodies[i]->get_IsTracked(&isTracked);
        if (isTracked) {
            furLog << " BodyIndex=" << i;
            bodies[i]->GetJoints(JointType_Count, joints);
            bodies[i]->GetJointOrientations(JointType_Count, orientations);
            discardFrame = false;
        }
    }

    if (!discardFrame) {
        m_acceptedFrames++;
        furLog << " Frame=" << m_acceptedFrames;

        if (m_frameBegin != 0) m_frameEnd = clock();
        m_totalSeconds += double(m_frameEnd - m_frameBegin) / CLOCKS_PER_SEC;
        furLog << " Seconds=" << m_totalSeconds;

        m_skeleton.addFrame(joints, orientations, m_totalSeconds, m_isRecording);
        //addMarkerData();
        calculateFPS();
        furLog << " FPS=" << m_fps << endl;
        m_frameBegin = clock();
    } else {
        furLog << " Frame dropped." << endl;
    }
}