#include "InertialTrajProcess.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;
}