/// <summary>
/// 已知第一个坐标系到第二个坐标系的转换，获得第二个坐标系到第一个坐标系的转换
/// </summary>
/// <param name="Relationship">第一个坐标系到第二个坐标系的转换</param>
/// <returns>返回第二个坐标系到第一个坐标系的转换</returns>
public static double[] ReverseReferenceRelationship(double[] Relationship)
{
double[] position = { Relationship[0], Relationship[1], Relationship[2] };
double[] posture = { Relationship[3], Relationship[4], Relationship[5] };
Quatnum qPosture = AxisAngle2Quatnum(posture);
double positionLength = LengthOfArray(position);
double[] positionDirection;
if (positionLength == 0)
{
positionDirection = new double[3] {
0.0, 0.0, 0.0
};
}
else
{
positionDirection = new double[3] {
position[0] / positionLength, position[1] / positionLength, position[2] / positionLength
};
}
double[] reversePositionDirection = FindDirectionToSecondReferenceFromFirstReference(positionDirection, qPosture);
double[] reversePosition = { -reversePositionDirection[0] * positionLength, -reversePositionDirection[1] * positionLength, -reversePositionDirection[2] * positionLength };
double[] reversePosture = Quatnum2AxisAngle(InvQuatnum(qPosture));
double[] reverseRelationship = { reversePosition[0], reversePosition[1], reversePosition[2],
reversePosture[0], reversePosture[1], reversePosture[2] };
return(reverseRelationship);
}
/// <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>
/// 将向量的方向在第一个坐标系中的表示转换到在第二个坐标系中的表示
/// </summary>
/// <param name="DirectionToFirstReference">在第一个坐标系中的向量方向表示</param>
/// <param name="PostureFromFirstToSecondReference">第一个坐标系到第二个坐标系的姿态转换关系</param>
/// <returns>返回在第二个坐标系中的向量方向表示</returns>
public static double[] FindDirectionToSecondReferenceFromFirstReference(double[] DirectionToFirstReference, Quatnum PostureFromFirstToSecondReference)
{
Quatnum qDirection1 = Array2Quatnum(DirectionToFirstReference);
Quatnum qDirection2 = RotateAlongAxis(InvQuatnum(PostureFromFirstToSecondReference), qDirection1);
double[] directionToSecondReference = Quatnum2Array(qDirection2);
return(directionToSecondReference);
}
/// <summary>
/// 获得第三个坐标系相对于第一个坐标系的位姿，中间隔着第二个坐标系
/// </summary>
/// <param name="SecondReferenceToFirstReference">第二个坐标系相对于第一个坐标系的位姿</param>
/// <param name="TransformFromSecondReferenceToThirdReference">第二个坐标系转换到第三个坐标系的位姿关系</param>
/// <returns>返回第三个坐标系相对于第一个坐标系的位姿</returns>
public static double[] FindThirdReferenceToFirstReference(double[] SecondReferenceToFirstReference, double[] TransformFromSecondReferenceToThirdReference)
{
// A-->B-->C
double[] positionAToB = { SecondReferenceToFirstReference[0], SecondReferenceToFirstReference[1], SecondReferenceToFirstReference[2] };
double[] postureAToB = { SecondReferenceToFirstReference[3], SecondReferenceToFirstReference[4], SecondReferenceToFirstReference[5] };
Quatnum qPostureAToB = AxisAngle2Quatnum(postureAToB);
double[] positionBToC = { TransformFromSecondReferenceToThirdReference[0], TransformFromSecondReferenceToThirdReference[1], TransformFromSecondReferenceToThirdReference[2] };
double positionBToCLength = LengthOfArray(positionBToC);
double[] positionBToCDirection;
if (positionBToCLength == 0)
{
positionBToCDirection = new double[3] {
0.0, 0.0, 0.0
};
}
else
{
positionBToCDirection = new double[3] {
positionBToC[0] / positionBToCLength, positionBToC[1] / positionBToCLength, positionBToC[2] / positionBToCLength
};
}
double[] positionBToCDirectionToA = FindDirectionToSecondReferenceFromFirstReference(positionBToCDirection, InvQuatnum(qPostureAToB));
double[] positionAToC = { positionAToB[0] + positionBToCDirectionToA[0] * positionBToCLength, positionAToB[1] + positionBToCDirectionToA[1] * positionBToCLength, positionAToB[2] + positionBToCDirectionToA[2] * positionBToCLength };
double[] postureBToC = { TransformFromSecondReferenceToThirdReference[3], TransformFromSecondReferenceToThirdReference[4], TransformFromSecondReferenceToThirdReference[5] };
double postureBToCAngle = LengthOfArray(postureBToC);
double[] postureBToCDirection;
if (postureBToCAngle == 0)
{
postureBToCDirection = new double[3] {
0.0, 0.0, 0.0
};
}
else
{
postureBToCDirection = new double[3] {
postureBToC[0] / postureBToCAngle, postureBToC[1] / postureBToCAngle, postureBToC[2] / postureBToCAngle
};
}
double[] postureBToCDirectionToA = FindDirectionToSecondReferenceFromFirstReference(postureBToCDirection, InvQuatnum(qPostureAToB));
double[] postureBToCToA = { postureBToCDirectionToA[0] * postureBToCAngle, postureBToCDirectionToA[1] * postureBToCAngle, postureBToCDirectionToA[2] * postureBToCAngle };
Quatnum qPostureBToCToA = AxisAngle2Quatnum(postureBToCToA);
Quatnum qPostureAToC = QuatnumRotate(new Quatnum[] { qPostureAToB, qPostureBToCToA });
double[] postureAToC = Quatnum2AxisAngle(qPostureAToC);
double[] thirdReferenceToFirstReference = { positionAToC[0], positionAToC[1], positionAToC[2],
postureAToC[0], postureAToC[1], postureAToC[2] };
return(thirdReferenceToFirstReference);
}
/// <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>
/// 依次绕轴旋转的旋转矩阵用四元数表示，
/// QMutiplyAxis[end] * ... * QMutiplyAxis[1] * QMutiplyAxis[0]
/// </summary>
/// <param name="QMutiplyAxis">四元数数组，
/// 元素依次旋转，次序即排序</param>
/// <returns>返回多次旋转叠加结果</returns>
public static Quatnum QuatnumRotate(Quatnum[] QMutiplyAxis)
{
int rotateLength = QMutiplyAxis.Length;
if (rotateLength > 2)
{
Quatnum[] QFrontMutiplyAxis = new Quatnum[rotateLength - 1];
for (int k = 0; k < rotateLength - 1; k++)
{
QFrontMutiplyAxis[k] = QMutiplyAxis[k];
}
return(QuatnumMutiply(QMutiplyAxis[rotateLength - 1], QuatnumRotate(QFrontMutiplyAxis)));
}
else if (rotateLength == 2)
{
return(QuatnumMutiply(QMutiplyAxis[1], QMutiplyAxis[0]));
}
else
{
return(new Quatnum(new double[] { 1.0, 0.0, 0.0, 0.0 }));
}
}
/// <summary>
/// 将力或者位姿在第一个坐标系中的表示转换到在第二个坐标系中的表示
/// 如果被转换对象是力，则输入力的格式为[力大小(1) 力方向(3) 力矩大小(1) 力矩方向(3)]
/// 如果被转换对象是位姿，则输入位姿的格式为[位置(3) 姿态轴角表示(3)]
/// 小括号内的数字表示该变量的维度，即力为8维向量，位姿为6维向量
/// </summary>
/// <param name="CordinateToFirstReference">在第一个坐标系中的坐标表示</param>
/// <param name="TransformFromFirstToSecondReference">第一个坐标系到第二个坐标系的转换关系</param>
/// <returns>返回在第二个坐标系中的坐标表示</returns>
public static double[] FindCordinateToSecondReferenceFromFirstReference(double[] CordinateToFirstReference, double[] TransformFromFirstToSecondReference)
{
int cordinateLength = CordinateToFirstReference.Length;
if (cordinateLength == 8) // 力信号转换
{
Quatnum qPosture1To2 = AxisAngle2Quatnum(new double[] { TransformFromFirstToSecondReference[3], TransformFromFirstToSecondReference[4], TransformFromFirstToSecondReference[5] });
double forceAmplitude = CordinateToFirstReference[0];
double[] forceDirection1 = new double[] { CordinateToFirstReference[1], CordinateToFirstReference[2], CordinateToFirstReference[3] };
double[] forceDirection2 = FindDirectionToSecondReferenceFromFirstReference(forceDirection1, qPosture1To2);
double torqueAmplitude = CordinateToFirstReference[4];
double[] torqueDirection1 = new double[] { CordinateToFirstReference[5], CordinateToFirstReference[6], CordinateToFirstReference[7] };
double[] torqueDirection2 = FindDirectionToSecondReferenceFromFirstReference(torqueDirection1, qPosture1To2);
double[] cordinateToSecondReference = { forceAmplitude *forceDirection2[0], forceAmplitude *forceDirection2[1], forceAmplitude *forceDirection2[2],
torqueAmplitude *torqueDirection2[0], torqueAmplitude *torqueDirection2[1], torqueAmplitude *torqueDirection2[2] };
return(cordinateToSecondReference);
}
else if (cordinateLength == 6) // 坐标信号转换
{
double[] position1To2 = { TransformFromFirstToSecondReference[0], TransformFromFirstToSecondReference[1], TransformFromFirstToSecondReference[2] };
Quatnum qPosture1To2 = AxisAngle2Quatnum(new double[] { TransformFromFirstToSecondReference[3], TransformFromFirstToSecondReference[4], TransformFromFirstToSecondReference[5] });
double[] position1 = { CordinateToFirstReference[0], CordinateToFirstReference[1], CordinateToFirstReference[2] };
double positionLength = LengthOfArray(position1);
double[] positionDirection1;
if (positionLength == 0)
{
positionDirection1 = new double[3] {
0.0, 0.0, 0.0
};
}
else
{
positionDirection1 = new double[3] {
position1[0] / positionLength, position1[1] / positionLength, position1[2] / positionLength
};
}
double[] positionDirection2 = FindDirectionToSecondReferenceFromFirstReference(positionDirection1, qPosture1To2);
double position1To2Length = LengthOfArray(position1To2);
double[] position1To2Direction1;
if (position1To2Length == 0)
{
position1To2Direction1 = new double[3] {
0.0, 0.0, 0.0
};
}
else
{
position1To2Direction1 = new double[3] {
position1To2[0] / position1To2Length, position1To2[1] / position1To2Length, position1To2[2] / position1To2Length
};
}
double[] position1To2Direction2 = FindDirectionToSecondReferenceFromFirstReference(position1To2Direction1, qPosture1To2);
double[] position2 = { -position1To2Direction2[0] * position1To2Length + positionDirection2[0] * positionLength, -position1To2Direction2[1] * position1To2Length + positionDirection2[1] * positionLength, -position1To2Direction2[2] * position1To2Length + positionDirection2[2] * positionLength };
double[] posture1 = { CordinateToFirstReference[3], CordinateToFirstReference[4], CordinateToFirstReference[5] };
double posture1Angle = LengthOfArray(posture1);
double[] posture1Direction;
if (posture1Angle == 0)
{
posture1Direction = new double[3] {
0.0, 0.0, 0.0
};
}
else
{
posture1Direction = new double[3] {
posture1[0] / posture1Angle, posture1[1] / posture1Angle, posture1[2] / posture1Angle
};
}
double[] posture1Direction2 = FindDirectionToSecondReferenceFromFirstReference(posture1Direction, qPosture1To2);
double[] posture12 = { posture1Direction2[0] * posture1Angle, posture1Direction2[1] * posture1Angle, posture1Direction2[2] * posture1Angle };
Quatnum qPosture12 = AxisAngle2Quatnum(posture12);
Quatnum qPosture2 = QuatnumRotate(new Quatnum[] { InvQuatnum(qPosture1To2), qPosture12 });
double[] posture2 = Quatnum2AxisAngle(qPosture2);
double[] cordinateToSecondReference = { position2[0], position2[1], position2[2],
posture2[0], posture2[1], posture2[2] };
return(cordinateToSecondReference);
}
else // 转换输入出错
{
return(null);
}
}
/// <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] });
}
/// <summary>
/// 向量绕轴旋转一定角度后的结果用四元数表示，
/// QRotateAxis * QBaseArray * QRotateAxis ^ (-1)
/// </summary>
/// <param name="QRotateAxis">表示旋转的四元数</param>
/// <param name="QBaseArray">待旋转的向量四元数表示</param>
/// <returns>返回旋转后得到的向量四元数表示</returns>
public static Quatnum RotateAlongAxis(Quatnum QRotateAxis, Quatnum QBaseArray)
{
return(QuatnumMutiply(QuatnumMutiply(QRotateAxis, QBaseArray), InvQuatnum(QRotateAxis)));
}
