#include  "xtools++.h"
#include  "NiJointsTool.h"




using namespace jbxl;
using namespace jbxwl;





//////////////////////////////////////////////////////////////////////////
//
// SDK(OpenNI, Kinect SDK)用 作業ジョイント番号．
// 二つのSDKのジョイント名はこの名前に集約される．
//

// Default or No SDK
int	 NI_SDK_PELVIS	  = NI_PELVIS;
int	 NI_SDK_TORSO	  = NI_TORSO;
int	 NI_SDK_NECK	  = NI_NECK;
int	 NI_SDK_HEAD	  = NI_HEAD;

int	 NI_SDK_L_EYE	  = NI_L_EYE;
int	 NI_SDK_R_EYE	  = NI_R_EYE;
int	 NI_SDK_L_BUST	  = NI_L_BUST;
int	 NI_SDK_R_BUST	  = NI_R_BUST;
	
int  NI_SDK_L_SHLDR	  = NI_L_SHLDR;
int  NI_SDK_L_ELBOW	  = NI_L_ELBOW;
int	 NI_SDK_L_WRIST	  = NI_L_WRIST;
int	 NI_SDK_L_HAND	  = NI_L_HAND;
int	 NI_SDK_L_FNGRTIP = NI_L_FNGRTIP;

int	 NI_SDK_R_SHLDR	  = NI_R_SHLDR;
int	 NI_SDK_R_ELBOW	  = NI_R_ELBOW;
int	 NI_SDK_R_WRIST	  = NI_R_WRIST;
int	 NI_SDK_R_HAND	  = NI_R_HAND;
int	 NI_SDK_R_FNGRTIP = NI_R_FNGRTIP;

int	 NI_SDK_L_HIP	  = NI_L_HIP;
int  NI_SDK_L_KNEE	  = NI_L_KNEE;
int	 NI_SDK_L_ANKLE	  = NI_L_ANKLE;
int	 NI_SDK_L_FOOT	  = NI_L_FOOT;
	
int	 NI_SDK_R_HIP	  = NI_R_HIP;
int	 NI_SDK_R_KNEE	  = NI_R_KNEE;
int	 NI_SDK_R_ANKLE	  = NI_R_ANKLE;
int	 NI_SDK_R_FOOT	  = NI_R_FOOT;

int  NI_SDK_L_FINGER  = NI_L_FINGER;	// Parameter of Finger (Dummy Joint)
int  NI_SDK_R_FINGER  = NI_R_FINGER;	// Parameter of Finger (Dummy Joint)
int  NI_SDK_FACE	  = NI_FACE;		// Parameter of Expression (Dummy Joint)




/////////////////////////////////////////////////////////////////////////////
//
//

static std::string  _NiJointName[] =		// NI_JOINT_NAME_NUM
{
	// 0         1        2        3        4       5       6        
	"PELVIS", "WAIST", "TORSO", "CHEST", "NECK", "HEAD", "SKULL", 
	//  7        8        9        10
	"L_EYE", "R_EYE", "L_BUST", "R_BUST",
	// 11           12         13        14         15        16
	"L_COLLAR", "L_SHLDR", "L_ELBOW", "L_WRIST", "L_HAND", "L_FNGRTIP",
	// 17           18         19        20         21        22
	"R_COLLAR", "R_SHLDR", "R_ELBOW", "R_WRIST", "R_HAND", "R_FNGRTIP",
	// 23       24        25         26        27
	"L_HIP", "L_KNEE", "L_ANKLE", "L_FOOT", "L_TOE",
	// 28       29        30         31        32
	"R_HIP", "R_KNEE", "R_ANKLE", "R_FOOT", "R_TOE",
	// 33          34         35
	"L_FINGER", "R_FINGER", "FACE"
};




////////////////////////////////////////////////////////////////////////////////////////////
//
// 共通のジョイントの名前と番号
//

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

	if (n>=0 && n<NI_JOINT_NAME_NUM) {
		str = _NiJointName[n];
	}

	return str; 
}


