#include "StdAfx.h"


#ifndef DISABLE_KINECT_SDK


#pragma comment(lib, "Kinect10.lib")





#include  "ExKinectWin.h"





using namespace jbxwl;





/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// CExOpenNiWin ƒNƒ‰ƒX
//

CExKinectWin::CExKinectWin(void)
{
	niFileDev		= NULL;
	niExportData	= NULL;

	niJoints		= NULL;
	niNetwork		= NULL;
	sharedMem		= NULL;

	pLogFrame		= NULL;
	pSensorFrame	= NULL;
	pDepthFrame		= NULL;
	pModelFrame		= NULL;
	pLogDoc			= NULL;

	dataSaving		= FALSE;

	vect_fwrd.set(1.0, 0.0, 0.0, 1.0);

	appParam.init();
	clearJointsData();
}



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

	niNetwork		= NULL;
	sharedMem		= NULL;

	pLogFrame		= NULL;
	pSensorFrame	= NULL;
	pDepthFrame		= NULL;
	pModelFrame		= NULL;
	pLogDoc			= NULL;
}




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

void  CExKinectWin::setLogFramePtr(CExTextFrame* pfrm)
{
	pLogFrame = pfrm;
	
	if (pfrm!=NULL) pLogDoc = (CLogWndDoc*)(((CLogWndFrame*)pLogFrame)->pDoc);
	else            pLogDoc = NULL;
}



void  CExKinectWin::setParameter(CParameterSet param) 
{ 
	appParam  = param;

	m_use_knct_smth = param.useKnctSmooth;
	smoothParam.fSmoothing  = param.smoothKNCT;
	smoothParam.fCorrection = param.correction;
	m_profile = param.detectParts;
}





///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Virtual Function
//

void  CExKinectWin::saveJointsData(void)
{
	if (dataSaving && niFileDev!=NULL) {
		//
		if (niJoints!=NULL) {
			niFileDev->writeTempJText(niJoints->niPosVect, niJoints->niRotQuat, m_is_mirroring, NiSDK_None);
		}
		else {
			niFileDev->writeTempJText(posVect, rotQuat, m_is_mirroring);
		}
	}
}



void  CExKinectWin::logingJointsData(void)
{
	if (!isNull(pLogDoc)) {
		pLogDoc->lock();
		
		//
		for (int j=0; j<KINECT_JOINT_NUM; j++) {
			Vector4 pos = jointPositionData(j);
			if (appParam.printPosMode || appParam.printQutMode) {
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: %s (%2d)\n", NiSDK2JointName(j, NiSDK_Kinect).c_str(), j);
			}
			
			if (appParam.printPosMode) {
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: POS1: %f, %f, %f\n", pos.x, pos.y, pos.z);
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: POS2: %f, %f, %f\n", posVect[j].x, posVect[j].y, posVect[j].z);
			}
			if (appParam.printQutMode) {
				if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: QUAT: %f, %f, %f, %f\n", rotQuat[j].x, rotQuat[j].y, rotQuat[j].z, rotQuat[j].s);
			}
		}

		// Special Joints
		if (niJoints!=NULL) {
			for (int j=0; j<NI_SPECL_JOINT_NUM; j++) {
				int n = NI_NORML_JOINT_NUM + j;

				if (appParam.printPosMode || appParam.printQutMode) {
					if (pLogDoc!=NULL) pLogDoc->printFormat("LOCAL: %s (%2d)\n", NiSDK2JointName(n, NiSDK_None).c_str(), n);
				}
			
				if (appParam.printPosMode) {
					if (pLogDoc!=NULL) {
						Vector<double> vect = niJoints->niPosVect[n];
						pLogDoc->printFormat("LOCAL: POS : %f, %f, %f\n", vect.x, vect.y, vect.z);
					}
				}
				if (appParam.printQutMode) {
					if (pLogDoc!=NULL) {
						Quaternion quat = niJoints->niRotQuat[n];
						pLogDoc->printFormat("LOCAL: QUAT: %f, %f, %f, %f\n", quat.x, quat.y, quat.z, quat.s);
					}
				}
			}	
		}

		//
		if (!isNull(pLogDoc)) pLogDoc->unlock();
	}
}



