四元数-坐标系转换-旋转-转欧拉角

1.四元数世界坐标系转换到父节点坐标系下
公式：Q=Q父-1*Q子

//把四元数转到父坐标系下
ChildLocalQuat VDFULL::GetChildLocalQuat(quaternion father,quaternion child)
{
	quaternion invq = father.inv();		// 求出father的逆
	quaternion child_loc = invq * child;	
	auto ChildLocal = child_loc.get_vector4();
	ChildLocal_w = std::get<0>(ChildLocal);
	ChildLocal_x = std::get<1>(ChildLocal);
	ChildLocal_y = std::get<2>(ChildLocal);
	ChildLocal_z = std::get<3>(ChildLocal);
	return ChildLocalQuat(ChildLocal_w, ChildLocal_x, ChildLocal_y, ChildLocal_z);
}

2.四元数旋转固定角度-左乘和右乘
公式：定义Q1(cosθ/2,sinθ/2*v)----v代表旋转轴，然后直接乘要旋转的四元数即可       
注意：左乘和右乘结果是不同的,当然绕xyz轴单独旋转时左乘和右乘结果是相等的
         Q1(a,b,c,d);Q2(w,x,y,z);
         Q1*Q2=(aw-bx-cy-dz)+(ax+bw+cz-dy)i+(ay-bz+cw+dx)j+(az+by-cx+dw)k
         Q2*Q1=(aw-bx-cy-dz)+(ax+bw-cz+dy)i+(ay+bz+cw-dx)j+(az-by+cx+dw)k

//右乘
ChildLocalQuat VDFULL::GetChildLocalQuat_L(quaternion father,quaternion child)
{
	quaternion r90(cos(θ/2), i*sin(θ/2),j*sin(θ/2),k*sin(θ/2));//ijk表示旋转轴，转多少度可以自定义
	quaternion quat;
	quaternion r90_Child = child * r90;  //Q1*Q2  
	//quaternion invq = father.inv();		// 求出father的逆
	//quaternion child_q = invq * r90_Child ;

	auto ChildLocal = child_q.get_vector4();
	ChildLocal_w = std::get<0>(ChildLocal);
	ChildLocal_x = std::get<1>(ChildLocal);
	ChildLocal_y = std::get<2>(ChildLocal);
	ChildLocal_z = std::get<3>(ChildLocal);
	
	return ChildLocalQuat(ChildLocal_w, ChildLocal_x, ChildLocal_y, ChildLocal_z);
}
//左乘
ChildLocalQuat VDFULL::GetChildLocalQuat_RA(quaternion father,quaternion child)
{
	quaternion r90(cos(θ/2), i*sin(θ/2),j*sin(θ/2),k*sin(θ/2));//ijk表示旋转轴，转多少度可以自定义
	quaternion r90_Child = quat.L_multi(r90, child); //Q2*Q1 
	//quaternion invq = father.inv();		// 求出father的逆
	//quaternion child_q = invq * r90_Child;

	auto ChildLocal = child_q.get_vector4();
	ChildLocal_w = std::get<0>(ChildLocal);
	ChildLocal_x = std::get<1>(ChildLocal);
	ChildLocal_y = std::get<2>(ChildLocal);
	ChildLocal_z = std::get<3>(ChildLocal);
	return ChildLocalQuat(ChildLocal_w, ChildLocal_x, ChildLocal_y, ChildLocal_z);
}

3.四元数转欧拉角
四元数转欧拉角需要注意旋转次序（下面转换给出ZYX和XYZ）。

需要注意arcsin的取值范围

mT_Rotation VDFULL::QuaternionToEuler(double q1, double q2, double q3, double q4)
{
    //四元数转ZYX
	double roll, yaw, pitch;
	double pitch_max = 2 * (q2 * q4 - q1 * q3);

	if (pitch_max > 1) { pitch_max = 1; }
	else if (pitch_max < -1) { pitch_max = -1; }

	yaw = atan2(2 * (q2 * q3 + q1 * q4), (q1 * q1 + q2 * q2 - q3 * q3 - q4 * q4)) * 180 / pi;  
	pitch = asin(-pitch_max) * 180 / pi;               
	roll = atan2(2 * (q3 * q4 + q1 * q2), (q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4)) * 180 / pi;  

	return mT_Position(yaw,-pitch , -roll);   //这里的正负号是根据实际情况做了一些坐标系转换

	//四元数转XYZ
	//double pitch_max = 2 * (q2 * q4 + q1 * q3);
	//if (pitch_max > 1) { pitch_max = 1; }
	//else if (pitch_max < -1) { pitch_max = -1; }

	//roll = atan2(-2 * (q3 * q4 - q1 * q2), q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4) * 180 / pi; // roll
	//pitch = asin(pitch_max) * 180 / pi;                // pitch
	//yaw = atan2(-2 * (q2 * q3 - q1 * q4), q1 * q1 + q2 * q2 - q3 * q3 - q4 * q4) * 180 / pi; // yaw
	//return mT_Position(pitch, yaw, -roll);   //这里的正负号是根据实际情况做了一些坐标系转换

}

大概就总结这么多，记录一下。未经允许，禁止转载。