shoes/shoes_sdk/DanceFoot.cpp

#include "DanceFoot.h"

/*
* 初始化脚步的数据
*/
DanceFoot::DanceFoot(int LEFT_OR_RIGHT)
{
	left_or_right = LEFT_OR_RIGHT;

	foot_rssi = 0;
	foot_zupt = 1;

	rssi_zupt = 1;
	
	max_rssi = 0;
	min_rssi = 100;

	memset(attitude, 0, 3 * sizeof(float));
	memset(globalPos, 0, 3 * sizeof(float));
	memset(offsetPos, 0, 3 * sizeof(float));
	memset(last_pos_g, 0, 2 * sizeof(float));
	memset(pos, 0, 2 * sizeof(float));

	last_pos[0] = 0;  last_pos[1] = 0;

	last_zupt = 0;

	danceTraj[0] = 0; danceTraj[1] = 0;

	rotate_err = 0;

	rotateCor[0] = 1.0; rotateCor[1] = 0.0; rotateCor[2] = 0.0; rotateCor[3] = 1.0;

}

/*
* 设置脚的姿态，当前位置，鞋子上来的位置偏移量, rssi, zupt变量
*/

int DanceFoot::setAttitude(float  att[3])
{
	memcpy(attitude, att, 3 * sizeof(float));

	return 0;
}

int DanceFoot::setPosGlobal(float pos[3])
{
	memcpy(globalPos, pos, 3 * sizeof(float));

	return 0;
}

int DanceFoot::setPosOffset(float pos[3])
{
	memcpy(offsetPos, pos, 3 * sizeof(float));

	return 0;
}

int DanceFoot::setFootZupt(int zupt)
{
	foot_zupt = zupt;

	return 0;
}

int DanceFoot::setFootRssi(int rssi)
{
	foot_rssi = rssi;
	
	return 0;
}

int DanceFoot::setMaxRssi(int rssi)
{
	max_rssi = rssi;

	return 0;
}

int DanceFoot::setMinRssi(int rssi)
{
	min_rssi = rssi;
	
	return 0;
}

/*
*  通过判断rssi的变化范围来确定是否没有位移，
*  原地踏步返回0 ，有明显变化的返回1
*/
int DanceFoot::isMoveRssi(int rssi, int zupt)
{
	int result = 1;

	if (zupt == 0)
	{
		if (rssi > max_rssi)
		{
			setMaxRssi(rssi);
		}

		if (rssi < min_rssi)
		{
			setMinRssi(rssi);
		}
	}
	else
	{
		if (rssi_zupt == 0)
		{
			if (max_rssi - min_rssi < 3)
			{
				result = 0;
			}
		}

		setMaxRssi(0);
		setMinRssi(100);
	}
	
	rssi_zupt = zupt;

	return result;
}
/*
* 处理全局位置的边界问题，全局位置应为40cmX40cm区域
*/
int DanceFoot::posBoundary(float pos[3])
{
	float posLength = sqrt(pos[0] * pos[0] + pos[1] * pos[1]);

	if (posLength > 0.25)
	{
		pos[0] = pos[0] / posLength * 0.25;
		pos[1] = pos[1] / posLength * 0.25;
	}

	return 0;
}

/*
* 通过角度来判断跳舞毯的方向
* 
*/
int DanceFoot::getGameDiretion(float *pos)
{
	float x = pos[0] ;
	float y = pos[1] ;

	if (left_or_right == LEFT_FOOT)
	{
		std::cout << "left_foot ";
	}
	else
	{
		std::cout << "right_foot ";
	}

	if(sqrt(x * x + y * y) < 0.08)
	{
		std::cout << "danceGame result: zeros point. " << std::endl;
		return MOTION_STEP;
	}

	float angle = atan2(y, x);


	if (angle > 0.0f)
	{
		if (angle < 1.5708)
		{
			std::cout << "danceGame result: right up. " << std::endl;
			return MOTION_RIGHT_UP;
		}
		else
		{
			std::cout << "danceGame result: left up. " << std::endl;
			return MOTION_LEFT_UP;
		}
	}
	else
	{
		if (angle > -1.5708)
		{
			std::cout << "danceGame result: right down. " << std::endl;
			return MOTION_RIGHT_DOWN;
		}
		else
		{
			std::cout << "danceGame result: left down. " << std::endl;
			return MOTION_LEFT_DOWN;
		}
	}

	return MOTION_STEP;
}