int   jbxwl::NiJointNum(char* name)
{
	for (int i=0; i<NI_JOINT_NAME_NUM; i++) {
		if (!strcmp(name, _NiJointName[i].c_str())) return i;
	}
	return -1;
}




////////////////////////////////////////////////////////////////////////////////////////////
//
// Library 固有のジョイント番号から共通のジョイント名を得る
//

static  int  _OpenNI2JointNameNum[] =	// NI_OPENNI_JOINT_NUM (25)
{
	 0,  5,  4,  2,  1,
	17, 18, 19, 20, 21, 22,
	11, 12, 13, 14, 15, 16,
	28, 29, 30, 31,
	23, 24, 25, 26
};

static  int  _Kinect2JointNameNum[] =	// NI_KINECT_JOINT_NUM (20)
{
	 0,  2,  4,  5, 
	12, 13, 14, 15,
	18, 19, 20, 21,
	23, 24, 25, 26,
	28, 29, 30, 31 
};

/*
static  int  _Ni2JointNameNum[] =		// NI_JOINT_NUM (34)
{
	 0,  1,  2,  3,  4,  5,  6,
	 7,  8,  9, 10,
	11, 12, 13, 14, 15, 16,
	17, 18, 19, 20, 21, 22, 
	23, 24, 25, 26, 27,
	28, 29, 30, 31, 32, 
	33, 34, 35
};
*/


std::string  jbxwl::NiSDK2JointName(int j, NiSDK_Lib lib)
{
	std::string str = "";

	if (lib==NiSDK_OpenNI) {
		int n = _OpenNI2JointNameNum[j];
		if (n>=0) str = _NiJointName[n];
	}
	else if (lib==NiSDK_Kinect) {
		int n = _Kinect2JointNameNum[j];
		if (n>=0) str = _NiJointName[n];
	}
	else if (lib==NiSDK_None) {
		//str = _NiJointName[_Ni2JointNameNum[j]];
		str = _NiJointName[j];
	}

	return str;
}




////////////////////////////////////////////////////////////////////////////////////////////
//
// Library 固有のジョイント番号からミラージョイントの番号を得る
//

static  int  _OpenNIMirrorJointNum[] =	// NI_OPENNI_JOINT_NUM (25)
{
	 0,  1,  2,  3,  4,
	11, 12, 13, 14, 15, 16,
	 5,  6,  7,  8,  9, 10,
	21, 22, 23, 24,
	17, 18, 19, 20
};

static  int  _KinectMirrorJointNum[] =	// NI_KINECT_JOINT_NUM (20)
{
	 0,  1,  2,  3, 
	 8,  9, 10, 11,
	 4,  5,  6,  7,
	16, 17, 18, 19,
	12, 13, 14, 15
};

static  int  _NiMirrorJointNum[] =		// NI_JOINT_NUM (36)
{
	 0,  1,  2,  3,  4,  5,  6,
	 8,  7, 10,  9, 
	17, 18, 19, 20, 21, 22, 
	11, 12, 13, 14, 15, 16,
	28, 29, 30, 31, 32, 
	23, 24, 25, 26, 27,
	34, 33, 35 
};


int  jbxwl::NiSDKMirrorJointNum(int j, NiSDK_Lib lib)
{
	int ret = -1;
	if (j<0) return ret;

	if (lib==NiSDK_OpenNI) {
		ret = _OpenNIMirrorJointNum[j];
	}
	else if (lib==NiSDK_Kinect) {
		ret = _KinectMirrorJointNum[j];
	}
	else if (lib==NiSDK_None) {
		ret = _NiMirrorJointNum[j];
	}

	return ret;
}





////////////////////////////////////////////////////////////////////////////////////////////
//
// 共通ジョイント番号から，Library固有の座標用ジョイント番号を得る
//

static  int  _Ni2OpenNIPosJointNum[] =		// NI_JOINT_NUM (36)
{
	 0, -1,  3, -1,  2,  1, -1, 
	-1, -1, -1, -1,
	-1, 12, 13, -1, 15, -1,
	-1,  6,  7, -1,  9, -1,
	21, 22, -1, 24, -1,
	17, 18, -1, 20, -1,
	-1, -1, -1
};

