/**
	SLKinect.cpp  by Fumi.Iseki (c)

		http://www.nsl.tuis.ac.jp/
*/



#include  "stdafx.h"

#include  "SLKinect.h"

#include  "BasicLib.h"
#include  "MathLib.h"
#include  "GraphLib.h"
#include  "WinBaseLib.h"
#include  "DLGBoxLib.h"
#include  "Rotation.h"

#include  "SensorFrame.h"
#include  "MainFrm.h"
#include  "SetLogMode.h"
#include  "SetViewMode.h"



#ifdef _DEBUG
#define new DEBUG_NEW
#endif



using namespace jbxl;
using namespace jbxwl;




/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Joints
//

/*
	OpenNI Joints

	DUMMY,
	XN_SKEL_HEAD, XN_SKEL_NECK, XN_SKEL_TORSO, XN_SKEL_WAIST,

	XN_SKEL_LEFT_COLLAR,  XN_SKEL_LEFT_SHOULDER,  XN_SKEL_LEFT_ELBOW,  XN_SKEL_LEFT_WRIST,  XN_SKEL_LEFT_HAND,  XN_SKEL_LEFT_FINGERTIP,
	XN_SKEL_RIGHT_COLLAR, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_WRIST, XN_SKEL_RIGHT_HAND, XN_SKEL_RIGHT_FINGERTIP,

	XN_SKEL_LEFT_HIP,  XN_SKEL_LEFT_KNEE,  XN_SKEL_LEFT_ANKLE,  XN_SKEL_LEFT_FOOT,
	XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_ANKLE, XN_SKEL_RIGHT_FOOT
*/


// SL/OpenSim Joints
static  std::string SLJointName[] =		// SL_MAX_JOINT_NUM
{
	"mPelvis",
	"mTorso", "mChest", "mNeck", "mHead",

	"mCollarLeft",  "mShoulderLeft",  "mElbowLeft",  "mWristLeft",
	"mCollarRight", "mShoulderRight", "mElbowRight", "mWristRight",
	
	"mHipLeft",  "mKneeLeft",  "mAnkleLeft",
	"mHipRight", "mKneeRight", "mAnkleRight"
};



/*
 0: DUMMY					=> none
 1: XN_SKEL_HEAD			=> mHead
 2: XN_SKEL_NECK			=> mNeck
 3: XN_SKEL_TORSO			=> mTrso, mPelvis
 4: XN_SKEL_WAIST			=> none
 5: XN_SKEL_LEFT_COLLAR		=> none
 6: XN_SKEL_LEFT_SHOULDER	=> mShoulderRight
 7: XN_SKEL_LEFT_ELBOW		=> mElbowRight
 8: XN_SKEL_LEFT_WRIST		=> none
 9: XN_SKEL_LEFT_HAND		=> mWristRight
10: XN_SKEL_LEFT_FINGERTIP	=> none
11: XN_SKEL_RIGHT_COLLAR	=> none
12: XN_SKEL_RIGHT_SHOULDER	=> mShoulderLeft
13: XN_SKEL_RIGHT_ELBOW		=> mElbowLeft
14: XN_SKEL_RIGHT_WRIST		=> none
15: XN_SKEL_RIGHT_HAND		=> mWristLeft
16: XN_SKEL_RIGHT_FINGERTIP	=> none
17: XN_SKEL_LEFT_HIP		=> mHipRight
18: XN_SKEL_LEFT_KNEE		=> mKneeRight
19: XN_SKEL_LEFT_ANKLE		=> none 
20: XN_SKEL_LEFT_FOOT		=> mAnkleRight
21: XN_SKEL_RIGHT_HIP		=> mHipLeft
22: XN_SKEL_RIGHT_KNEE		=> mKneeLeft
23: XN_SKEL_RIGHT_ANKLE		=> none
24: XN_SKEL_RIGHT_FOOT		=> mAnkleLeft
*/

static  int  OpenNI2SLJointNum[] = 
{
	-1, 
	 4,  3,  1, -1, 
	-1, 10, 11, -1, 12, -1, 
	-1,  6,  7, -1,  8, -1,  
	16, 17, -1, 18,
	13, 14, -1, 15 
};




