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




using namespace jbxl;
using namespace jbxwl;





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

// Default or No SDK
int	 NI_JNT_PELVIS	 = NI_PELVIS;
int	 NI_JNT_TORSO	 = NI_TORSO;
int	 NI_JNT_NECK	 = NI_NECK;
int	 NI_JNT_HEAD	 = NI_HEAD;

int	 NI_JNT_L_EYE	 = NI_L_EYE;
int	 NI_JNT_R_EYE	 = NI_R_EYE;
int	 NI_JNT_L_BUST	 = NI_L_BUST;
int	 NI_JNT_R_BUST	 = NI_R_BUST;
	
int  NI_JNT_L_SHLDR	 = NI_L_SHLDR;
int  NI_JNT_L_ELBOW	 = NI_L_ELBOW;
int	 NI_JNT_L_WRIST	 = NI_L_WRIST;
int	 NI_JNT_L_HAND	 = NI_L_HAND;

int	 NI_JNT_R_SHLDR	 = NI_R_SHLDR;
int	 NI_JNT_R_ELBOW	 = NI_R_ELBOW;
int	 NI_JNT_R_WRIST	 = NI_R_WRIST;
int	 NI_JNT_R_HAND	 = NI_R_HAND;

int	 NI_JNT_L_HIP	 = NI_L_HIP;
int  NI_JNT_L_KNEE	 = NI_L_KNEE;
int	 NI_JNT_L_ANKLE	 = NI_L_ANKLE;
int	 NI_JNT_L_FOOT	 = NI_L_FOOT;
	
int	 NI_JNT_R_HIP	 = NI_R_HIP;
int	 NI_JNT_R_KNEE	 = NI_R_KNEE;
int	 NI_JNT_R_ANKLE	 = NI_R_ANKLE;
int	 NI_JNT_R_FOOT	 = NI_R_FOOT;

int  NI_JNT_L_FINGER = NI_L_FINGER;		// Parameter of Finger (Dummy Joint)
int  NI_JNT_R_FINGER = NI_R_FINGER;		// Parameter of Finger (Dummy Joint)
int  NI_JNT_FACE	 = NI_FACE;	// Parameter of Expression (Dummy Joint)




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

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

static  int  _Kinect2JointNameNum[] =	// NI_KINECT_JOINT_NUM (20)
{
	 0,  1,  4,  5, 
	12, 13, 14, 15,
	17, 18, 19, 20,
	21, 22, 23, 24,
	26, 27, 28, 29 
};

/*
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
};
*/

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 (34)
{
	 0,  1,  2,  3,  4,  5,  6,
	 8,  7, 10,  9, 
	16, 17, 18, 19, 20,  
	11, 12, 13, 14, 15, 
	26, 27, 28, 29, 30,
	21, 22, 23, 24, 25,
	32, 31, 33 
};


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

	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 (34)
{
	 0,  3, -1, -1,  2,  1, -1, 
	-1, -1, -1, -1,
	-1, 12, 13, -1, 15,
	-1,  6,  7, -1,  9,
	21, 22, -1, 24, -1,
	17, 18, -1, 20, -1,
	-1, -1, -1
};