/*
* 计算鞋子的全局位置
*/

int DanceFoot::calGlobalPos(float pos_g[3], int rssi, int zupt)
{

	int pos_index = -1;

	float pos_zero[2] = { 0, 0 };

	float pos_offset[2] = {0, 0};

	float pos_g_offset[2] = { 0,0 };

	pos_g_offset[0] = pos_g[0] - last_pos_g[0];
	pos_g_offset[1] = pos_g[1] - last_pos_g[1];

	if (zupt == 1)
	{
		if (last_zupt == 1)
		{
			pos_g_offset[0] = 0;
			pos_g_offset[1] = 0;
		}
	}//略去惯导的补偿项，这样是避免补偿过大，导致跳舞的轨迹有拖尾情况

	last_pos_g[0] = pos_g[0];
	last_pos_g[1] = pos_g[1];

	pos[0] += pos_g_offset[0];
	pos[1] += pos_g_offset[1]; 

	if (zupt == 1 && rssi < DANCEGAME_MIN_RSSI)
	{
		setLastPos(pos);

		setDanceTraj(pos_zero);
	}


	pos_offset[0] = pos[0] - last_pos[0];
	pos_offset[1] = pos[1] - last_pos[1];

	if (fabsf(pos_offset[0]) > 10 || fabsf(pos_offset[1]) > 10)
	{
		std::cout << "dancegame receive unnormal pos_offset: " << pos_offset[0] << " " << pos_offset[1] << std::endl;
	}

	if (last_zupt == 1 && zupt == 1)
	{
		data.clear();
		
	}

	posData pos_data;
	pos_data.pos_x = rotateCor[0] * (danceTraj[0] + pos_offset[0]) + rotateCor[1] * (danceTraj[1] + pos_offset[1]);
	pos_data.pos_y = rotateCor[2] * (danceTraj[0] + pos_offset[0]) + rotateCor[3] * (danceTraj[1] + pos_offset[1]);

	pos_data.rssi = rssi;

	data.push_back(pos_data);


	if (zupt == 1 && last_zupt == 0)
	{
		/*
		* 检测rssi的信号变化，为直线特征做准备
		*/
		if (data.size() > 5)
		{
			int rssi_min = data[0].rssi;
			int rssi_max = data[0].rssi;
			int rssi_min_index = 0;

			float pos_x_min = data[0].pos_x;
			float pos_x_max = data[0].pos_x;

			float pos_y_min = data[0].pos_y;
			float pos_y_max = data[0].pos_y;

			int len = data.size();

			for (int i = 0; i < data.size(); i++)
			{
				if (data[i].rssi < rssi_min)
				{
					rssi_min = data[i].rssi;
					rssi_min_index = i;
				}

				if (data[i].rssi > rssi_max)
				{
					rssi_max = data[i].rssi;
				}

				if (data[i].pos_x > pos_x_max)
				{
					pos_x_max = data[i].pos_x;
				}

				if (data[i].pos_x < pos_x_min)
				{
					pos_x_min = data[i].pos_x;
				}

				if (data[i].pos_y > pos_y_max)
				{
					pos_y_max = data[i].pos_y;
				}

				if (data[i].pos_y < pos_y_min)
				{
					pos_y_min = data[i].pos_y;
				}
			}

			if (data.size() > 9 && fabsf(data[0].pos_y - data[len - 1].pos_y) > 0.3 && fabsf(pos_x_max - pos_x_min) < 0.2 && data[0].rssi >= DANCEGAME_MIN_RSSI)
			{
				/////////////////检测到有跨越象限的情况，左脚当然会经过y = 0的时候, 而x = 0.1则为一个经验值

				std::cout << "detect pass special point" << std::endl;

				//计算斜率，最小二乘拟合
				/*
				float slope = calSlope(data);

				float angle = atan(slope);

				if (angle > 0)
				{
					rotate_err = PI / 2 - angle;
				}
				else
				{
					rotate_err = -PI / 2 - angle;
				}

				if (fabsf(rotate_err) < 0.3 && pos_x_max - pos_x_min < 0.2)
				{
					rotate_err = 0.5 * rotate_err;

					std::cout << "cal rotate_err by straight line: " << rotate_err << std::endl;

					float rotateTmp[4] = { 1,0,0,1 };
					memcpy(rotateTmp, rotateCor, 4 * sizeof(float));

					rotateCor[0] = cos(rotate_err) * rotateTmp[0] - sin(rotate_err) * rotateTmp[2];
					rotateCor[1] = cos(rotate_err) * rotateTmp[1] - sin(rotate_err) * rotateTmp[3];

					rotateCor[2] = sin(rotate_err) * rotateTmp[0] + cos(rotate_err) * rotateTmp[2];
					rotateCor[3] = sin(rotate_err) * rotateTmp[1] + cos(rotate_err) * rotateTmp[3];

					if (fabs(rotateCor[0]) > 1 || fabs(rotateCor[1]) > 1 || fabs(rotateCor[2]) > 1 || fabs(rotateCor[3]) > 1)
					{
						std::cout << "appear unnormal val" << std::endl;
					}
					//rotateCor[0] = cos(rotate_err); rotateCor[1] = -sin(rotate_err);

					//rotateCor[2] = sin(rotate_err); rotateCor[3] = cos(rotate_err);
				}
				*/
				float special_offset[2] = {0, 0};

				float mid_x = 0.5 * (data[0].pos_x + data[len - 1].pos_x);

				float mid_y = 0.5 * (data[0].pos_y + data[len - 1].pos_y);

				if (pos_x_max - pos_x_min < 0.2)
				{
					if (left_or_right == LEFT_FOOT)
					{
						special_offset[0] = -0.2 - mid_x;
					}
					else
					{
						special_offset[0] = 0.2 - mid_x;
					}

				}

				special_offset[1] =  - mid_y;
			

				pos_offset[0] = pos_offset[0] +  special_offset[0];

				pos_offset[1] = pos_offset[1] + special_offset[1];

			}

			//检测到横移的情况
			else if (data.size() > 9 && fabs(data[0].pos_x - data[len - 1].pos_x) > 0.3 && data[0].rssi >= DANCEGAME_MIN_RSSI)
			{
				std::cout << "detected horizontal line by pos_x" << std::endl;

				float special_offset[2] = { 0,0 };

				
				float mid_x = 0.5 * (data[0].pos_x + data[len - 1].pos_x);

				float mid_y = 0.5 * (data[0].pos_y + data[len - 1].pos_y);

				if (pos_y_max - pos_y_min < 0.15)
				{
					if (left_or_right == LEFT_FOOT)
					{
						special_offset[0] = -0.05 - mid_x;

					}
					else
					{
						special_offset[0] = 0.05 - mid_x;
					}

					if (data[0].pos_y > 0)
					{
						special_offset[1] = 0.2 - mid_y;
					}
					else
					{
						special_offset[1] = -0.2 - mid_y;
					}

					std::cout << "debug :" << special_offset[0] << " " << special_offset[1] << std::endl;
					pos_offset[0] = pos_offset[0] + special_offset[0];

					pos_offset[1] = pos_offset[1] + special_offset[1];
				}
				else
				{
					special_offset[0] = - mid_x;

					if (rssi_min <= DANCEGAME_MIN_RSSI && data[0].rssi - rssi_min > 10 && data[len - 1].rssi - rssi_min > 10)
						special_offset[1] = - mid_y;
				
					pos_offset[0] = pos_offset[0] + special_offset[0];

					pos_offset[1] = pos_offset[1] + special_offset[1];

					std::cout << "debug2 :" << special_offset[0] << " " << special_offset[1] << std::endl;

				}
	
			}

			else if (sqrt(pos_offset[0] * pos_offset[0] + pos_offset[1] * pos_offset[1]) < 0.1)
			{
				std::cout << "dectect no move by disance!" << std::endl;

				pos_offset[0] = 0.0; pos_offset[1] = 0.0;
			}
		}
		
		
		
		danceTraj[0] = danceTraj[0] + pos_offset[0];
		danceTraj[1] = danceTraj[1] + pos_offset[1];
		/*
		if (data[0].rssi < 30)
		{
			//从原点出发必须可以相信
			//暂时先将rotateCor 设置为单位矩阵
			std::cout << "debug: set rotateCor as eye(2)";
			rotateCor[0] = 1; rotateCor[1] = 0; rotateCor[2] = 0; rotateCor[0] = 1;
		}
		*/
		if (rssi < DANCEGAME_MIN_RSSI || sqrt(danceTraj[0] * danceTraj[0] + danceTraj[1] * danceTraj[1] ) < 0.05)
		{
			danceTraj[0] = 0.0; danceTraj[1] = 0.0;
		}

		setLastPos(pos);

		pos_offset[0] = 0.0; pos_offset[1] = 0.0;

		float dancePos[2];

		dancePos[0] = rotateCor[0] * danceTraj[0] + rotateCor[1] * danceTraj[1];
		dancePos[1] = rotateCor[2] * danceTraj[0] + rotateCor[3] * danceTraj[1];

		pos_index = getGameDiretion(dancePos);
	}

	float dancePos[2];

	dancePos[0] = rotateCor[0] * (danceTraj[0] + pos_offset[0]) + rotateCor[1] * (danceTraj[1] + pos_offset[1]);
	dancePos[1] = rotateCor[2] * (danceTraj[0] + pos_offset[0]) + rotateCor[3] * (danceTraj[1] + pos_offset[1]);

	if (last_zupt == 0)
	{
		if (left_or_right == LEFT_FOOT)
			std::cout << "left ";
		else
			std::cout << "right ";

		std::cout << "dancePos: " << dancePos[0] << " " << dancePos[1] << " " << std::endl;
	}

	setLastZupt(zupt);

	gamePos[0] = dancePos[0];
	gamePos[1] = dancePos[1];
	gamePos[2] = pos_g[2];

	return pos_index;

}