////////////////////////////////////////////
//
// ƒWƒ‡ƒCƒ“ƒgƒf[ƒ^‚̏ˆ—‚Əo—Í
//
void  CExKinectWin::convertJointsData(void)
{
	// Face Tracking
	if (m_use_face) {
		FT_VECTOR3D  hint_pos[2];

		Vector4 pos_neck = device->jointPosData[NUI_SKELETON_POSITION_SHOULDER_CENTER];
		Vector4 pos_head = device->jointPosData[NUI_SKELETON_POSITION_HEAD];

		hint_pos[0].x = pos_neck.x;
		hint_pos[0].y = pos_neck.y;
		hint_pos[0].z = pos_neck.z;
		hint_pos[1].x = pos_head.x;
		hint_pos[1].y = pos_head.y;
		hint_pos[1].z = pos_head.z;
		if (!m_is_mirroring) {
			hint_pos[0].x = - hint_pos[0].x;
			hint_pos[1].x = - hint_pos[1].x;
		}
		//
		isDetectFace = device->face->detect(hint_pos);
	}


	// Convert Data
	if (appParam.detectParts==KINECT_SKEL_PROFILE_ALL) {
		convertPos2JointsData();
	}
	else if (appParam.detectParts==KINECT_SKEL_PROFILE_UPPER) {
		convertUpperPos2JointsData();
	}
	else {
		return;
	}

	// Additional Processing
	if (niJoints!=NULL) {
		niJoints->copyJoints2NiJoints();
		niJoints->niPosVect[NI_AVATAR] = currentPos;
		//
		//if (appParam.useFaceDetect) {
		//	niJoints->execFaceTracking();
		//}
	}

	//
	if (pSensorFrame==NULL) return;

	//
	// ƒf[ƒ^o—Í
	if (niExportData!=NULL) {
		if (niJoints!=NULL) {
			niExportData->exportJointsData(niJoints->niPosVect, niJoints->niRotQuat, niNetwork, NiSDK_None, NI_TOTAL_JOINT_NUM);
		}
		else {
			niExportData->exportJointsData(posVect, rotQuat, niNetwork, NiSDK_Kinect, KINECT_JOINT_NUM);
		}
	}
}





void  CExKinectWin::drawAddition(void)
{
	if (isDetectFace) {
		if (device->face!=NULL) {
			device->face->drawFaceRect(pViewData, m_image_scale, m_is_mirroring, BGRA2Int(255,255,255,0));
		}
	}

	/*
	if (niJoints!=NULL) {
		if (appParam.useFaceDetect) niJoints->drawFaceTracking(BGRA2Int(255,255,255,0));
	}*/
}