static  int  _Ni2KinectPosJointNum[] =		// NI_JOINT_NUM (34)
{
	 0,  1, -1, -1,  2,  3, -1, 
	-1, -1, -1, -1,
	-1,  4,  5,  6,  7,
	-1,  8,  9, 10, 11,
	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
{
	 0,  3, -1, -1,  2, -1, -1,
	-1, -1, -1, -1,
	-1, 12, 13, -1, -1,
	-1,  6,  7, -1, -1,
	21, 22, -1, -1, -1,
	17, 18, -1, -1, -1,
	-1, -1, -1
};

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

static  int  _Ni2RotJointNum[] =			// NI_JOINT_NUM
{
	 0,  1,  2,  3,  4,  5, -1,  
	 7,  8,  9, 10, 
	11, 12, 13, 14, -1, 
	16, 17, 18, 19, -1, 
	21, 22, 23, 24, -1, 
	26, 27, 38, 29, -1,
	31, 32, 33 
};


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 (33)
{
	 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
};
*/

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_JNT_PELVIS	=  0;
	NI_JNT_TORSO	=  3;	// XN_SKEL_TORSO;
	NI_JNT_NECK		=  2;	// XN_SKEL_NECK;
	NI_JNT_HEAD		=  1;	// XN_SKEL_HEAD;

	NI_JNT_L_EYE	= -1;
	NI_JNT_R_EYE	= -1;
	NI_JNT_L_BUST	= -1;
	NI_JNT_R_BUST	= -1;
	
	NI_JNT_L_SHLDR	= 12;	// XN_SKEL_RIGHT_SHOULDER;
	NI_JNT_L_ELBOW	= 13;	// XN_SKEL_RIGHT_ELBOW;
	NI_JNT_L_WRIST	= -1;
	NI_JNT_L_HAND	= 15;	// XN_SKEL_RIGHT_HAND;

	NI_JNT_R_SHLDR	=  6;	// XN_SKEL_LEFT_SHOULDER;
	NI_JNT_R_ELBOW	=  7;	// XN_SKEL_LEFT_ELBOW;
	NI_JNT_R_WRIST	= -1;
	NI_JNT_R_HAND	=  9;	// XN_SKEL_LEFT_HAND;
	
	NI_JNT_L_HIP	= 21;	// XN_SKEL_RIGHT_HIP;
	NI_JNT_L_KNEE	= 22;	// XN_SKEL_RIGHT_KNEE;
	NI_JNT_L_ANKLE	= -1;
	NI_JNT_L_FOOT	= 24;	// XN_SKEL_RIGHT_FOOT;

	NI_JNT_R_HIP	= 17;	// XN_SKEL_LEFT_HIP;
	NI_JNT_R_KNEE	= 18;	// XN_SKEL_LEFT_KNEE;
	NI_JNT_R_ANKLE	= -1;
	NI_JNT_R_FOOT	= 20;	// XN_SKEL_LEFT_FOOT;

	NI_JNT_L_FINGER	= -1;
	NI_JNT_R_FINGER	= -1;
	NI_JNT_FACE		= -1;

	return;
}



// Kinect SDK
void  jbxwl::NiSetKinectJointNums(void)
{
	NI_JNT_PELVIS	=  0;	// NUI_SKELETON_POSITION_HIP_CENTER
	NI_JNT_TORSO	=  1;	// SPINE は書き換えて使用 
	NI_JNT_NECK		=  2;	// NUI_SKELETON_POSITION_SHOULDER_CENTER
	NI_JNT_HEAD		=  3;	// NUI_SKELETON_POSITION_HEAD

	NI_JNT_L_EYE	= -1;
	NI_JNT_R_EYE	= -1;
	NI_JNT_L_BUST	= -1;
	NI_JNT_R_BUST	= -1;
		
	NI_JNT_L_SHLDR	=  4;	// NUI_SKELETON_POSITION_SHOULDER_LEFT
	NI_JNT_L_ELBOW	=  5;	// NUI_SKELETON_POSITION_ELBOW_LEFT
	NI_JNT_L_WRIST	=  6;	// NUI_SKELETON_POSITION_WRIST_LEFT
	NI_JNT_L_HAND	=  7;	// NUI_SKELETON_POSITION_HAND_LEFT
	
	NI_JNT_R_SHLDR	=  8;	// NUI_SKELETON_POSITION_SHOULDER_RIGHT
	NI_JNT_R_ELBOW	=  9;	// NUI_SKELETON_POSITION_ELBOW_RIGHT
	NI_JNT_R_WRIST	= 10;	// NUI_SKELETON_POSITION_WRIST_RIGHT
	NI_JNT_R_HAND	= 11;	// NUI_SKELETON_POSITION_HAND_RIGHT

	NI_JNT_L_HIP	= 12;	// NUI_SKELETON_POSITION_HIP_LEFT
	NI_JNT_L_KNEE	= 13;	// NUI_SKELETON_POSITION_KNEE_LEFT
	NI_JNT_L_ANKLE	= 14;	// NUI_SKELETON_POSITION_ANKLE_LEFT
	NI_JNT_L_FOOT	= 15;	// NUI_SKELETON_POSITION_FOOT_LEFT
	
	NI_JNT_R_HIP	= 16;	// NUI_SKELETON_POSITION_HIP_RIGHT
	NI_JNT_R_KNEE	= 17;	// NUI_SKELETON_POSITION_KNEE_RIGHT
	NI_JNT_R_ANKLE	= 18;	// NUI_SKELETON_POSITION_ANKLE_RIGHT
	NI_JNT_R_FOOT	= 19;	// NUI_SKELETON_POSITION_FOOT_RIGHT

	NI_JNT_L_FINGER	= -1;
	NI_JNT_R_FINGER	= -1;
	NI_JNT_FACE		= -1;

	return;
}






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

