#include "stdafx.h"

#include "SLJoints.h"



// SL/OpenSim Joints
static  std::string _SLJointName[] =	// SL_MAX_JOINT_NUM
{
	"mPelvis", "mTorso", "mChest", "mNeck", "mHead",
	"mCollarLeft",  "mShoulderLeft",  "mElbowLeft",  "mWristLeft",  "L_Hand",
	"mCollarRight", "mShoulderRight", "mElbowRight", "mWristRight", "R_Hand",
	"mHipLeft",  "mKneeLeft",  "mAnkleLeft",  "mFootLeft",  "mToeLeft",
	"mHipRight", "mKneeRight", "mAnkleRight", "mFootRight", "mToeRight"
};



std::string  SLJointName(int n)
{
	std::string str = "";

	if (n>=0 && n<SL_MAX_JOINT_NUM) {
		str = _SLJointName[n];
	}

	return str; 
}



/*
mPelvis(0), mTorso(1), mChest(2), mNeck(3), mHead(4),
mCollarLeft(5),   mShoulderLeft(6),   mElbowLeft(7),   mWristLeft(8),   L_Hand(9),
mCollarRight(10), mShoulderRight(11), mElbowRight(12), mWristRight(13), R_Hand(14),
mHipLeft(15),  mKneeLeft(16),  mAnkleLeft(17),  mFootLeft(18),  mToeLeft(19),
mHipRight(20), mKneeRight(21), mAnkleRight(22), mFootRight(23), mToeRight(24)
*/




////////////////////////////////////////////////////////////////////////////////////////
//
// OpenNI
//

/*                        位置  回転  
 0: PELVIS					×  ×  => <mPelvis(0)>
 1: XN_SKEL_HEAD	        ○  ×  => [mHead(4)]
 2: XN_SKEL_NECK			○  ○  => mNeck(3)                        1
 3: XN_SKEL_TORSO			○  ○  => mTrso(1)                        |
 4: XN_SKEL_WAIST			×  ×  => none              9 -  7 -  6 - 2 - 12 - 13 - 15
 5: XN_SKEL_LEFT_COLLAR		×  ×  => none                            |
 6: XN_SKEL_LEFT_SHOULDER	○  ○  => mShoulderRight(11)              3
 7: XN_SKEL_LEFT_ELBOW		○  ○  => mElbowRight(12)                 |
 8: XN_SKEL_LEFT_WRIST		×  ×  => none                            0    
 9: XN_SKEL_LEFT_HAND		○  ×  => [mWristRight(13)]           17      22
10: XN_SKEL_LEFT_FINGERTIP	×  ×  => none                        |       |
11: XN_SKEL_RIGHT_COLLAR	×  ×  => none                        18      22
12: XN_SKEL_RIGHT_SHOULDER	○  ○  => mShoulderLeft(6)            |       |
13: XN_SKEL_RIGHT_ELBOW		○  ○  => mElbowLeft(7)               20      24
14: XN_SKEL_RIGHT_WRIST		×  ×  => none
15: XN_SKEL_RIGHT_HAND      ○  ×  => [mWristLeft(8)]
16: XN_SKEL_RIGHT_FINGERTIP ×  ×  => none
17: XN_SKEL_LEFT_HIP		○  ○  => mHipRight(20)
18: XN_SKEL_LEFT_KNEE		○  ○  => mKneeRight(21)
19: XN_SKEL_LEFT_ANKLE		×  ×  => none 
20: XN_SKEL_LEFT_FOOT		○  ×  => [mAnkleLeft(23)]
21: XN_SKEL_RIGHT_HIP		○  ○  => mHipLeft(15)
22: XN_SKEL_RIGHT_KNEE		○  ○  => mKneeLeft(16)
23: XN_SKEL_RIGHT_ANKLE		×  ×  => none
24: XN_SKEL_RIGHT_FOOT		○  ×  => [mAnkleLeft(17)]
*/
static  int  _OpenNI2SLPosJoint[] =		// OPENNI_MAX_JOINT_NUM
{
	 0,  4,  3,  1, -1, 
	-1, 11, 12, -1, 13, -1, 
	-1,  6,  7, -1,  8, -1,
	20, 21, -1, 23,
	15, 16, -1, 17 
};