/*
* 打印pos和RSSI的
*/

void DanceFoot::printPosData(vector<posData> data)
{
	for (int i = 0; i < data.size(); i++)
	{
		std::cout << data[i].pos_x << " " <<data[i].pos_y << " "<< data[i].rssi << std::endl;
	}
}

/*
* 通过位移以及RSSI来判断是否为原地踏步
*/

int DanceFoot::isMove(vector<posData> data)
{
	int n = data.size();

	if (n <= 1)
	{
		return 0;
	}

	int rssi_left = data[0].rssi;
	int rssi_right = data[(n-1)].rssi;

	float x = data[0].pos_x - data[(n-1)].pos_x;
	float y = data[0].pos_y - data[(n-1)].pos_y;

	int rssi_min = rssi_left;

	for (int i = 0; i < n; i++)
	{
		if (rssi_min > data[i].rssi)
			rssi_min = data[i].rssi;
	}
		
	if (abs(rssi_right - rssi_left) > 10 && rssi_min > 40
		&& fabsf(x) > 0.1 )
	{
		std::cout << "检测到左右横移" << std::endl;

		if (x < 0)
			return -1;
		else
			return 1;
	}

	return 0;
}

/*
* 设置上一次的last_pos
*/
void DanceFoot::setLastPos(float* lastPos)
{
	last_pos[0] = lastPos[0];
	last_pos[1] = lastPos[1];
}


