/// <summary>
/// 旋转的四元数表示转换为轴角表示
/// </summary>
/// <param name="Q">输入旋转的四元数表示</param>
/// <returns>返回旋转的轴角表示</returns>
public static double[] Quatnum2AxisAngle(Quatnum Q)
{
double[] QData = Q.GetData();
if (Math.Abs(QData[0]) > 1.0) // 限幅到-1到+1之内
{
QData[0] = (Math.Sign(QData[0]) == 1) ? 1.0 : -1.0;
}
if (QData[0] == 1 || QData[0] == -1) // 旋转整周，相当于不转
{
return(new double[] { 0.0, 0.0, 0.0 });
}
else
{
double angle = Math.Acos(QData[0]); // 返回在0到pi之间
double[] axis = { QData[1] / Math.Sin(angle), QData[2] / Math.Sin(angle), QData[3] / Math.Sin(angle) };
angle *= 2.0; // 返回在0到2pi之间
if (angle >= Math.PI)
{
angle = -(2.0 * Math.PI - angle);
} // 返回在-pi到pi之间
return(new double[] { axis[0] * angle, axis[1] * angle, axis[2] * angle });
}
}
/// <summary>
/// 四元数相乘，Q2 * Q1
/// </summary>
/// <param name="Q2">四元数q2</param>
/// <param name="Q1">四元数q1</param>
/// <returns>返回相乘结果</returns>
public static Quatnum QuatnumMutiply(Quatnum Q2, Quatnum Q1)
{
double[] Q3 = new double[4];
double[] q2Data = Q2.GetData();
double[] q1Data = Q1.GetData();
Q3[0] = q2Data[0] * q1Data[0] - q2Data[1] * q1Data[1] - q2Data[2] * q1Data[2] - q2Data[3] * q1Data[3];
Q3[1] = q2Data[1] * q1Data[0] + q2Data[0] * q1Data[1] - q2Data[3] * q1Data[2] + q2Data[2] * q1Data[3];
Q3[2] = q2Data[2] * q1Data[0] + q2Data[3] * q1Data[1] + q2Data[0] * q1Data[2] - q2Data[1] * q1Data[3];
Q3[3] = q2Data[3] * q1Data[0] - q2Data[2] * q1Data[1] + q2Data[1] * q1Data[2] + q2Data[0] * q1Data[3];
return(new Quatnum(Q3));
}
/// <summary>
/// 寻找两个四元数之间的过渡四元数，即
/// Q2 = Qt * Q1，已知Q1，Q2，求Qt
/// </summary>
/// <param name="Q1">初始四元数</param>
/// <param name="Q2">终态四元数</param>
/// <returns>返回过渡四元数</returns>
public static Quatnum FindTransitQuatnum(Quatnum Q1, Quatnum Q2)
{
double[] Q1Data = Q1.GetData();
double[] Q2Data = Q2.GetData();
double[,] A = { { Q1Data[0], -Q1Data[1], -Q1Data[2], -Q1Data[3] },
{ Q1Data[1], Q1Data[0], Q1Data[3], -Q1Data[2] },
{ Q1Data[2], -Q1Data[3], Q1Data[0], Q1Data[1] },
{ Q1Data[3], Q1Data[2], -Q1Data[1], Q1Data[0] } };
return(new Quatnum(SolveEqu4Self(A, Q2Data)));
}
/// <summary>
/// 四元数求逆
/// </summary>
/// <param name="Q">带求逆的四元数</param>
/// <returns>返回求逆结果</returns>
public static Quatnum InvQuatnum(Quatnum Q)
{
double[] QData = Q.GetData();
return(new Quatnum(new double[] { QData[0], -QData[1], -QData[2], -QData[3] }));
}
/// <summary>
/// 向量的四元数表示转化为向量的一般表示
/// </summary>
/// <param name="Q">输入向量的四元数表示</param>
/// <returns>返回向量的一般表示</returns>
public static double[] Quatnum2Array(Quatnum Q)
{
double[] QData = Q.GetData();
return(new double[] { QData[1], QData[2], QData[3] });
}