/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// CSLKinectApp メインクラス
//

BEGIN_MESSAGE_MAP(CSLKinectApp, CWinApp)
	ON_COMMAND(ID_APP_ABOUT, &CSLKinectApp::OnAppAbout)
	// 標準のファイル基本ドキュメント コマンド
	ON_COMMAND(ID_MODEL_WND_OPEN, &CSLKinectApp::OnModelingWndOpen)
	ON_COMMAND(ID_LOG_WND_OPEN, &CSLKinectApp::OnLogWndOpen)
	ON_COMMAND(ID_SNSR_WND_OPEN_1, &CSLKinectApp::OnSnsrWndOpen_x1)
	ON_COMMAND(ID_SNSR_WND_OPEN_2, &CSLKinectApp::OnSnsrWndOpen_x2)
	ON_COMMAND(ID_SNSR_WND_OPEN_3, &CSLKinectApp::OnSnsrWndOpen_x3)
	ON_COMMAND(ID_SNSR_WND_OPEN_4, &CSLKinectApp::OnSnsrWndOpen_x4)
	ON_COMMAND(ID_SETTING_LOGINGMODE, &CSLKinectApp::OnSettingLogingmode)
	ON_COMMAND(ID_SETTING_VIEWMODE, &CSLKinectApp::OnSettingViewmode)
END_MESSAGE_MAP()






// CSLKinectApp コンストラクション
CSLKinectApp::CSLKinectApp()
{
	pMainFrame		= NULL;
	pLogFrame		= NULL;
	pLogDoc			= NULL;
	pSensorView		= NULL;
	pModelView		= NULL;

	kinect			= NULL;

	pDocTemplLOG	= NULL;
	pDocTemplSNR	= NULL;
	pDocTemplMDL	= NULL;

	cameraScale		= 2;

	// Shared Memory
	for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
		mapHandle[i] = NULL;
		ptrShm[i]    = NULL;
	}
	isShmCreated = CreateShm();
}




CSLKinectApp::~CSLKinectApp()
{
	CloseShm();

	Dx9ReleaseInterface();

	if (kinect!=NULL) {
		if (kinect->m_state==OPENNI_DETECT_EXEC) {
			kinect->stop_Detection();
		}
		DeleteKinect();
	}
}





// 唯一の CSLKinectApp オブジェクトです。

CSLKinectApp theApp;



// CSLKinectApp 初期化