void   CExKinectWin::convertPos2JointsData(void)
{
	//
	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		if (posVect[j].c<m_confidence) {
			Vector<double>* ptr = (Vector<double>*)posRing[j].get(-1);
			if (ptr!=NULL && ptr->c>0.0) posVect[j] = *ptr;
			else posVect[j].init(-1.0);
		}
	}

	//
	if (appParam.useMvavSmooth) niJoints->PosMovingAverage(NiSDK_Kinect);
	if (appParam.useJointConst) niJoints->PosVibNoiseCanceler();

	//
	Vector<double> torso_down  =(posVect[NI_SDK_R_HIP] + posVect[NI_SDK_L_HIP])/2.0 - posVect[NI_SDK_PELVIS];
	Vector<double> shldr_left  = posVect[NI_SDK_L_SHLDR] - posVect[NI_SDK_R_SHLDR];
	Vector<double> torso_up	   = - torso_down;
	Vector<double> shldr_right = - shldr_left;

	rotQuat[NI_SDK_PELVIS].init();

	// PELVIS YZŽ˛‚̉ń“]
	if (posVect[NI_SDK_R_HIP].c>=m_confidence && posVect[NI_SDK_L_HIP].c>=m_confidence) { 
		Vector<double> vect = NewellMethod(posVect[NI_SDK_R_HIP], posVect[NI_SDK_L_HIP], posVect[NI_SDK_PELVIS]);
		//
		rotQuat[NI_SDK_PELVIS] = V2VQuaternion(vect_fwrd, vect); 
		//if (rotQuat[NI_SDK_PELVIS].c==-1.0 && rotQuat[NI_SDK_PELVIS].s==0.0 && rotQuat[NI_SDK_PELVIS].n==0.0) {
		//	rotQuat[NI_SDK_PELVIS].setRotation(PI, torso_up);
		//}
	}

	// X Ž˛‰ń“]
	Quaternion quat_lhip = ~rotQuat[NI_SDK_PELVIS]*posVect[NI_SDK_L_HIP]*rotQuat[NI_SDK_PELVIS];
	Quaternion quat_rhip = ~rotQuat[NI_SDK_PELVIS]*posVect[NI_SDK_R_HIP]*rotQuat[NI_SDK_PELVIS];

	Vector<double> hip_left  = quat_lhip.getVector() - quat_rhip.getVector(); 
	Vector<double> hip_right = - hip_left;
	double thx = atan2(hip_left.z, hip_left.y);
	Quaternion xrot;
	xrot.setRotation(thx, 1.0, 0.0, 0.0, 1.0);

	// YŽ˛‰ń“]•âł
	Quaternion yrot;
	yrot.setRotation((double)appParam.YaxisCorrect, 0.0, 1.0, 0.0, 1.0);

	//
	rotQuat[NI_SDK_PELVIS] = rotQuat[NI_SDK_PELVIS]*xrot*yrot;

	//
	rotQuat[NI_SDK_TORSO]  = VPPQuaternion(torso_up, posVect[NI_SDK_TORSO], posVect[NI_SDK_NECK]);
	rotQuat[NI_SDK_NECK]   = PPPQuaternion(posVect[NI_SDK_TORSO],  posVect[NI_SDK_NECK],  posVect[NI_SDK_HEAD]);

	// To Relative Coordinate
	rotQuat[NI_SDK_L_SHLDR] = VPPQuaternion(shldr_left,  posVect[NI_SDK_L_SHLDR], posVect[NI_SDK_L_ELBOW]);
	rotQuat[NI_SDK_R_SHLDR] = VPPQuaternion(shldr_right, posVect[NI_SDK_R_SHLDR], posVect[NI_SDK_R_ELBOW]);
	rotQuat[NI_SDK_L_HIP]   = VPPQuaternion(torso_down,  posVect[NI_SDK_L_HIP],   posVect[NI_SDK_L_KNEE]);
	rotQuat[NI_SDK_R_HIP]   = VPPQuaternion(torso_down,  posVect[NI_SDK_R_HIP],   posVect[NI_SDK_R_KNEE]);
//	rotQuat[NI_SDK_L_ANKLE] = VPPQuaternion(vect_ft_left,  posVect[NI_SDK_L_ANKLE], posVect[NI_SDK_L_FOOT]);
//	rotQuat[NI_SDK_R_ANKLE] = VPPQuaternion(vect_ft_right, posVect[NI_SDK_R_ANKLE], posVect[NI_SDK_R_FOOT]);
	//
	rotQuat[NI_SDK_L_ELBOW] = PPPQuaternion(posVect[NI_SDK_L_SHLDR], posVect[NI_SDK_L_ELBOW], posVect[NI_SDK_L_WRIST]);
	rotQuat[NI_SDK_R_ELBOW] = PPPQuaternion(posVect[NI_SDK_R_SHLDR], posVect[NI_SDK_R_ELBOW], posVect[NI_SDK_R_WRIST]);
	rotQuat[NI_SDK_L_WRIST] = PPPQuaternion(posVect[NI_SDK_L_ELBOW], posVect[NI_SDK_L_WRIST], posVect[NI_SDK_L_HAND]);
	rotQuat[NI_SDK_R_WRIST] = PPPQuaternion(posVect[NI_SDK_R_ELBOW], posVect[NI_SDK_R_WRIST], posVect[NI_SDK_R_HAND]);
	rotQuat[NI_SDK_L_KNEE]  = PPPQuaternion(posVect[NI_SDK_L_HIP],   posVect[NI_SDK_L_KNEE],  posVect[NI_SDK_L_ANKLE]);
	rotQuat[NI_SDK_R_KNEE]  = PPPQuaternion(posVect[NI_SDK_R_HIP],   posVect[NI_SDK_R_KNEE],  posVect[NI_SDK_R_ANKLE]);


	// To Avatar Coordinate
	rotQuat[NI_SDK_L_WRIST] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_L_WRIST]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_R_WRIST] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_R_WRIST]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_L_ELBOW] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_L_ELBOW]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_R_ELBOW] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_R_ELBOW]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_L_SHLDR] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_L_SHLDR]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_R_SHLDR] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_R_SHLDR]*rotQuat[NI_SDK_PELVIS];