static  int  _Ni2KinectPosJointNum[] =		// NI_JOINT_NUM (36)
{
	 0, -1,  1, -1,  2,  3, -1, 
	-1, -1, -1, -1,
	-1,  4,  5,  6,  7, -1,
	-1,  8,  9, 10, 11, -1,
	12, 13, 14, 15, -1,
	16, 17, 18, 19, -1,
	-1, -1, -1
};


int  jbxwl::Ni2SDKPosJointNum(int j, NiSDK_Lib lib)
{
	int ret = -1;

	if (lib==NiSDK_Kinect) {
		ret = _Ni2KinectPosJointNum[j];
	}
	else if (lib==NiSDK_OpenNI) {
		ret = _Ni2OpenNIPosJointNum[j];
	}
	else if (lib==NiSDK_None) {
		ret = j;
	}

	return ret;
}




////////////////////////////////////////////////////////////////////////////////////////////
//
// 共通ジョイント番号から，Library固有の回転用ジョイント番号を得る
//

static  int  _Ni2OpenNIRotJointNum[] =		// NI_JOINT_NUM (36)
{
	 0, -1,  3, -1,  2, -1, -1,
	-1, -1, -1, -1,
	-1, 12, 13, -1, -1, -1,
	-1,  6,  7, -1, -1, -1,
	21, 22, -1, -1, -1,
	17, 18, -1, -1, -1,
	-1, -1, -1
};

static  int  _Ni2KinectRotJointNum[] =		// NI_JOINT_NUM (36)
{
	 0, -1,  1, -1,  2, -1, -1,
	-1, -1, -1, -1,
	-1,  4,  5,  6, -1, -1,
	-1,  8,  9, 10, -1, -1,
	12, 13, 14, -1, -1,
	16, 17, 18, -1, -1,
	-1, -1, -1
};

static  int  _Ni2RotJointNum[] =			// NI_JOINT_NUM (36)
{
	 0,  1,  2,  3,  4,  5, -1,  
	 7,  8,  9, 10, 
	11, 12, 13, 14, 15, -1, 
	17, 18, 19, 20, 21, -1, 
	23, 24, 25, 26, -1,
	28, 29, 30, 31, -1, 
	33, 34, 35

};



int  jbxwl::Ni2SDKRotJointNum(int j, NiSDK_Lib lib)
{
	int ret = -1;

	if (lib==NiSDK_Kinect) {
		ret = _Ni2KinectRotJointNum[j];
	}
	else if (lib==NiSDK_OpenNI) {
		ret = _Ni2OpenNIRotJointNum[j];
	}
	else if (lib==NiSDK_None) {
		ret = _Ni2RotJointNum[j];
	}

	return ret;
}




////////////////////////////////////////////////////////////////////////////////////////////
//
// 共通のジョイント名から Library 固有のジョイント番号を得る
//

/*
static  int  _JointNameNum2Ni[] =		// NI_JOINT_NAME_NUM (36)
{
	 0,  1,  2,  3,  4,  5,  6,
	 7,  8,  9, 10,
	11, 12, 13, 14, 15, 16,
	17, 18, 19, 20, 21, 22, 
	23, 24, 25, 26, 27,
	28, 29, 30, 31, 32, 
	33, 34, 35
};
*/

int   jbxwl::JointName2NiSDK(char* jname, NiSDK_Lib lib)
{
	int ret = -1;

	int j;
	for (j=0; j<NI_JOINT_NAME_NUM; j++) {
		if (!strcasecmp(jname, (char*)_NiJointName[j].c_str())) break;
	}

	if (j<NI_JOINT_NAME_NUM) {
		ret = Ni2SDKJointNum(j, lib);
	}
	else if (j==NI_JOINT_NAME_NUM) {
		//DEBUG_ERR("JointName2NiSDK: Unknown Joint Name = %s", jname);
	}

	return ret;
}