BOOL CSLKinectApp::InitInstance()
{
	CWinApp::InitInstance();

	SetRegistryKey(_T("SLKinect by Fumi.Iseki and NSL"));
	LoadStdProfileSettings(4);  // 標準の INI ファイルのオプションをロードします (MRU を含む)

	//
	CMultiDocTemplate* pDocTemplate;

	pDocTemplate = new CMultiDocTemplate(
		IDR_MAINFRAME,
		RUNTIME_CLASS(CLogWndDoc),
		RUNTIME_CLASS(CLogWndFrame),
		RUNTIME_CLASS(CLogWndView));
	if (!pDocTemplate) return FALSE;
	AddDocTemplate(pDocTemplate);
	pDocTemplLOG = pDocTemplate;

	pDocTemplate = new CMultiDocTemplate(
		IDR_MAINFRAME,
		RUNTIME_CLASS(CExDocument),
		RUNTIME_CLASS(CSensorFrame), 
		RUNTIME_CLASS(CDxDirectView));
	if (!pDocTemplate) return FALSE;
	AddDocTemplate(pDocTemplate);
	pDocTemplSNR = pDocTemplate;

	pDocTemplate = new CMultiDocTemplate(
		IDR_MAINFRAME,
		RUNTIME_CLASS(CExDocument),
		RUNTIME_CLASS(CExFrame), 
		RUNTIME_CLASS(CDx3DDirectView));
	if (!pDocTemplate) return FALSE;
	AddDocTemplate(pDocTemplate);
	pDocTemplMDL = pDocTemplate;


	// メインフレーム ウィンドウを作成
	pMainFrame = new CMainFrame;
	pMainFrame->pApp = this;
	if (!pMainFrame->LoadFrame(IDR_MAINFRAME)) return FALSE;
	m_pMainWnd = pMainFrame;

	// メインフレーム ウィンドウのサイズ
	int sx = GetSystemMetrics(SM_CXSCREEN)/2;	// X方向の解像度
	int sy = GetSystemMetrics(SM_CYSCREEN)/2;	// Y方向の解像度
	m_pMainWnd->SetWindowPos(NULL, sx-APP_WND_XSIZE/2, sy-APP_WND_YSIZE/2, APP_WND_XSIZE, APP_WND_YSIZE, 0);
	
	// メインフレームウィンドウの表示
	m_pMainWnd->ShowWindow(SW_SHOW);
	m_pMainWnd->UpdateWindow();



	///////////////////////////////////////////////////////////////////////////////////////////////
	// DirectX9 の初期化

	BOOL rslt = Dx9CreateInterface(this);
	if (!rslt) {
		CString mesg, noti;
		mesg.LoadString(IDS_STR_FAIL_GET_DX9DEV);			// "DirectX9の初期化に失敗しました"
		noti.LoadString(IDS_STR_ERROR);						// "エラー"
		MessageBox(m_pMainWnd->m_hWnd, mesg, noti, MB_OK);
		return FALSE;
	}


	///////////////////////////////////////////////////////////////////////////////////////////////
	// Kinect の初期化

	CString mesg, noti;
	mesg.LoadString(IDS_STR_INIT_KINECT);				// "Kinect の初期化中"
	noti.LoadString(IDS_STR_INFO);						// "情報"

	CMessageBoxDLG* mesgBox = new CMessageBoxDLG(noti, m_pMainWnd);
	if (mesgBox!=NULL) mesgBox->Display((char*)(LPCSTR)mesg);

	BOOL ret = FALSE;
	kinect = new CExKinectWnd();
	if (kinect!=NULL && kinect->context!=NULL) {
		if (kinect->isUseCamera) ret = kinect->create_Image();
		else ret = TRUE;
		if (ret) ret = kinect->create_Depth();
	}

	if (!ret) {
		if (mesgBox!=NULL) {
			delete(mesgBox);
			mesgBox = NULL;
		}
		//CString mesg, noti;
		mesg.LoadString(IDS_STR_FAIL_INIT_OPENNI);		// "OpenNIデバイスの初期化に失敗しました"
		noti.LoadString(IDS_STR_ERROR);					// "エラー"
		if (kinect!=NULL && kinect->m_err_mesg.buf!=NULL) {
			mesg.Append("\n");
			mesg.Append((char*)kinect->m_err_mesg.buf);
		}
		MessageBox(m_pMainWnd->m_hWnd, mesg, noti, MB_OK);
		DeleteKinect();

		Dx9ReleaseInterface();
		return FALSE;
	}

	if (mesgBox!=NULL) delete(mesgBox);

	//
	readConfigFile(SLKINECT_CONFIG_FILE);


	// デフォルトのミラーリング
	if (kinect!=NULL) {
		nowMirroring = kinect->isMirroring;
		kinect->context->SetGlobalMirror(kinect->isMirroring);
	}



	///////////////////////////////////////////////////////////////////////////////////////////////
	// Main Loop

	return TRUE;
}





////////////////////////////////////////////////////////////////////////////////////////////////////
// 共有メモリ

BOOL  CSLKinectApp::CreateShm(void)
{
	BOOL ret = TRUE;

	for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
		mapHandle[i] = CreateFileMapping((HANDLE)0xffffffff, NULL, PAGE_READWRITE, 0, SL_SHM_SIZE, SLJointName[i].c_str()); 
		if (mapHandle[i]==NULL) {
			ret = FALSE;
			break;
		}
		ptrShm[i] = (double*)MapViewOfFile(mapHandle[i], FILE_MAP_ALL_ACCESS, 0, 0, 0);
		if (ptrShm[i]==NULL) {
			ret = FALSE;
			break;
		}
		memset(ptrShm[i], 0, SL_SHM_SIZE);
	}

	if (!ret) CloseShm();
	return ret;
}