//	rotQuat[NI_SDK_L_ANKLE] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_L_ANKLE]*rotQuat[NI_SDK_PELVIS];
//	rotQuat[NI_SDK_R_ANKLE] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_R_ANKLE]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_L_KNEE]  = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_L_KNEE] *rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_R_KNEE]  = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_R_KNEE] *rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_L_HIP]   = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_L_HIP]  *rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_R_HIP]   = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_R_HIP]  *rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_NECK]    = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_NECK]   *rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_TORSO]   = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_TORSO]  *rotQuat[NI_SDK_PELVIS];

	//
	// Face
	rotQuat[NI_SDK_HEAD].init(-1.0);
	if (isDetectFace) {
		rotQuat[NI_SDK_HEAD] = getFaceRotation();
		rotQuat[NI_SDK_HEAD] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_HEAD]*rotQuat[NI_SDK_PELVIS];
	}

	//
	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		if (rotQuat[j].c<m_confidence) {
			Quaternion* ptr = (Quaternion*)rotRing[j].get(-1);
			if (ptr!=NULL && ptr->c>0.0) rotQuat[j] = *ptr;
			else rotQuat[j].init();
		}
	}

	//
	if (appParam.useJointConst) niJoints->CheckJointsRotation();

	return;
}





void   CExKinectWin::convertUpperPos2JointsData(void)
{
	//
	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		if (posVect[j].c<m_confidence) {
			Vector<double>* ptr = (Vector<double>*)posRing[j].get(-1);
			if (ptr!=NULL && ptr->c>0.0) posVect[j] = *ptr;
			else posVect[j].init(-1.0);
		}
	}

	//
	if (appParam.useMvavSmooth) niJoints->PosMovingAverage(NiSDK_Kinect);
	if (appParam.useJointConst) niJoints->PosVibNoiseCanceler();

	//