///////////////////////////////////////////////////////////////////////////
//
//
//

// OpenNI
void  jbxwl::NiSetOpenNIJointNums(void)
{
	NI_SDK_PELVIS	 =  0;
	NI_SDK_TORSO	 =  3;	// XN_SKEL_TORSO;
	NI_SDK_NECK		 =  2;	// XN_SKEL_NECK;
	NI_SDK_HEAD		 =  1;	// XN_SKEL_HEAD;

	NI_SDK_L_EYE	 = -1;
	NI_SDK_R_EYE	 = -1;
	NI_SDK_L_BUST	 = -1;
	NI_SDK_R_BUST	 = -1;
	
	NI_SDK_L_SHLDR	 = 12;	// XN_SKEL_RIGHT_SHOULDER;
	NI_SDK_L_ELBOW	 = 13;	// XN_SKEL_RIGHT_ELBOW;
	NI_SDK_L_WRIST	 = -1;
	NI_SDK_L_HAND	 = 15;	// XN_SKEL_RIGHT_HAND;

	NI_SDK_R_SHLDR	 =  6;	// XN_SKEL_LEFT_SHOULDER;
	NI_SDK_R_ELBOW	 =  7;	// XN_SKEL_LEFT_ELBOW;
	NI_SDK_R_WRIST	 = -1;
	NI_SDK_R_HAND	 =  9;	// XN_SKEL_LEFT_HAND;
	NI_SDK_R_FNGRTIP = -1;
	
	NI_SDK_L_HIP	 = 21;	// XN_SKEL_RIGHT_HIP;
	NI_SDK_L_KNEE	 = 22;	// XN_SKEL_RIGHT_KNEE;
	NI_SDK_L_ANKLE	 = -1;
	NI_SDK_L_FOOT	 = 24;	// XN_SKEL_RIGHT_FOOT;
	NI_SDK_L_FNGRTIP = -1;

	NI_SDK_R_HIP	 = 17;	// XN_SKEL_LEFT_HIP;
	NI_SDK_R_KNEE	 = 18;	// XN_SKEL_LEFT_KNEE;
	NI_SDK_R_ANKLE	 = -1;
	NI_SDK_R_FOOT	 = 20;	// XN_SKEL_LEFT_FOOT;

	NI_SDK_L_FINGER	 = -1;
	NI_SDK_R_FINGER	 = -1;
	NI_SDK_FACE		 = -1;

	return;
}



// Kinect SDK
void  jbxwl::NiSetKinectJointNums(void)
{
	NI_SDK_PELVIS	 =  0;	// NUI_SKELETON_POSITION_HIP_CENTER
	NI_SDK_TORSO	 =  1;	// SPINE は書き換えて使用 
	NI_SDK_NECK		 =  2;	// NUI_SKELETON_POSITION_SHOULDER_CENTER
	NI_SDK_HEAD		 =  3;	// NUI_SKELETON_POSITION_HEAD

	NI_SDK_L_EYE	 = -1;
	NI_SDK_R_EYE	 = -1;
	NI_SDK_L_BUST	 = -1;
	NI_SDK_R_BUST	 = -1;
		
	NI_SDK_L_SHLDR	 =  4;	// NUI_SKELETON_POSITION_SHOULDER_LEFT
	NI_SDK_L_ELBOW	 =  5;	// NUI_SKELETON_POSITION_ELBOW_LEFT
	NI_SDK_L_WRIST	 =  6;	// NUI_SKELETON_POSITION_WRIST_LEFT
	NI_SDK_L_HAND	 =  7;	// NUI_SKELETON_POSITION_HAND_LEFT
	NI_SDK_L_FNGRTIP = -1;
	
	NI_SDK_R_SHLDR	 =  8;	// NUI_SKELETON_POSITION_SHOULDER_RIGHT
	NI_SDK_R_ELBOW	 =  9;	// NUI_SKELETON_POSITION_ELBOW_RIGHT
	NI_SDK_R_WRIST	 = 10;	// NUI_SKELETON_POSITION_WRIST_RIGHT
	NI_SDK_R_HAND	 = 11;	// NUI_SKELETON_POSITION_HAND_RIGHT
	NI_SDK_R_FNGRTIP = -1;

	NI_SDK_L_HIP	 = 12;	// NUI_SKELETON_POSITION_HIP_LEFT
	NI_SDK_L_KNEE	 = 13;	// NUI_SKELETON_POSITION_KNEE_LEFT
	NI_SDK_L_ANKLE	 = 14;	// NUI_SKELETON_POSITION_ANKLE_LEFT
	NI_SDK_L_FOOT	 = 15;	// NUI_SKELETON_POSITION_FOOT_LEFT
	
	NI_SDK_R_HIP	 = 16;	// NUI_SKELETON_POSITION_HIP_RIGHT
	NI_SDK_R_KNEE	 = 17;	// NUI_SKELETON_POSITION_KNEE_RIGHT
	NI_SDK_R_ANKLE	 = 18;	// NUI_SKELETON_POSITION_ANKLE_RIGHT
	NI_SDK_R_FOOT	 = 19;	// NUI_SKELETON_POSITION_FOOT_RIGHT

	NI_SDK_L_FINGER	 = -1;
	NI_SDK_R_FINGER	 = -1;
	NI_SDK_FACE		 = -1;

	return;
}