void  CSLKinectApp::CloseShm(void)
{
	for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
		if (ptrShm[i]!=NULL) {
			memset(ptrShm[i], 0, SL_SHM_SIZE);
			UnmapViewOfFile(ptrShm[i]);
			ptrShm[i] = NULL;
		}
		if (mapHandle[i]!=NULL) {
			CloseHandle(mapHandle[i]);
			mapHandle[i] = NULL;
		}
	}
}




void  CSLKinectApp::ClearShm(void)
{
	for (int i=0; i<SL_MAX_JOINT_NUM; i++) {
		if (ptrShm[i]!=NULL) {
			memset(ptrShm[i], 0, SL_SHM_SIZE);
		}
	}
}







////////////////////////////////////////////////////////////////////////////////////////////
// アプリケーションのバージョン情報に使われる CAboutDlg ダイアログ

class CAboutDlg : public CDialog
{
public:
	CAboutDlg();

// ダイアログ データ
	enum { IDD = IDD_ABOUTBOX };

protected:
	virtual void DoDataExchange(CDataExchange* pDX);

// 実装
protected:
	DECLARE_MESSAGE_MAP()
};



CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
}



void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
}



BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
END_MESSAGE_MAP()




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

// ダイアログを実行するためのアプリケーション コマンド
void CSLKinectApp::OnAppAbout()
{
	CAboutDlg aboutDlg;
	aboutDlg.DoModal();
}





//////////////////////////////////////////////////////////////////
// Distruction of child window
//

// ログフレーム
void CSLKinectApp::FrameDistructor(CExTextFrame* frm)
{
	if (frm==pLogFrame) {
		//DEBUG_ERR("CSLKinectApp::FrameDistructor: Stop Log Window Frame");

		if (kinect!=NULL) kinect->pLogDoc = NULL;
		pLogFrame = NULL;
		pLogDoc   = NULL;
		::Sleep(500);
	}
}




// センサービュー
void CSLKinectApp::ViewDistructor(CExView* view)
{
	if (view==pSensorView) {
		if (kinect!=NULL) { 
			if (kinect->pSensorFrame!=NULL) {
				kinect->pSensorFrame = NULL;
				::Sleep(500);
			}
		}

		pSensorView = NULL;
		//DEBUG_ERR("CSLKinectApp::ViewDistructor: Stop Screen View");
	}
}







// CSLKinectApp メッセージ ハンドラ

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

void CSLKinectApp::OnModelingWndOpen()
{
	if (kinect==NULL || pModelView!=NULL) return;

	CExFrame* pfrm = CreateDocFrmView(pDocTemplMDL, this);

	if (pfrm!=NULL) {
		pfrm->pDoc->Title		= "Modeling Window";
		pfrm->pDoc->hasReadData = TRUE;                                           
		pfrm->pDoc->hasViewData = TRUE;                                        
		pfrm->pView->cnstXYRate = TRUE;
   		pfrm->pView->colorMode  = GRAPH_COLOR_BGRA;
		pfrm->pView->xsize		= kinect->outputMode.nXRes;
		pfrm->pView->ysize		= kinect->outputMode.nYRes;
		pModelView = (CDx3DDirectView*)pfrm->pView;

		int ret = ExecDocFrmView(pfrm);
		if (ret==0) {

		}
		else if (ret>0) {
			ExecDocFrmViewError(pMainFrame->m_hWnd, ret);
		}
	}
	else {
		ExecDocFrmViewError(pMainFrame->m_hWnd, MSG_DFV_FR_NULL);	// Frameへのポインタが NULLです
	}

	return;
}



// Log Window
void CSLKinectApp::OnLogWndOpen()
{
	if (pLogFrame==NULL) {
		pLogFrame = ExecLogWnd(pDocTemplLOG, "Log Window", this);
		pLogDoc = pLogFrame->pDoc;
		if (kinect!=NULL) kinect->pLogDoc = pLogDoc;
	}
	return;
}




void CSLKinectApp::OnSnsrWndOpen_x1()
{
	cameraScale = 1;	// 1/1
	SensorWndOpen();
	return;
}