//	Vector<double> torso_down(0.0, 0.0, -1.0);
	Vector<double> torso_down  = posVect[NI_SDK_TORSO]   - posVect[NI_SDK_NECK];
	Vector<double> torso_up	   = - torso_down;
	Vector<double> shldr_left  = posVect[NI_SDK_L_SHLDR] - posVect[NI_SDK_R_SHLDR];
	Vector<double> shldr_right = - shldr_left;

	//
	rotQuat[NI_SDK_PELVIS].init();
	posVect[NI_SDK_PELVIS].init();

	// PELVIS YZŽ˛‚̉ń“]
	if (posVect[NI_SDK_R_SHLDR].c>=m_confidence && posVect[NI_SDK_L_SHLDR].c>=m_confidence) { 
		Vector<double> vect = NewellMethod(posVect[NI_SDK_L_SHLDR], posVect[NI_SDK_R_SHLDR], posVect[NI_SDK_TORSO]);
		//
		rotQuat[NI_SDK_PELVIS] = V2VQuaternion(vect_fwrd, vect); 
		//if (rotQuat[NI_SDK_PELVIS].c==-1.0 && rotQuat[NI_SDK_PELVIS].s==0.0 && rotQuat[NI_SDK_PELVIS].n==0.0) {
		//	rotQuat[NI_SDK_PELVIS].setRotation(PI, torso_up);
		//}
	}

	// X Ž˛‰ń“]
	Quaternion quat_lshd = ~rotQuat[NI_SDK_PELVIS]*posVect[NI_SDK_L_SHLDR]*rotQuat[NI_SDK_PELVIS];
	Quaternion quat_rshd = ~rotQuat[NI_SDK_PELVIS]*posVect[NI_SDK_R_SHLDR]*rotQuat[NI_SDK_PELVIS];

	Vector<double> shd_left  = quat_lshd.getVector() - quat_rshd.getVector(); 
	Vector<double> shd_right = - shd_left;
	double thx = atan2(shd_left.z, shd_left.y);
	Quaternion xrot;
	xrot.setRotation(thx, 1.0, 0.0, 0.0, 1.0);

	// YŽ˛‰ń“]•âł
	Quaternion yrot;
	yrot.setRotation((double)appParam.YaxisCorrect, 0.0, 1.0, 0.0, 1.0);

	//
	rotQuat[NI_SDK_PELVIS] = rotQuat[NI_SDK_PELVIS]*xrot*yrot;


	/////////////////////////
	//
	rotQuat[NI_SDK_TORSO]  = VPPQuaternion(torso_up, posVect[NI_SDK_TORSO], posVect[NI_SDK_NECK]);
	rotQuat[NI_SDK_NECK]   = PPPQuaternion(posVect[NI_SDK_TORSO],  posVect[NI_SDK_NECK],  posVect[NI_SDK_HEAD]);

	// To Relative Coordinate
	rotQuat[NI_SDK_L_SHLDR] = VPPQuaternion(shldr_left,  posVect[NI_SDK_L_SHLDR], posVect[NI_SDK_L_ELBOW]);
	rotQuat[NI_SDK_R_SHLDR] = VPPQuaternion(shldr_right, posVect[NI_SDK_R_SHLDR], posVect[NI_SDK_R_ELBOW]);
	//
	rotQuat[NI_SDK_L_ELBOW] = PPPQuaternion(posVect[NI_SDK_L_SHLDR], posVect[NI_SDK_L_ELBOW], posVect[NI_SDK_L_WRIST]);
	rotQuat[NI_SDK_R_ELBOW] = PPPQuaternion(posVect[NI_SDK_R_SHLDR], posVect[NI_SDK_R_ELBOW], posVect[NI_SDK_R_WRIST]);
	rotQuat[NI_SDK_L_WRIST] = PPPQuaternion(posVect[NI_SDK_L_ELBOW], posVect[NI_SDK_L_WRIST], posVect[NI_SDK_L_HAND]);
	rotQuat[NI_SDK_R_WRIST] = PPPQuaternion(posVect[NI_SDK_R_ELBOW], posVect[NI_SDK_R_WRIST], posVect[NI_SDK_R_HAND]);

	//
	Quaternion pelvisXY;
	Vector<double> euler = Quaternion2EulerXYZ(rotQuat[NI_SDK_PELVIS]);
	euler.z = 0.0;
	pelvisXY.setEulerXYZ(euler);


	// To Avatar Coordinate
	rotQuat[NI_SDK_L_WRIST] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_L_WRIST]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_R_WRIST] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_R_WRIST]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_L_ELBOW] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_L_ELBOW]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_R_ELBOW] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_R_ELBOW]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_L_SHLDR] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_L_SHLDR]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_R_SHLDR] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_R_SHLDR]*rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_NECK]    = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_NECK]   *rotQuat[NI_SDK_PELVIS];
	rotQuat[NI_SDK_TORSO]   = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_TORSO]  *rotQuat[NI_SDK_PELVIS];

//	rotQuat[NI_SDK_L_ANKLE].init();
//	rotQuat[NI_SDK_R_ANKLE].init();
	rotQuat[NI_SDK_L_KNEE].init();
	rotQuat[NI_SDK_R_KNEE].init();
	rotQuat[NI_SDK_L_HIP]   = ~pelvisXY;
	rotQuat[NI_SDK_R_HIP]   = ~pelvisXY;

	//
	// Face
	rotQuat[NI_SDK_HEAD].init(-1.0);
	if (isDetectFace) {
		rotQuat[NI_SDK_HEAD] = getFaceRotation();
		rotQuat[NI_SDK_HEAD] = ~rotQuat[NI_SDK_PELVIS]*rotQuat[NI_SDK_HEAD]*rotQuat[NI_SDK_PELVIS];
	}

	//
	for (int j=0; j<KINECT_JOINT_NUM; j++) {
		if (rotQuat[j].c<m_confidence) {
			Quaternion* ptr = (Quaternion*)rotRing[j].get(-1);
			if (ptr!=NULL && ptr->c>0.0) rotQuat[j] = *ptr;
			else rotQuat[j].init();
		}
	}

	//
	if (appParam.useJointConst) niJoints->CheckJointsRotation();
}





