G:/work/subversion/scol-technologies/trunk/plugins/OpenNiSCOL/OpenNiSCOL/generator/User.cpp

/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2010 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

You may alternatively use this source under the terms of a specific version of
the OpenSpace3D Unrestricted License provided you have obtained such a license from
I-maginer.
-----------------------------------------------------------------------------
*/

#include "User.h"

#include "../core/Exception.h"
#include "../DeviceManager.h"
#include "../objects/KinectDevice.h"

#include "../ScolPlugin.h"

void XN_CALLBACK_TYPE NewUser(xn::UserGenerator& generator, const XnUserID nUserId, void* pCookie)
{
  NIDevice::Objects::KinectDevice* This = (NIDevice::Objects::KinectDevice*)pCookie;
  NIDevice::Objects::KinectUser* user = 0;
  boost::mutex::scoped_lock lock(This->m_mutex); 

  //get user pixels and depth pixels
  xn::SceneMetaData udata;
  This->GetUserGenerator()->GetGenerator().GetUserPixels(nUserId, udata);
  const unsigned short* ubuf = udata.Data();
  const unsigned short* dbuf = This->GetDepthGenerator()->GetDepthBuffer(false);
  //count user pixels
  int ucnt = 0;
  for (int i=0; i < (udata.XRes() * udata.YRes()); i++)
  {
    if ((ubuf[i] * dbuf[i]) > 0)
      ucnt++;
  }

  user = This->GetUser(-1);
  //ignore user if out of the depth limit
  if(user && (ucnt > 0))
  {
    user->SetId(nUserId);
    generator.GetPoseDetectionCap().StartPoseDetection("Psi", nUserId);
    PostMessage(HScol, KINECT_NEW_USER_CB, (int)user, 0);
  }
  else
  {
    This->AddExistingId(nUserId);
  }
}

void XN_CALLBACK_TYPE LostUser(xn::UserGenerator& generator, const XnUserID nUserId, void* pCookie)
{
  NIDevice::Objects::KinectDevice* This = (NIDevice::Objects::KinectDevice*)pCookie;
  NIDevice::Objects::KinectUser* user = 0;
  boost::mutex::scoped_lock lock(This->m_mutex);
 
  user = This->GetUser(nUserId);
  int ucnt = 0;

  if(user)
  {
    int uavailable = This->GetExistingAvailableId();
    //attribute an available user Id if possible
  
    //get user pixels and depth pixels
    if (uavailable != -1)
    {
      xn::SceneMetaData udata;
      This->GetUserGenerator()->GetGenerator().GetUserPixels(uavailable, udata);
      const unsigned short* ubuf = udata.Data();
      const unsigned short* dbuf = This->GetDepthGenerator()->GetDepthBuffer(false);
      //count user pixels
      for (int i=0; i < (udata.XRes() * udata.YRes()); i++)
      {
        if ((ubuf[i] * dbuf[i]) > 0)
          ucnt++;
      }

      if (ucnt > 0)
        user->SetId(uavailable);
      else
      {
        This->AddExistingId(uavailable);
        user->SetId(-1);
      }
    }
    else
      user->SetId(-1);

    PostMessage(HScol, KINECT_LOST_USER_CB, (int)user, 0);
    
    if ((uavailable != -1) && (ucnt > 0))
    {
      generator.GetPoseDetectionCap().StartPoseDetection("Psi", uavailable);
      PostMessage(HScol, KINECT_NEW_USER_CB, (int)user, 0);
    }
  }

  This->RemoveExistingId(nUserId);
}

void XN_CALLBACK_TYPE CalibrationStart(xn::SkeletonCapability& skeleton, const XnUserID nUserId, void* pCookie)
{
  NIDevice::Objects::KinectDevice* device = (NIDevice::Objects::KinectDevice*)pCookie;
  NIDevice::Objects::KinectUser* user = 0;
  user = device->GetUser(nUserId);
  if(user)
  {
    PostMessage(HScol, KINECT_USER_CALIBRATION_START_CB, (int)user, 0);
  }
}

void XN_CALLBACK_TYPE CalibrationEnd(xn::SkeletonCapability& skeleton, const XnUserID nUserId, XnBool bSuccess, void* pCookie)
{
  NIDevice::Objects::KinectDevice* device = (NIDevice::Objects::KinectDevice*)pCookie;
  NIDevice::Objects::KinectUser* user = 0;
  user = device->GetUser(nUserId);
  if(user)
  {
    if (bSuccess)
    {
      device->GetUserGenerator()->GetGenerator().GetSkeletonCap().StartTracking(nUserId);
      PostMessage(HScol, KINECT_USER_CALIBRATION_END_CB, (int)user, 0);
    }
    else
    {
      device->GetUserGenerator()->GetGenerator().GetPoseDetectionCap().StartPoseDetection("Psi", nUserId);
    }
  }
}

