G:/work/subversion/scol-technologies/trunk/plugins/OpenNiSCOL/OpenNiSCOL/core/DataSkeleton.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 "DataSkeleton.h"

#include "../lib/common.h"

#include "../generator/User.h"

#include "../objects/KinectDevice.h"
#include "../objects/KinectUser.h"

void FromRotationMatrix(XnSkeletonJointOrientation jointOri, Quaternion* mQuat)
{
  Matrix3 kRot = Matrix3(jointOri.orientation.elements[0],-jointOri.orientation.elements[1],jointOri.orientation.elements[2],
                                                                -jointOri.orientation.elements[3],jointOri.orientation.elements[4],-jointOri.orientation.elements[5],
                                                                jointOri.orientation.elements[6],-jointOri.orientation.elements[7],jointOri.orientation.elements[8]);

  // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
  // article "Quaternion Calculus and Fast Animation".

  float fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
  float fRoot;

  if ( fTrace > 0.0 )
  { 
      // |w| > 1/2, may as well choose w > 1/2
      fRoot = sqrt(fTrace + 1.0f);  // 2w
      mQuat->w = 0.5f*fRoot;
      fRoot = 0.5f/fRoot;  // 1/(4w)
      mQuat->x = (kRot[2][1]-kRot[1][2])*fRoot;
      mQuat->y = (kRot[0][2]-kRot[2][0])*fRoot;
      mQuat->z = (kRot[1][0]-kRot[0][1])*fRoot;
  }
  else
  {
      // |w| <= 1/2
      static size_t s_iNext[3] = { 1, 2, 0 };
      size_t i = 0;
      if ( kRot[1][1] > kRot[0][0] )
          i = 1;
      if ( kRot[2][2] > kRot[i][i] )
          i = 2;
      size_t j = s_iNext[i];
      size_t k = s_iNext[j];

      fRoot = sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0f);
      float* apkQuat[3] = { &mQuat->x, &mQuat->y, &mQuat->z };
      *apkQuat[i] = 0.5f*fRoot;
      fRoot = 0.5f/fRoot;
      mQuat->w = (kRot[k][j]-kRot[j][k])*fRoot;
      *apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;
      *apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;
  }
}

namespace NIDevice
{
  namespace Core
  {
    DataSkeleton::DataSkeleton(NIDevice::Objects::KinectUser* existingUser)
    {
      p_User = existingUser;
    }

    DataSkeleton::DataSkeleton()
    {
        p_User = 0;
    }

    DataSkeleton::~DataSkeleton()
    {
      p_User = 0;
    }

    bool DataSkeleton::GetBoneCurrentPosition(XnSkeletonJoint skelJoint, XnPoint3D &position, float minConfidence)
    {
      boost::mutex::scoped_lock lock(p_User->GetParentDevice()->m_mutex);

      XnSkeletonJointPosition posJoint;
      if (p_User->GetParentDevice()->GetUserGenerator() != 0)
      {
        if(p_User->GetParentDevice()->GetUserGenerator()->GetGenerator().GetSkeletonCap().IsTracking(p_User->GetId()) &&
           p_User->GetParentDevice()->GetUserGenerator()->GetGenerator().GetSkeletonCap().IsJointAvailable(skelJoint))
          //&& (p_User->GetParentDevice()->GetUserGenerator()->GetGenerator().GetSkeletonCap().IsJointActive(skelJoint, true)))
          p_User->GetParentDevice()->GetUserGenerator()->GetGenerator().GetSkeletonCap().GetSkeletonJointPosition(p_User->GetId(), skelJoint, posJoint);
        else
          return false;
      }
      else
        return false;

      if(posJoint.fConfidence < minConfidence)
      {
        return false;
      }
      position = posJoint.position;
      return true;
    }

    bool DataSkeleton::GetBoneCurrentOrientation(XnSkeletonJoint skelJoint, Quaternion* mQuat, float minConfidence)
    {
      boost::mutex::scoped_lock lock(p_User->GetParentDevice()->m_mutex);

      XnSkeletonJointOrientation orientationJoint;
      if (p_User->GetParentDevice()->GetUserGenerator() != 0)
      {
        if(p_User->GetParentDevice()->GetUserGenerator()->GetGenerator().GetSkeletonCap().IsTracking(p_User->GetId()) &&
           p_User->GetParentDevice()->GetUserGenerator()->GetGenerator().GetSkeletonCap().IsJointAvailable(skelJoint))
          p_User->GetParentDevice()->GetUserGenerator()->GetGenerator().GetSkeletonCap().GetSkeletonJointOrientation(p_User->GetId(), skelJoint, orientationJoint);
        else
          return false;
      }
      else
        return false;

      if(orientationJoint.fConfidence < minConfidence)
      {
        return false;
      }
      FromRotationMatrix(orientationJoint, mQuat);
      return true;
    }
  }
}