旋转矢量（法向量）转化欧拉角

应用：点云的法向量转化为机械臂的欧拉角

原理：

 code：

//姿态转化欧拉角
double*  posture_solve(double *machine,int &n) {
	vector<double> machine_temp;
	for (int i = 0; i < n; i += 6) {
		point3d temp;
		temp.x = machine[i]; 
		temp.y = machine[i + 1]; 
		temp.z = machine[i + 2];
		tuchuang::TCRotationMatrix33 mat;//旋转矩阵

		mat.a02 = -machine[i + 3];          
		mat.a12 = -machine[i + 4];
		mat.a22 = -machine[i + 5]; //z

		mat.a01 = machine[i] - machine[(i + 6) % n];
		mat.a11 = machine[i + 1] - machine[(i + 1 + 6) % n];
		mat.a21 = machine[i + 2] - machine[(i + 2 + 6) % n]; //y
		

		mat.a00 = mat.a11*mat.a22 - mat.a21*mat.a12;
		mat.a10 = mat.a21*mat.a02 - mat.a01*mat.a22;
		mat.a20 = mat.a01*mat.a12 - mat.a11*mat.a02;//x
		double flag = 1.0;
		if (fabs(mat.a20) <= 1e-5&&fabs(mat.a21) <= 1e-5&&fabs(mat.a22 - 1.0) <= 1e-5) {
			flag = -1.0;
		}
		temp.b = atan2(flag*sqrt(mat.a20*mat.a20 + mat.a21*mat.a21), mat.a22);
		temp.a = atan2(mat.a12 / sin(temp.b), mat.a02 / sin(temp.b));
		temp.c = atan2(mat.a21 / sin(temp.b), -mat.a20 / sin(temp.b));
		machine[i + 3] = temp.a;
		machine[i + 4] = temp.b;
		machine[i + 5] = temp.c;
		if (temp.b < 2.0) {
			machine_temp.push_back(machine[i]); machine_temp.push_back(machine[i+1]); machine_temp.push_back(machine[i+2]);
			machine_temp.push_back(machine[i+3]); machine_temp.push_back(machine[i+4]); machine_temp.push_back(machine[i+5]);
		
		}
	}
	n = machine_temp.size();
	memcpy(machine, &machine_temp[0], machine_temp.size() * sizeof(double));
	return machine;
}