static  int  _OpenNI2SLRotJoint[] =		// OPENNI_MAX_JOINT_NUM
{
	 0, -1,  3,  1, -1, 
	-1, 11, 12, -1, -1, -1, 
	-1,  6,  7, -1, -1, -1,
	20, 21, -1, -1,
	15, 16, -1, -1 
};


int	 OpenNI2SLPosJointNum(int n)
{
	return _OpenNI2SLPosJoint[n];
}


int	 OpenNI2SLRotJointNum(int n)
{
	return _OpenNI2SLRotJoint[n];
}




////////////////////////////////////////////////////////////////////////////////////////
//
// Kinect SDK
//

/*
 0: NUI_SKELETON_POSITION_HIP_CENTER      => mPelvis(0)
 1: NUI_SKELETON_POSITION_SPINE           => <mTorso(1)>
 2: NUI_SKELETON_POSITION_SHOULDER_CENTER => mNeck(3)
 3: NUI_SKELETON_POSITION_HEAD            => [mHead(4)]                           3
 4: NUI_SKELETON_POSITION_SHOULDER_LEFT   => mShoulderLeft(6)                     |
 5: NUI_SKELETON_POSITION_ELBOW_LEFT      => mElbowLeft(7)      11 - 10 - 9 - 8 - 2 - 4 - 5 - 6 - 7
 6: NUI_SKELETON_POSITION_WRIST_LEFT      => mWristLeft(8)                        |
 7: NUI_SKELETON_POSITION_HAND_LEFT       => [L_Hand(9)]                          1
 8: NUI_SKELETON_POSITION_SHOULDER_RIGHT  => mShoulderRight(11)                   |
 9: NUI_SKELETON_POSITION_ELBOW_RIGHT     => mElbowRight(12)                 16   0   12
10: NUI_SKELETON_POSITION_WRIST_RIGHT     => mWristRight(13)                 |        |
11: NUI_SKELETON_POSITION_HAND_RIGHT      => [R_Hand(14)]                    17       13
12: NUI_SKELETON_POSITION_HIP_LEFT        => mHipLeft(15)                    |        |
13: NUI_SKELETON_POSITION_KNEE_LEFT       => mKneeLeft(16)                   18       14
14: NUI_SKELETON_POSITION_ANKLE_LEFT      => mAnkleLeft(17)                  |        |
15: NUI_SKELETON_POSITION_FOOT_LEFT       => [mFootLeft(18)]                 19       15
16: NUI_SKELETON_POSITION_HIP_RIGHT       => mHipRight(20)
17: NUI_SKELETON_POSITION_KNEE_RIGHT      => mKneeRight(21) 
18: NUI_SKELETON_POSITION_ANKLE_RIGHT     => mAnkleRight(22) 
19: NUI_SKELETON_POSITION_FOOT_RIGHT      => [mFootRight(23)] 
*/
static  int  _Kinect2SLPosJoint[] =		// KINECT_MAX_JOINT_NUM
{
	 0,  1,  3,  4, 
	 6,  7,  8,  9,
	11, 12, 13, 14,
	15, 16, 17, 18, 
	20, 21, 22, 23
};

static  int  _Kinect2SLRotJoint[] =		// KINECT_MAX_JOINT_NUM
{
	 0,  1,  3, -1, 
	 6,  7,  8, -1,
	11, 12, 13, -1,
	15, 16, 17, -1, 
	20, 21, 22, -1
};


int	 Kinect2SLRotJointNum(int n)
{
	return _Kinect2SLRotJoint[n];
}


int	 Kinect2SLPosJointNum(int n)
{
	return _Kinect2SLPosJoint[n];
}




////////////////////////////////////////////////////////////////////////////////////////
//
// 共通ジョイント番号
//

