main.cpp

/*
// Retrieve Body Data
IBody* bodies[BODY_COUNT];
ERROR_CHECK( bodyFrame->GetAndRefreshBodyData( BODY_COUNT, &bodies[0] ) );

// Find Closest Body
const int count = findClosestBody( bodies );

// Retrieve Closest Body
IBody* body = bodies[count];

// Check Tracked
BOOLEAN tracked = FALSE;
ERROR_CHECK( body->get_IsTracked( &tracked ) );
if( !tracked ){
    // Not Tracked
    return;
}
else{
    // Tracked
    return;
}
*/

// Find Closest Body
int findClosestBody( IBody* const bodies[] )
{
    int closest_count = 0;
    float closest_distance = std::numeric_limits<float>::max();
    
    for( int count = 0; count < BODY_COUNT; count++ ){
        IBody* const body = bodies[count];
        BOOLEAN tracked;
        ERROR_CHECK( body->get_IsTracked( &tracked ) );
        if( !tracked ){
            continue;
        }

        // Retrieve Joint (SpineBase)
        Joint joints[JointType::JointType_Count];
        ERROR_CHECK( body->GetJoints( JointType::JointType_Count, &joints[0] ) );
        const Joint joint = joints[JointType::JointType_SpineBase];
        if( joint.TrackingState == TrackingState::TrackingState_NotTracked ){
            continue;
        }

        // Calculate Distance from Sensor ( √( x^2 + y^2 + z^2 ) )
        const CameraSpacePoint point = joint.Position;
        const float distance = std::sqrt( std::pow( point.X, 2 ) + std::pow( point.Y, 2 ) + std::pow( point.Z, 2 ) );
        if( closest_distance <= distance ){
            continue;
        }
        
        // Update Closest Distance and Count
        closest_distance = distance;
        closest_count = count;
    }
    
    return closest_count;
}