void CSLKinectApp::OnSnsrWndOpen_x2()
{
	cameraScale = 2;	// 1/2
	SensorWndOpen();
	return;
}



void CSLKinectApp::OnSnsrWndOpen_x3()
{
	cameraScale = 3;	// 1/3
	SensorWndOpen();
	return;
}



void CSLKinectApp::OnSnsrWndOpen_x4()
{
	cameraScale = 4;	// 1/4
	SensorWndOpen();
	return;
}




void CSLKinectApp::SensorWndOpen()
{
	if (kinect==NULL || pSensorView!=NULL) return;

	CExFrame* pfrm = CreateDocFrmView(pDocTemplSNR, this);

	if (pfrm!=NULL) {
		((CSensorFrame*)pfrm)->kinectApp = this;
		pfrm->pDoc->Title		= "Sensor Window";
		pfrm->pDoc->hasReadData = TRUE;                                           
		pfrm->pDoc->hasViewData = TRUE;                                        
		pfrm->pView->cnstXYRate	= TRUE;
   		pfrm->pView->colorMode  = GRAPH_COLOR_BGRA;		// for faster with Little Endian
		pfrm->pView->xsize		= kinect->outputMode.nXRes/cameraScale;
		pfrm->pView->ysize		= kinect->outputMode.nYRes/cameraScale;
		pSensorView = (CDxDirectView*)pfrm->pView;


		// ミラーリング処理
		if (nowMirroring!=kinect->isMirroring) {
			if (kinect->m_state==OPENNI_DETECT_EXEC) ((CSensorFrame*)pfrm)->OnCalibrationStop();
			kinect->context->SetGlobalMirror(kinect->isMirroring);
			nowMirroring = kinect->isMirroring;
			::Sleep(100);
		}

		// Camera
		if (kinect->image==NULL && kinect->isUseCamera) {
			CString mesg, noti;
			mesg.LoadString(IDS_STR_CREATE_IMAGE);				// "イメージの作成中"
			noti.LoadString(IDS_STR_INFO);						// "情報"
			CMessageBoxDLG* mesgBox = new CMessageBoxDLG(noti, m_pMainWnd);
			if (mesgBox!=NULL) mesgBox->Display((char*)(LPCSTR)mesg);
			if (kinect->m_state==OPENNI_DETECT_EXEC) ((CSensorFrame*)pfrm)->OnCalibrationStop();
			kinect->create_Image();
			if (mesgBox!=NULL) delete(mesgBox);
		}
		else if (kinect->image!=NULL && !kinect->isUseCamera) {
			if (kinect->m_state==OPENNI_DETECT_EXEC) ((CSensorFrame*)pfrm)->OnCalibrationStop();
			kinect->delete_Image();
		}


		//
		int ret = ExecDocFrmView(pfrm);
		if (ret==0) {
			if (kinect!=NULL) {
				kinect->m_scale = cameraScale;
				kinect->pSensorFrame = pfrm;
				//if (kinect->image!=NULL && kinect->depth!=NULL) kinect->depth->GetAlternativeViewPointCap().SetViewPoint(*(kinect->image));
				AfxBeginThread(kinectEventLoop, (LPVOID)kinect, /*THREAD_PRIORITY_HIGHEST*/ THREAD_PRIORITY_NORMAL);
			}
		}
		else if (ret>0) {
			ExecDocFrmViewError(pMainFrame->m_hWnd, ret);
		}
	}
	else {
		ExecDocFrmViewError(pMainFrame->m_hWnd, MSG_DFV_FR_NULL);	// Frameへのポインタが NULLです
	}

	return;
}




void CSLKinectApp::OnSettingLogingmode()
{
	CSetLogMode* logdlg = new CSetLogMode(kinect->printPosMode, kinect->printRotMode, kinect->printQutMode);
	if (logdlg==NULL) return;
	logdlg->DoModal();

	kinect->printPosMode = logdlg->printPosMode();
	kinect->printRotMode = logdlg->printRotMode();
	kinect->printQutMode = logdlg->printQutMode();
	
	delete (logdlg);
	writeConfigFile(SLKINECT_CONFIG_FILE);
}