void  CExKinectWin::lostTrackingUser(int uid)
{ 
	DEBUG_ERR("LOST   TRACKING USER (%d)", uid);

	clearJointsData();
	//
	sharedMem->isTracking = FALSE;
	sharedMem->clearLocalAnimationData();

	if (niNetwork->sendSocket>0) {
		if (appParam.netOutMode==NETandLOCAL) sharedMem->clearLocalAnimationIndex();
		/*
		if (appParam.netFastMode) {
			sendAnimationData<float> (posVect, rotQuat, niNetwork, NiSDK_Kinect, KINECT_JOINT_NUM);
		}
		else {
			sendAnimationData<double>(posVect, rotQuat, niNetwork, NiSDK_Kinect, KINECT_JOINT_NUM);
		}*/
		//
		sendAnimationData<float> (posVect, rotQuat, niNetwork, NiSDK_Kinect, KINECT_JOINT_NUM);
	}
}




void  CExKinectWin::detectTrackingUser(int uid)
{ 
	DEBUG_ERR("DETECT TRACKING USER (%d)", uid);

	sharedMem->isTracking = TRUE;
}




////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// ƒXƒŒƒbƒh
//

UINT  kinectEventLoop(LPVOID pParam)
{
	if (pParam==NULL) return 1;

	CExKinectWin* kinect = (CExKinectWin*)pParam;
	if (kinect->device->context==NULL) return 1; 
	if (kinect->pSensorFrame==NULL)    return 1; 
	
	CExView* pview    = kinect->pSensorFrame->pView;
	kinect->pViewData = &pview->viewData;


	BOOL ret;
	//
	try {
		Loop {
			if (kinect->getDevState()==NI_STATE_DETECT_STOPPING) continue;

			if (kinect->m_use_image) {
				ret = kinect->device->wait_Image();
			}

			//
			if (kinect->getDevState()==NI_STATE_DETECT_STOPPING) continue;
			kinect->hasDepthData = FALSE;
			if (kinect->getDevState()==NI_STATE_DETECT_EXEC || !isNull(kinect->pDepthFrame)) {
				ret = kinect->device->wait_Depth();
				if (ret) kinect->hasDepthData = TRUE;
			}

			if (kinect->getDevState()==NI_STATE_DETECT_STOPPING) continue;
			if (isNull(kinect->pSensorFrame)) break;
			kinect->makeDisplayImage();			// need Depth Data when detected users are painted

			if (kinect->getDevState()==NI_STATE_DETECT_EXEC) {
				kinect->trackingJoints();
			}

			//
			if (isNull(kinect->pSensorFrame)) break;
			if (isNull(kinect->pSensorFrame->pView)) break;
			if (!pview->SetNewSurface()) break;
			if (isNull(kinect->pSensorFrame)) break;
			pview->ExecRender();

			
			// Depth Image
			if (!isNull(kinect->pDepthFrame) && kinect->hasDepthData) {
				kinect->makeDisplayDepth(kinect->pDepthFrame->pView);
				if (!isNull(kinect->pDepthFrame)) {
					if (kinect->pDepthFrame->pView->SetNewSurface()) {
						if (!isNull(kinect->pDepthFrame)) kinect->pDepthFrame->pView->ExecRender();
					}
				}
			}
		}
	}

	catch (std::exception& ex) {
		kinect->m_err_mesg = _T("kinectEventLoop() Exception: ");
		kinect->m_err_mesg+= mbs2ts((char*)ex.what());
		
		DEBUG_ERR("kinectEventLoop: Exception occurred");
		return 2;
	}
	
	return 0;
}




#endif