//////////////////////////////////////////////////////////////////////////////////////////////////
//
// Frame Data
//

/**
NiFrameData*  jbxwl::makeVarFrameData(int frame, int* joint_num, int* frame_num)

フレームデータの格納先メモリ（フレーム毎のジョイント数とフレーム間隔が可変）を作成，初期化．

@param frame     フレームの数
@param joint_num ジョイント数
@param frame_num フレームの番号（時間）の入った配列．NULLならフレーム間隔一定．
@return フレームデータを格納するデータ領域へのポインタ
*/
NiFrameData*  jbxwl::makeFramesData(int frame, int jnum, int* frame_num)
{
	if (jnum<0) return NULL;	// 0 is OK for dummy

	NiFrameData* frmdata = (NiFrameData*)malloc(frame*sizeof(NiFrameData));
	if (frmdata==NULL) return NULL;
	memset(frmdata, 0, frame*sizeof(NiFrameData));

	for (int i=0; i<frame; i++) {
		if (frame_num!=NULL) {
			frmdata[i].frmn = frame_num[i];
			frmdata[i].msec = frame_num[i];		// デフォルトはフレーム間 1ms
		}
		else {
			frmdata[i].frmn = i;
			frmdata[i].msec = i;
		}
		frmdata[i].jnum = jnum;

		if (frmdata[i].jnum>0) {
			frmdata[i].jdat = (NiJointData*)malloc(frmdata[i].jnum*sizeof(NiJointData));
			if (frmdata[i].jdat==NULL) {
				freeFramesData(frmdata, frame);
				return NULL;
			}
			//
			for (int j=0; j<frmdata[i].jnum; j++) {
				frmdata[i].jdat[j].joint = -1;
				frmdata[i].jdat[j].index = -1;
				frmdata[i].jdat[j].vect.init(-1.0);
				frmdata[i].jdat[j].quat.init(-1.0);
			}
		}
		else {
			frmdata[i].jdat = NULL;		// for dummy
		}
	}

	return frmdata;
}



/**
NiFrameData*  jbxwl::makeFramesData(int frame, int jnum)

フレームデータの格納先メモリ（ジョイント数固定）を作成，初期化．@n
フレーム毎のジョイント数，フレーム間隔が一定

@param frame     フレームの数
@param joint_num ジョイントの数
@return フレームデータを格納するデータ領域へのポインタ
*/
/*
NiFrameData*  jbxwl::makeFramesData(int frame, int jnum)
{
	if (jnum<0) return NULL;

	int* joint_num = (int*)malloc(frame*sizeof(int));
	if (joint_num==NULL) return NULL;
	for (int i=0; i<frame; i++) joint_num[i] = jnum;

	NiFrameData* frmdata = makeVarFrameData(frame, joint_num, NULL);
	::free(joint_num);

	return frmdata;
}*/