//
// フレームデータの格納先メモリ（ジョイント数可変）を作成，初期化
//
NiFrameData*  jbxwl::makeVarFrameData(int frame, int* joint_num, int* frame_num)
{
	if (joint_num==NULL) return NULL;

	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];
		}
		else {
			frmdata[i].frmn = i;
			frmdata[i].msec = i;
		}
		frmdata[i].jnum = joint_num[i];

		if (frmdata[i].jnum>0) {
			frmdata[i].jdat = (NiJointData*)malloc(frmdata[i].jnum*sizeof(NiJointData));
			if (frmdata[i].jdat==NULL) {
				freeFrameData(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();
				frmdata[i].jdat[j].quat.init();
			}
		}
	}

	return frmdata;
}



//
// フレームデータの格納先メモリ（ジョイント数固定）を作成，初期化
//
NiFrameData*  jbxwl::makeFixFrameData(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::freeFrameData(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;
}






//////////////////////////////////////////////////////////////////////////////////////////////////
//
// CJTextTool Class
//

void  CJTextTool::clearData(void)
{
	if (jtxt_joints!=NULL) ::freeFrameData(jtxt_joints, jtxt_frmnum);

	jtxt_joints = NULL;
	jtxt_frmnum = 0;
}



void  CJTextTool::init(void)
{
	jtxt_joints = NULL;
	jtxt_frmnum = 0;

	frameData   = frame_data;

	clearJointsData();
}



void  CJTextTool::clearJointsData(double val)
{
//	clearStartPosition();

	for (int i=0; i<NI_JOINT_NUM; i++) {
		posVect[i].init(val);
		rotQuat[i].init(val);
	}
}




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

BOOL  CJTextTool::readFile(FILE* fp)
{
	if (fp==NULL) return FALSE;

	Buffer buf = make_Buffer(LDATA);	// Line 読み込み用バッファ

	// ファイルヘッダのチェック
	fgets_Buffer(&buf, fp);
	if (strncmp(NI_JTXT_FILE_ID, (char*)buf.buf, strlen(NI_JTXT_FILE_ID))) {
		free_Buffer(&buf);
		return FALSE;
	}

	// フレーム数を数える
	int frame = 0;
	fgets_Buffer(&buf, fp);
	while(!feof(fp)) {
		if (buf.buf[0]>='0' && buf.buf[0]<='9') frame++; // 行頭が数字の場合はフレーム時間
		fgets_Buffer(&buf, fp);
	}
	if (frame==0) {
		free_Buffer(&buf);
		return FALSE;
	}


	// 各フレームのジョイントの数を調べる
	int* frame_num = (int*)malloc(frame*sizeof(int));
	int* joint_num = (int*)malloc(frame*sizeof(int));
	memset(frame_num, 0, frame*sizeof(int));
	memset(joint_num, 0, frame*sizeof(int));

	fseek(fp, 0, SEEK_SET);
	fgets_Buffer(&buf, fp);
	// 最初のフレーム時間行まで移動
	while (buf.buf[0]<'0' || buf.buf[0]>'9') fgets_Buffer(&buf, fp);

	for (int i=0; i<frame; i++) {
		frame_num[i] = atoi((char*)buf.buf);

		int joints = 0;
		fgets_Buffer(&buf, fp);
		while (!feof(fp) && (buf.buf[0]<'0' || buf.buf[0]>'9')) {
			if (buf.buf[0]==' ') joints++;
			fgets_Buffer(&buf, fp);
		}
		joint_num[i] = joints;
	}


	// Frame Data の格納先を作る
	jtxt_joints = makeVarFrameData(frame, joint_num, frame_num);

	::free(frame_num);
	::free(joint_num);
	if (jtxt_joints==NULL) {
		free_Buffer(&buf);
		return FALSE;
	}
	jtxt_frmnum = frame;


	//
	// データの読み込み
	//
	int prvms = 0;
	fseek(fp, 0, SEEK_SET);
	fgets_Buffer(&buf, fp);

	// 最初のフレーム時間行まで移動
	while (buf.buf[0]<'0' || buf.buf[0]>'9') fgets_Buffer(&buf, fp);

	char jname[L_ID];
	memset(jname, 0, L_ID);

	for (unsigned int i=0; i<jtxt_frmnum; i++) {
		int nowms = jtxt_joints[i].msec;
		if (i==0) prvms = nowms;
		int msec = nowms - prvms;
		if (msec<0) msec += 60000;	// + 60sec
		prvms = nowms;

		if (i==0) jtxt_joints[0].msec = 0;
		else      jtxt_joints[i].msec = jtxt_joints[i-1].msec + msec;
		jtxt_joints[i].frmn = jtxt_joints[i].msec;

		//
		float px, py, pz;
		float qx, qy, qz, qs;
		NiJointData* jdat = jtxt_joints[i].jdat;
		for (int j=0; j<jtxt_joints[i].jnum; j++) {
			fgets_Buffer(&buf, fp);
			while (buf.buf[0]!=' ') fgets_Buffer(&buf, fp);

			//
			sscanf((char*)buf.buf, "%s %f %f %f %f %f %f %f", jname, &px, &py, &pz, &qx, &qy, &qz, &qs);
			jdat[j].vect.set((double)px, (double)py, (double)pz);
			jdat[j].quat.set((double)qs, (double)qx, (double)qy, (double)qz);
			jdat[j].joint = NiJointNum(jname);
			jdat[j].index = i;
		}
	}

 	free_Buffer(&buf);

	return TRUE;
}




void  CJTextTool::writeData(FILE* fp, unsigned short msec)
{
	if (fp==NULL) return;

	fprintf(fp, "%d\n", msec);

	for (int j=0; j<NI_JOINT_NUM; j++) {
		std::string jn = NiJointName(j);
		fprintf(fp, " %-10s %11.6f %11.6f %11.6f    ",  jn.c_str(), posVect[j].x, posVect[j].y, posVect[j].z);
		fprintf(fp, "%11.8f %11.8f %11.8f %11.8f\n",  rotQuat[j].x, rotQuat[j].y, rotQuat[j].z, rotQuat[j].s);
	}

	return;
}




void  CJTextTool::setPosVect(Vector<double>* pos, NiSDK_Lib lib, BOOL mirror)
{
	for (int j=0; j<NI_JOINT_NUM; j++) {
		int n = Ni2SDKJointNum(j, lib);
		if (mirror) n = NiSDKMirrorJointNum(n, lib);

		if (n>=0) {
			posVect[j] = pos[n];
			if (mirror) posVect[j].y = -posVect[j].y;
		}
		else {
			posVect[j].init(-1.0);
		}
	}

	return;
}



void  CJTextTool::setRotQuat(Quaternion* rot, NiSDK_Lib lib, BOOL mirror)
{
	for (int j=0; j<NI_JOINT_NUM; j++) {
		int n = Ni2SDKJointNum(j, lib);
		if (mirror) n = NiSDKMirrorJointNum(n, lib);

		if (n>=0) {
			rotQuat[j] = rot[n];
			if (mirror) {
				rotQuat[j].x = -rotQuat[j].x;
				rotQuat[j].z = -rotQuat[j].z;
			}
		}
		else {
			rotQuat[j].init(-1.0);
		}
	}

	return;
}





NiJointData*  CJTextTool::getFrameData(int frmnum) 
{ 
	for (int j=0; j<NI_JOINT_NUM; j++) {
		frameData[j].joint = -1;
		frameData[j].index = -1;
		frameData[j].vect.init();
		frameData[j].quat.init();
	}
	
	return frameData;
}