/*
* 设置上一次的zupt
*/

void DanceFoot::setLastZupt(int lastZupt)
{
	last_zupt = lastZupt;
}

/*
* 设置跳舞轨迹
*/

void DanceFoot::setDanceTraj(float* traj)
{
	danceTraj[0] = traj[0];
	danceTraj[1] = traj[1];
}

/*
* 计算斜率
*/

float DanceFoot::calSlope(vector<posData> data)
{
	int i = 0;
	int len = data.size();

	float x2_sum = 0, x_sum = 0, xy_sum = 0, y_sum = 0;

	float slope = 0.0f;

	for (i = 0; i < len; i++)
	{
		x2_sum += (data[i].pos_x * data[i].pos_x);

		x_sum += (data[i].pos_x);
		
		y_sum += (data[i].pos_y);

		xy_sum += (data[i].pos_x * data[i].pos_y);
	}

	//b = (x2_sum * y_sum - x_sum * xy_sum) / ((len * x2_sum) - x_sum * x_sum);

	slope = (len * xy_sum - x_sum * y_sum) / (len * x2_sum - x_sum * x_sum);

	std::cout << len << " " << x2_sum << " " << x_sum << " " << y_sum << " " << xy_sum << " " << atan(slope) << std::endl;

	return slope;

}


float DanceFoot::getGamePos(int index)
{
	if (index < 0 || index > 2)
		return -1;
	return gamePos[index];
}