void CSLKinectApp::OnSettingViewmode()
{
	CSetViewMode* viwdlg = new CSetViewMode(kinect->isMirroring, kinect->isUseCamera);
	if (viwdlg==NULL) return;
	viwdlg->DoModal();
	
	kinect->isMirroring = viwdlg->isMirroring();
	kinect->isUseCamera = viwdlg->isUseCamera();
	
	delete (viwdlg);
	writeConfigFile(SLKINECT_CONFIG_FILE);
}






/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Tools
//

BOOL  CSLKinectApp::getBoolfromList(tList* lt, char* key, BOOL dflt)
{
	BOOL  ret = dflt;
	char* val = NULL;

	if (dflt) val = get_strparam_tList(lt, key, "true");
	else      val = get_strparam_tList(lt, key, "false");

	if (val!=NULL) {
		if (!strcasecmp("true", val)) ret = TRUE;
		else ret = FALSE;
		free(val);
	}

	return ret;
}




void  CSLKinectApp::readConfigFile(char* fn)
{    
	tList* lt = read_index_tList_file(fn, ' ');

	if (lt!=NULL) {
		kinect->isMirroring		= getBoolfromList(lt, "Mirroring", kinect->isMirroring);
		kinect->isUseCamera		= getBoolfromList(lt, "UseCamera", kinect->isUseCamera);

		kinect->printPosMode	= getBoolfromList(lt, "printPosMode", kinect->printPosMode);
		kinect->printRotMode	= getBoolfromList(lt, "printRotMode", kinect->printRotMode);
		kinect->printQutMode	= getBoolfromList(lt, "printQutMode", kinect->printQutMode);

		del_all_tList(&lt);
    }
}




void  CSLKinectApp::writeConfigFile(char* fn)
{    
	FILE* fp = fopen(fn, "wb");

	if (kinect->isMirroring)  fprintf(fp, "Mirroring %s\n", "TRUE");
	else					  fprintf(fp, "Mirroring %s\n", "FALSE");
	if (kinect->isUseCamera)  fprintf(fp, "UseCamera %s\n", "TRUE");
	else					  fprintf(fp, "UseCamera %s\n", "FALSE");

	if (kinect->printPosMode) fprintf(fp, "printPosMode %s\n", "TRUE");
	else					  fprintf(fp, "printPosMode %s\n", "FALSE");
	if (kinect->printRotMode) fprintf(fp, "printRotMode %s\n", "TRUE");
	else					  fprintf(fp, "printRotMode %s\n", "FALSE");
	if (kinect->printQutMode) fprintf(fp, "printQutMode %s\n", "TRUE");
	else					  fprintf(fp, "printQutMode %s\n", "FALSE");

	fclose(fp);
}










/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// CExKinctWnd クラス
//

CExKinectWnd::CExKinectWnd(void)
{
	clearBoneData();
	clearStartPosData();

	isUseCamera		= TRUE;

	printPosMode	= TRUE;
	printRotMode	= FALSE;
	printQutMode	= TRUE;
}




void  CExKinectWnd::clearBoneData(void)
{
	memset(posVect, 0, sizeof(Vector<double>)*OPENNI_MAX_JOINT_NUM);
	memset(prvVect, 0, sizeof(Vector<double>)*OPENNI_MAX_JOINT_NUM);

	memset(rotQuat, 0, sizeof(Quaternion)*OPENNI_MAX_JOINT_NUM);	
	memset(prvQuat, 0, sizeof(Quaternion)*OPENNI_MAX_JOINT_NUM);
}




void  CExKinectWnd::clearStartPosData(void)
{
	startPos = Vector<double>(0.0, 0.0, 0.0);
	isCalibrated = FALSE;
}




void  CExKinectWnd::setStartPosData(Vector<double> vect)
{
	startPos = vect;
	isCalibrated = TRUE;
}



