KinectUser.cpp

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

Copyright (c) 2012 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

-----------------------------------------------------------------------------
*/

#include "objects/KinectUser.h"
#include "core/DataSkeleton.h"
#include "DeviceManager.h"
#include "generator/User.h"
#include "objects/KinectDevice.h"
#include "openNiScolPlugin.h"

// Constructors
KinectUser::KinectUser()
{
}

KinectUser::KinectUser(KinectDevice* pDevice)
{
  p_KinectDevice = pDevice;
  mDataSkeleton = new DataSkeleton(this);
  uId = -1;
  
  handsData = 0;
  mleftHand = new KinectUserHand(this, nite::JOINT_LEFT_HAND);
  mrightHand = new KinectUserHand(this, nite::JOINT_RIGHT_HAND);

  mCalibrated = false;
  GenerateColor();
}

// Destructor
KinectUser::~KinectUser()
{
  if (uId > 0)
    p_KinectDevice->AddExistingId(uId);

  if (p_KinectDevice->GetUserGenerator())
    p_KinectDevice->GetUserGenerator()->stopTracking(this);

  SAFE_DELETE(mDataSkeleton);
  
  SAFE_DELETE(mleftHand);
  SAFE_DELETE(mrightHand);

  if (handsData != 0)
  {
    free(handsData);
    handsData = 0;
  }

  p_KinectDevice = 0;
}

void KinectUser::GenerateColor()
{
  unsigned int r = rand() %255;
  unsigned int g = rand() %255;
  unsigned int b = rand() %255;
  color = r + (g << 8) + (b << 16);
}

bool KinectUser::IsCalibrated()
{
  return mCalibrated;
}

void KinectUser::SetCalibrated(bool state)
{
  mCalibrated = state;
}


KinectDevice* KinectUser::GetParentDevice()
{
  return p_KinectDevice;
}

DataSkeleton* KinectUser::GetSkeletonData()
{
  return mDataSkeleton;
}

void KinectUser::SetId(int id)
{
  uId = id;
  if(id != -1)
    GenerateColor();
}

int KinectUser::GetId()
{
  return uId;
}

/*
unsigned short* KinectUser::GetNearestZone()
{
  int level = 200;
  if (uId > 0)
  {
    if (handsData == 0)
    {
      handsData = (unsigned short*)malloc(XN_VGA_X_RES * XN_VGA_Y_RES * sizeof(openni::DepthPixel));
      memset(handsData, 0, XN_VGA_X_RES * XN_VGA_Y_RES * sizeof(openni::DepthPixel));
    }

    xn::SceneMetaData udata;
    p_KinectDevice->GetUserGenerator()->GetUserPixels(uId, udata);
    xn::DepthMetaData depthMD;
    p_KinectDevice->GetDepthGenerator()->GetMetaData(depthMD);

    int nDist = 10000;
    for(int y(0); y < (int)depthMD.YRes(); ++y) 
    {
      for(int x(0); x < (int)depthMD.XRes(); ++x) 
      {
        int pos = x+y*depthMD.XRes();

        if((depthMD.Data()[pos] < nDist) && (udata.Data()[pos] != 0))
          nDist = depthMD.Data()[pos];
      }
    }

    for(int y(0); y < (int)depthMD.YRes(); ++y) 
    {
      for(int x(0); x < (int)depthMD.XRes(); ++x) 
      {
        int pos = x+y*depthMD.XRes();

        if(((depthMD.Data()[pos] * udata.Data()[pos]) >= nDist) && ((depthMD.Data()[pos] * udata.Data()[pos]) <= (nDist + level)))
          handsData[pos] = depthMD.Data()[pos];
        else
          handsData[pos] = 0;
      }
    }

    return handsData;
  }
  else
    return 0;
}*/

void KinectUser::DetectUserHands(cv::Mat depthMat, cv::Mat depthMatBgr)
{
  mleftHand->Detect(depthMat, depthMatBgr);
  mrightHand->Detect(depthMat, depthMatBgr);
}

bool KinectUser::GetBoneImgCoordinates(nite::JointType skelJoint, nite::Point3f &vec, float minConfidence)
{
  bool ret = mDataSkeleton->GetBoneImgCoordinates(skelJoint, vec, minConfidence);
  if (skelJoint == nite::JOINT_LEFT_HAND)
    vec = mleftHand->GetHandPos();
  else if (skelJoint == nite::JOINT_RIGHT_HAND)
    vec = mrightHand->GetHandPos();
  
  return ret;
}

bool KinectUser::GetBoneCurrentPosition(nite::JointType skelJoint, nite::Point3f &position, float minConfidence)
{
  return mDataSkeleton->GetBoneCurrentPosition(skelJoint, position, minConfidence);
}

bool KinectUser::GetBoneCurrentOrientation(nite::JointType skelJoint, Quaternion* mQuat, float minConfidence)
{
  return mDataSkeleton->GetBoneCurrentOrientation(skelJoint, mQuat, minConfidence);
}

bool KinectUser::GetFingersPixelPosition(nite::JointType hand, vector<cv::Point> &vec)
{
  if ((hand == nite::JOINT_LEFT_HAND) && (mleftHand->IsVisible()))
  {
    vec = mleftHand->GetFingersPos();
    return true;
  }
  else if ((hand == nite::JOINT_RIGHT_HAND) && (mrightHand->IsVisible()))
  {
    vec = mrightHand->GetFingersPos();
    return true;
  }

  return false;
}

bool KinectUser::IsHandVisible(nite::JointType skelJoint)
{
  if (skelJoint == nite::JOINT_RIGHT_HAND)
  {
    return mrightHand->IsVisible();
  }
  else if (skelJoint == nite::JOINT_LEFT_HAND)
  {
    return mleftHand->IsVisible();
  }

  return false;
}