void  jbxwl::freeFramesData(NiFrameData* frmdata, int frm_num)
{
	if (frmdata==NULL || frm_num<=0) return;

	for (int i=0; i<frm_num; i++) {
		if (frmdata[i].jdat!=NULL) {
			::free(frmdata[i].jdat);
			frmdata[i].jdat = NULL;
		}
	}

	::free(frmdata);
	return;
}







//////////////////////////////////////////////////////////////////////////////////////////////////
//
// 回転角の補正
//		see http://wiki.secondlife.com/wiki/Suggested_BVH_Joint_Rotation_Limits
//      回転の順序は？
//		色々検証の余地有り
//

void  jbxwl::NiCorrectJointsRotation(Quaternion* quat, NiSDK_Lib lib)
{
	int    joint;
	double d2r = PI/180.0;
	RBound<double> rb;

	joint = Ni2SDKRotJointNum(NI_TORSO, lib);	// abdomen
	if (joint>0) {
		// X:-30 to 30, Y:-45 to 68, Z:-45 to 45
		rb.set(-30.0, 30.0, -45.0, 68.0, -45.0, 45.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}

	joint = Ni2SDKRotJointNum(NI_CHEST, lib);
	if (joint>0) {
		// X:-30 to 30, Y:-45 to 22, Z:-45 to 45 
		rb.set(-30.0, 30.0, -45.0, 68.0, -45.0, 45.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}

	joint = Ni2SDKRotJointNum(NI_NECK, lib);
	if (joint>0) {
		// X:-60 to 60, Y:-74 to 44, Z:-90 to 90 
		rb.set(-60.0, 60.0, -74.0, 44.0, -90.0, 90.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}

/*	joint = Ni2SDKRotJointNum(NI_HEAD, lib);
	if (joint>0) {
		// X:-30 to 30, Y:-37 to 22, Z:-45 to 45 
		rb.set(-30.0, 30.0, -45.0, 68.0, -45.0, 45.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}*/
	
	joint = Ni2SDKRotJointNum(NI_L_COLLAR, lib);
	if (joint>0) {
		// X:-30 to 30, Y:-0 to 0, Z: -30 to 10
		rb.set(-30.0, 30.0, -180.0, 180.0, -30.0, 10.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}
	joint = Ni2SDKRotJointNum(NI_R_COLLAR, lib);
	if (joint>0) {
		// X: -30 to 30, Y:-0 to 0, Z:-10 to 30, 
		rb.set(-30.0, 30.0, -180.0, 180.0, -10.0, 30.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}

	joint = Ni2SDKRotJointNum(NI_L_SHLDR, lib);
	if (joint>0) {
		// X:-91 to 97, Y:-135 to 90, Z:-180 to 98
		rb.set(-120.0, 120.0, -150.0, 120.0, -180.0, 120.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}
	joint = Ni2SDKRotJointNum(NI_R_SHLDR, lib);
	if (joint>0) {
		// X:-97 to 91, Y:-135 to 90, Z:-98 to 180
		rb.set(-120.0, 120.0, -150.0, 120.0, -120.0, 180.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}

/*	joint = Ni2SDKRotJointNum(NI_L_ELBOW, lib);
	if (joint>0) {
		// X:-0 to 0, Y:-90 to 79, Z:-146 to 0
		rb.set(-180.0, 180.0, -120.0, 120.0, -160.0, 0.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}
	joint = Ni2SDKRotJointNum(NI_R_ELBOW, lib);
	if (joint>0) {
		// X:-0 to 0, Y:-90 to 79, Z:0 to 146
		rb.set(-180.0, 180.0, -120.0, 120.0, 0.0, 160.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}*/

	joint = Ni2SDKRotJointNum(NI_L_WRIST, lib);
	if (joint>0) {
		// X:-90 to 86, Y:-45 to 45, Z:-25 to 36
		rb.set(-90.0, 86.0, -45.0, 45.0, -25.0, 36.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}
	joint = Ni2SDKRotJointNum(NI_R_WRIST, lib);
	if (joint>0) {
		// X:-86 to 90, Y:-45 to 45, Z:-36 to 25
		rb.set(-86.0, 90.0, -45.0, 45.0, -36.0, 25.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}

/*	joint = Ni2SDKRotJointNum(NI_L_HIP, lib);
	if (joint>0) {
		// X:-17 to 88, Y:-155 to 45, Z:-85 to 105
		rb.set(-20.0, 100.0, -180.0, 60.0, -100.0, 120.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}
	joint = Ni2SDKRotJointNum(NI_R_HIP, lib);
	if (joint>0) {
		// X:-88 to 17, Y:-155 to 45, Z:-105 to 85
		rb.set(-100.0, 20.0, -180.0, 60.0, -120.0, 100.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}*/

	joint = Ni2SDKRotJointNum(NI_L_KNEE, lib);
	if (joint>0) {
		// X:-0 to 0, Y:0 to 150, Z:-0 to 0
		rb.set(-10.0, 10.0, 0.0, 170.0, -10.0, 10.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}
	joint = Ni2SDKRotJointNum(NI_R_KNEE, lib);
	if (joint>0) {
		// X:-0 to 0, Y:0 to 150, Z:-0 to 0
		rb.set(-10.0, 10.0, 0.0, 170.0, -10.0, 10.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}

	joint = Ni2SDKRotJointNum(NI_L_FOOT, lib);
	if (joint>0) {
		// X:-74 to 15, Y:-31 to 63, Z:-26 to 26
		rb.set(-74.0, 15.0, -31.0, 63.0, -26.0, 26.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}
	joint = Ni2SDKRotJointNum(NI_R_FOOT, lib);
	if (joint>0) {
		// X:-15 to 74, Y:-31 to 63, Z:-26 to 26
		rb.set(-15.0, 74.0, -31.0, 63.0, -26.0, 26.0);
		rb.multiple(d2r);
		NiCorrectEuler(quat+joint, rb);
	}

	return;
}










//////////////////////////////////////////////////////////////////////////////////////////////////
//
//  CBaseFrameTool Class
//

CBaseFrameTool::CBaseFrameTool(void)  
{ 
	init_data();
}




CBaseFrameTool::~CBaseFrameTool(void) 
{
	DEBUG_ERR("DESTRUCTOR: CBaseFrameTool");

	free_data();
}




void  CBaseFrameTool::free_data(void)
{
	if (framesData!=NULL) freeFramesData(framesData, frames_num);
	if (jointsData!=NULL) ::free(jointsData);

	if (posVect!=NULL) ::free(posVect);
	if (rotQuat!=NULL) ::free(rotQuat);

	framesData = NULL; 
	jointsData = NULL;

	posVect	   = NULL;
	rotQuat    = NULL;
	
	clear_data();
}




void  CBaseFrameTool::clear_data(void)
{
	frames_num = 0;
	joints_num = 0;

	start_time = 0;
	stop_time  = 0;
	exec_time  = 0;
}




void  CBaseFrameTool::init_data(void)
{
	framesData = NULL; 
	jointsData = NULL;

	posVect	   = NULL;
	rotQuat    = NULL;

	clear_data();
}





/////////////////////////////////////////////////////////////////


void  CBaseFrameTool::clearJointsData(int jnum)
{
	if (jointsData!=NULL) {
		for (int j=0; j<jnum; j++) {
			jointsData[j].index = -1;
			jointsData[j].joint = -1;
			jointsData[j].vect.init(-1.0);
			jointsData[j].quat.init(-1.0);
		}
	}
}





void  CBaseFrameTool::clearVectorData(int jnum)
{
	if (posVect!=NULL && rotQuat!=NULL) {
		for (int j=0; j<jnum; j++) {
			posVect[j].init(-1.0);
			rotQuat[j].init(-1.0);
		}
	}
}