void	CExKinectWnd::logingJointsData(int uid)
{
	if (pLogDoc!=NULL) {
		for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
			XnVector3D  pos = jointPositionData((XnSkeletonJoint)j);
			XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j);
			
			if (printPosMode || printRotMode || printQutMode) {
				pLogDoc->printFormat("%2d (NAME)-> %s\n", j, getJointName((XnSkeletonJoint)j).c_str());
			}
			
			if (printPosMode) pLogDoc->printFormat("%2d (POS) -> (%f, %f, %f)\n", j, pos.X, pos.Y, pos.Z);
			if (printRotMode) {
				pLogDoc->printFormat("%2d (ROT) -> (%f, %f, %f)\n", j, rot.elements[0], rot.elements[1], rot.elements[2]);
				pLogDoc->printFormat("%2d (ROT) -> (%f, %f, %f)\n", j, rot.elements[3], rot.elements[4], rot.elements[5]);
				pLogDoc->printFormat("%2d (ROT) -> (%f, %f, %f)\n", j, rot.elements[6], rot.elements[7], rot.elements[8]);
			}
			if (printQutMode) {
				pLogDoc->printFormat("%2d (QUT) -> (%f, %f, %f), %f\n", j, rotQuat[j].x, rotQuat[j].y, rotQuat[j].z, rotQuat[j].s);
			}
		}
	}
}




void	CExKinectWnd::convertData(int uid)
{
	convertRot2SLData(uid);
}





void	CExKinectWnd::convertRot2SLData(int uid)
{
	Vector<double> vect;
	bool normal_joint[OPENNI_MAX_JOINT_NUM];

	//
	for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
		if (OpenNI2SLJointNum[j]>=0) {
			if (jointPositionConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) { 
				XnVector3D  pos = jointPositionData((XnSkeletonJoint)j);
				posVect[j].set(-pos.Z, pos.X, pos.Y);
				prvVect[j] = posVect[j];
			}
			else {
				posVect[j] = prvVect[j];
			}
		}
	}


	// mPelvis
	posVect[0] = (posVect[17] + posVect[21])*0.5;
	vect = posVect[21] - posVect[17];
	double thz = atan2(-vect.x, vect.y);
	rotQuat[0].setRotation(thz, 0.0, 0.0, 1.0);
	if (!isCalibrated) setStartPosData(posVect[0]);


	//
	for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
		normal_joint[j] = true;

		if (OpenNI2SLJointNum[j]>=0) {
			if (jointRotationConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) { 
				XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j);
				double m11 = rot.elements[0];
				double m12 = rot.elements[1];
				double m13 = rot.elements[2];
				double m21 = rot.elements[3];
				double m31 = rot.elements[6];
				double m32 = rot.elements[7];
				double m33 = rot.elements[8];
				Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33);
	
				Vector<double> vct(-eul.x, -eul.y, eul.z);	// Mirror: vct(-eul.x, eul.y, -eul.z);
				rotQuat[j].setEulerYZX(vct);
				prvQuat[j] = rotQuat[j];
			}
			else {
				rotQuat[j] = prvQuat[j];
			}
		}
	}


	// Leg
	rotQuat[22] = ~rotQuat[21]*rotQuat[22];
	rotQuat[18] = ~rotQuat[17]*rotQuat[18];

	// Right Hand
	rotQuat[13] = ~rotQuat[12]*rotQuat[13];
	rotQuat[12] = ~rotQuat[ 2]*rotQuat[12];

	// Left Hand
	rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7];
	rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6];

	rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2];
	rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3];


	// Set to Shared Memory
	for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
		int n = OpenNI2SLJointNum[j];
		if (n>=0 && normal_joint[j]) {
			double* shm = theApp.ptrShm[n];
			if (shm!=NULL) {
				shm[4] = rotQuat[j].x;
				shm[5] = rotQuat[j].y;
				shm[6] = rotQuat[j].z;
				shm[7] = rotQuat[j].s;
			}
		}
	}

	// mPelvis(0)
	double* shm = theApp.ptrShm[0];
	if (shm!=NULL) {
		shm[1] = (posVect[0].x - startPos.x)/1000.;
		shm[2] = (posVect[0].y - startPos.y)/1000.;
		shm[3] = (posVect[0].z - startPos.z)/1000.;
		shm[4] = rotQuat[0].x;
		shm[5] = rotQuat[0].y;
		shm[6] = rotQuat[0].z;
		shm[7] = rotQuat[0].s;
	}
}