#include "InertialTrajProcess.h"
#include <math.h>


void InertialTrajProcess::TrajRotate(int *pos, float rotateMatrix[4])
{
	/*
		������Ϊ����������ĺ���ת�Ƶ��Զ����X����Ь�ӳ���
	*/
	float pos_f[3];

	pos_f[2] = float(pos[2]) * 0.01f;
	pos_f[1] = float(pos[1]) * 0.01f;
	pos_f[0] = float(pos[0]) * 0.01f;

	float posTemp[3];

	posTemp[2] = pos_f[2];

	posTemp[0] = rotateMatrix[0] * float(pos_f[0]) + rotateMatrix[1] * float(pos_f[1]);
	posTemp[1] = rotateMatrix[2] * float(pos_f[0]) + rotateMatrix[3] * float(pos_f[1]);


	pos[0] = (int)(posTemp[0] * 100.0f);
	pos[1] = (int)(posTemp[1] * 100.0f);
	pos[2] = (int)(posTemp[2] * 100.0f);
}
void InertialTrajProcess::TrajRotate(int* pos)
{
	/*
		������Ϊ����������ĺ���ת�Ƶ�Ӳ����ָ��X����Ь�ӳ���
	*/
	float pos_f[3];

	pos_f[2] = float(pos[2]) * 0.01f;
	pos_f[1] = float(pos[1]) * 0.01f;
	pos_f[0] = float(pos[0]) * 0.01f;

	float posTemp[3];

	posTemp[2] = pos_f[2];

	posTemp[0] = cos(curheading) * float(pos_f[0]) + sin(curheading) * float(pos_f[1]);
	posTemp[1] = -sin(curheading) * float(pos_f[0]) + cos(curheading) * float(pos_f[1]);

	
	pos[0] = (int)(posTemp[0] * 100.0f);
	pos[1] = (int)(posTemp[1] * 100.0f);
	pos[2] = (int)(posTemp[2] * 100.0f);

	//std::cout << "DanceGame:Rotate : " << pos[0] << " " << pos[1] << " " << pos[2] << std::endl;
}

void InertialTrajProcess::ResetHeading(int heading)
{
	/*
		����
	*/
	curheading = (float)(heading  * 0.0001f);
}

InertialTrajProcess::InertialTrajProcess()
{
	memset(shoesPos, 0, 3 * sizeof(int));
	
	curheading = 0.0f;
}