void XN_CALLBACK_TYPE PoseDetected(xn::PoseDetectionCapability& poseDetection, const XnChar* strPose, XnUserID nId, void* pCookie)
{
  NIDevice::Objects::KinectDevice* device = (NIDevice::Objects::KinectDevice*)pCookie;
  NIDevice::Objects::KinectUser* user = 0;
  user = device->GetUser(nId);
  if(user)
  {
    device->GetUserGenerator()->GetGenerator().GetPoseDetectionCap().StopPoseDetection(nId);
    device->GetUserGenerator()->GetGenerator().GetSkeletonCap().RequestCalibration(nId, true);
    PostMessage(HScol, KINECT_USER_POSE_DETECTED_CB, (int)user, 0);
  }
}

void XN_CALLBACK_TYPE PoseLost(xn::PoseDetectionCapability& poseDetection, const XnChar* strPose, XnUserID nId, void* pCookie)
{
  NIDevice::Objects::KinectDevice* device = (NIDevice::Objects::KinectDevice*)pCookie;
  NIDevice::Objects::KinectUser* user = 0;
  user = device->GetUser(nId);
  if(user)
  {
    PostMessage(HScol, KINECT_USER_POSE_LOST_CB, (int)user, 0);
  }
}

namespace NIDevice
{
  namespace Generator
  {
    User::User(NIDevice::Objects::KinectDevice* pDevice) : NIDevice::Core::GenericGenerator<xn::UserGenerator, XN_NODE_TYPE_USER>(pDevice)
    {
      registerCallbacks();
    }

    User::User(): NIDevice::Core::GenericGenerator<xn::UserGenerator, XN_NODE_TYPE_USER>(0)
    {
    }

    User::~User()
    {
      unRegisterCallbacks();
    }

    const unsigned short* User::GetUserPixels()
    {
      GetGenerator().GetUserPixels(0, g_sceneMD);
      return (const_cast<unsigned short*>(g_sceneMD.Data()));
    }

    void User::GetImageSize(int &w, int &h)
    {
      w = g_sceneMD.XRes();
      h = g_sceneMD.YRes();
    }

    void User::requestInitialPos(NIDevice::Objects::KinectUser* user)
    {
      if(user->GetId() > 0 && GetGenerator())
      {
        GetGenerator().GetPoseDetectionCap().StartPoseDetection("Psi", user->GetId());
        PostMessage(HScol, KINECT_NEW_USER_CB, (int)user, 0);
      }
    }

    void User::stopTracking(NIDevice::Objects::KinectUser* user)
    {
      if(user->GetId() > 0 && GetGenerator())
      {
        GetGenerator().GetSkeletonCap().StopTracking(user->GetId());
      }
    }

    void User::registerCallbacks()
    {
      xn::SkeletonCapability skl = GetGenerator().GetSkeletonCap();
      skl.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
      skl.SetSmoothing(0.6f);
      GetGenerator().RegisterUserCallbacks(NewUser, LostUser, GetParentDevice(), hUserCallbacks);
      skl.RegisterCalibrationCallbacks(CalibrationStart, CalibrationEnd, GetParentDevice(), m_hCalibrationCallbacks);
      GetGenerator().GetPoseDetectionCap().RegisterToPoseCallbacks(PoseDetected, PoseLost, GetParentDevice(), m_hPoseCallbacks);

      // Make sure OpenNI nodes start generating
      GetParentDevice()->GetContext().StartGeneratingAll();
    }

    void User::unRegisterCallbacks()
    {
      if (hUserCallbacks)
      {
        GetGenerator().UnregisterUserCallbacks(hUserCallbacks);
        hUserCallbacks = 0;
      }

      if(m_hCalibrationCallbacks)
      {
        GetGenerator().GetSkeletonCap().UnregisterCalibrationCallbacks(m_hCalibrationCallbacks);
        m_hCalibrationCallbacks = 0;
      }

      if(m_hPoseCallbacks)
      {
        GetGenerator().GetPoseDetectionCap().UnregisterFromPoseCallbacks(m_hPoseCallbacks);
        m_hPoseCallbacks = 0;
      }
    }
  }
}