/*
#define	  NI_CMN_PELVIS		 0
#define	  NI_CMN_TORSO		 1
#define	  NI_CMN_NECK		 2
#define	  NI_CMN_HEAD		 3
#define	  NI_CMN_L_EAR		 4
#define	  NI_CMN_R_EAR		 5
	
#define   NI_CMN_L_SHLDR	 6
#define   NI_CMN_L_ELBOW	 7
#define	  NI_CMN_L_WRIST	 8
#define	  NI_CMN_L_HAND		 9
	
#define	  NI_CMN_R_SHLDR	10
#define	  NI_CMN_R_ELBOW	11
#define	  NI_CMN_R_WRIST	12
#define	  NI_CMN_R_HAND		13

#define	  NI_CMN_L_HIP		14
#define   NI_CMN_L_KNEE		15
#define	  NI_CMN_L_ANKLE	16
#define	  NI_CMN_L_FOOT		17
	
#define	  NI_CMN_R_HIP		18
#define	  NI_CMN_R_KNEE		19
#define	  NI_CMN_R_ANKLE	20
#define	  NI_CMN_R_FOOT		21
*/

static  int  _Ni2SLPosJoint[] =		// NI_JOINT_NUM
{
	 0,  1,  3,  4, -1, -1,
	 6,  7,  8,  9,
	11, 12, 13, 14,
	15, 16, 17, 18, 
	20, 21, 22, 23
};

static  int  _Ni2SLRotJoint[] =		// NI_JOINT_NUM
{
	 0,  1,  3,  4, -1, -1,
	 6,  7,  8, -1,
	11, 12, 13, -1,
	15, 16, 17, -1, 
	20, 21, 22, -1
};


int	 Ni2SLPosJointNum(int n)
{
	return _Ni2SLRotJoint[n];
}


int	 Ni2SLRotJointNum(int n)
{
	return _Ni2SLRotJoint[n];
}






////////////////////////////////////////////////////////////////////////////////////////
//
// Wrapper
//

//
// Library 固有のジョイント番号から SL/OSの座標用ジョイント番号を得る
//
int  NiSDK2SLPosJointNum(int n, NiSDK_Lib lib)
{
	if (lib==NiSDK_OpenNI) {
		return _OpenNI2SLPosJoint[n];
	}
	else if (lib==NiSDK_Kinect) {
		return _Kinect2SLPosJoint[n];
	}
	else if (lib==NiSDK_None) {
		return _Ni2SLPosJoint[n];
	}

	return -1;
}



//
// Library 固有のジョイント番号から SL/OSの回転用ジョイント番号を得る
//
int  NiSDK2SLRotJointNum(int n, NiSDK_Lib lib)
{
	if (lib==NiSDK_OpenNI) {
		return _OpenNI2SLRotJoint[n];
	}
	else if (lib==NiSDK_Kinect) {
		return _Kinect2SLRotJoint[n];
	}
	else if (lib==NiSDK_None) {
		return _Ni2SLRotJoint[n];
	}

	return -1;
}



//
// Library 固有のジョイント番号から SL/OSの座標用ジョイント名を得る
//
std::string  NiSDK2SLPosJointName(int n, NiSDK_Lib lib)
{
	std::string  str = "";
	int j = -1;

	if (lib==NiSDK_OpenNI) {
		j = _OpenNI2SLPosJoint[n];
	}
	else if (lib==NiSDK_Kinect) {
		j = _Kinect2SLPosJoint[n];
	}
	else if (lib==NiSDK_None) {
		j = _Ni2SLPosJoint[n];
	}

	if (j>=0) str = _SLJointName[j];

	return str;
}



//
// Library 固有のジョイント番号から SL/OSの回転用ジョイント名を得る
//
std::string  NiSDK2SLRotJointName(int n, NiSDK_Lib lib)
{
	std::string  str = "";
	int j = -1;

	if (lib==NiSDK_OpenNI) {
		j = _OpenNI2SLRotJoint[n];
	}
	else if (lib==NiSDK_Kinect) {
		j = _Kinect2SLRotJoint[n];
	}
	else if (lib==NiSDK_None) {
		j = _Ni2SLRotJoint[n];
	}

	if (j>=0) str = _SLJointName[j];

	return str;
}



//
// 共通ジョイント番号から SL/OSの座標用ジョイント名を得る
//
std::string  Ni2SLPosJointName(int n)
{
	std::string  str = "";

	int j = _Ni2SLPosJoint[n];
	if (j>=0) str = _SLJointName[j];

	return str;
}



//
// 共通ジョイント番号から SL/OSの回転用ジョイント名を得る
//
std::string  Ni2SLRotJointName(int n)
{
	std::string  str = "";

	int j = _Ni2SLRotJoint[n];
	if (j>=0) str = _SLJointName[j];

	return str;
}