当将两个四元数相乘并将得到的旋转应用到我的局部右手坐标系时，我得到了意想不到的结果。 （X指向前方，Y指向右侧，Z指向下方。）
（参见下面的Java SCCE）
所以我试图首先将Z旋转90度（偏航），然后围绕局部X轴（滚动）旋转90度。 我试图通过乘以表示这两个旋转的两个四元数来实现这一点，从结果创建一个旋转矩阵并将其应用于我的坐标系的3个单位向量，但我得到的结果没有意义。 （即它们不代表你应该从这两个旋转得到的坐标系。）
我尝试更改四元数乘法顺序没有帮助（请参阅SCCE主方法中注释掉的代码行）。 我还尝试从全局Y创建第二次旋转的四元数，以模拟它是在第一次旋转后从结果局部坐标系创建的。
作为参考，我也通过应用两个单独的旋转矩阵（按预期工作）来计算结果。
我做错了什么？
import java.text.DecimalFormat;
import java.text.NumberFormat;
public class Quaternion {
public static final double NORMALIZATION_LOWER_TOLERANCE = 1 - 1e-4;
public static final double NORMALIZATION_UPPER_TOLERANCE = 1 + 1e-4;
private double w = 1.0;
private double x = 0.0;
private double y = 0.0;
private double z = 0.0;
public static void main(String[] args) {
Vector3D xVect = new Vector3D(1,0,0);
Vector3D yVect = new Vector3D(0,1,0);
Vector3D zVect = new Vector3D(0,0,1);
System.out.println("Initial Local Coordinate System: X:"+xVect+" / Y:"+yVect+ " / Z:"+zVect);
Quaternion rotZ = new Quaternion(Math.PI/2, zVect); // Yaw +90 deg
Quaternion rotY = new Quaternion(Math.PI/2, yVect); // Yaw +90 deg
Quaternion rotX = new Quaternion(Math.PI/2, xVect); // Then roll +90 deg
Matrix rotationMatrixZ = new Matrix(rotZ);
Vector3D localX = xVect.rotate(rotationMatrixZ);
Vector3D localY = yVect.rotate(rotationMatrixZ);
Vector3D localZ = zVect.rotate(rotationMatrixZ);
System.out.println("New Local Coordinate System after Yaw: X:"+localX+" / Y:"+localY+ " / Z:"+localZ); // Gives expected result
Quaternion localRotX = new Quaternion(Math.PI/2, localX);
Matrix localRotXMatrix = new Matrix(localRotX);
Vector3D rotatedX = localX.rotate(localRotXMatrix);
Vector3D rotatedY = localY.rotate(localRotXMatrix);
Vector3D rotatedZ = localZ.rotate(localRotXMatrix);
System.out.println("New Local Coordinate System two local rotations: X:"+rotatedX+" / Y:"+rotatedY+ " / Z:"+rotatedZ); // Gives expected result
Quaternion rotZX = rotZ.multiply(rotX);
// Quaternion rotZX = rotX.multiply(rotZ); // Tried both orders
// Quaternion rotZX = rotZ.multiply(rotY); // rotY is in fact the local rotX
// Quaternion rotZX = rotZ.multiply(rotY); // rotY is in fact the local rotX, tried both orders
rotZX.normalizeIfNeeded();
Matrix rotationXMatrixZX = new Matrix(rotZX);
rotatedX = xVect.rotate(rotationXMatrixZX);
rotatedY = localY.rotate(rotationXMatrixZX);
rotatedZ = localZ.rotate(rotationXMatrixZX);
System.out.println("New Local Coordinate System Quaternion Multiplication: X:"+rotatedX+" / Y:"+rotatedY+ " / Z:"+rotatedZ); // Expect same as above
}
public Quaternion() {
}
public Quaternion(double w, double x, double y, double z) {
this.w = w;
this.x = x;
this.y = y;
this.z = z;
}
public Quaternion(double angle, Vector3D vector){
double halfAngle = angle / 2;
double sin = Math.sin(halfAngle);
this.w = Math.cos(halfAngle);
this.x = vector.getX()*sin;
this.y = vector.getY()*sin;
this.z = vector.getZ()*sin;
}
public boolean normalizeIfNeeded() {
double sum = w * w + x * x + y * y + z * z;
if (NORMALIZATION_LOWER_TOLERANCE < sum && sum < NORMALIZATION_UPPER_TOLERANCE) {
return false;
}
double magnitude = Math.sqrt(sum);
w /= magnitude;
x /= magnitude;
y /= magnitude;
z /= magnitude;
return true;
}
public Quaternion multiply(Quaternion q2) {
Quaternion result = new Quaternion();
result.w = w * q2.w - x * q2.x - y * q2.y - z * q2.z;
result.x = w * q2.x + x * q2.w + y * q2.z - z * q2.y;
result.y = w * q2.y - x * q2.z + y * q2.w + z * q2.x;
result.z = w * q2.z + x * q2.y - y * q2.x + z * q2.w;
return result;
}
public Quaternion conjugate() {
return new Quaternion(w, -x, -y, -z);
}
public double getW() {
return w;
}
public double getX() {
return x;
}
public double getY() {
return y;
}
public double getZ() {
return z;
}
@Override
public String toString() {
return "Quaternion [w=" + w + ", x=" + x + ", y=" + y + ", z=" + z + "]";
}
static class Vector3D {
double x=0;
double y=0;
double z=0;
public Vector3D(double x, double y, double z) {
this.x = x;
this.y = y;
this.z = z;
}
public Vector3D rotate(Matrix rotationMatrix){
return rotationMatrix.multiply(this);
}
public double getX() {
return x;
}
public double getY() {
return y;
}
public double getZ() {
return z;
}
@Override
public String toString() {
NumberFormat df = DecimalFormat.getNumberInstance();
return "[x=" + df.format(x) + ", y=" + df.format(y) + ", z=" + df.format(z) + "]";
}
}
static class Matrix {
private double[][] values;
public Matrix(int rowCount, int colCount) {
values = new double[rowCount][colCount];
}
public Matrix(Quaternion quaternionForRotationMatrix) {
this(3,3);
double w = quaternionForRotationMatrix.getW();
double x = quaternionForRotationMatrix.getX();
double y = quaternionForRotationMatrix.getY();
double z = quaternionForRotationMatrix.getZ();
double ww = w*w;
double wx = w*x;
double xx = x*x;
double xy = x*y;
double xz = x*z;
double wy = w*y;
double yy = y*y;
double yz = y*z;
double wz = w*z;
double zz = z*z;
values[0][0] = ww + xx - yy - zz;
values[0][1] = 2 * xy - 2 * wz;
values[0][2] = 2 * xz + 2 * wy;
values[1][0] = 2 * xy + 2 * wz;
values[1][1] = ww - xx + yy - zz;
values[1][2] = 2 * yz + 2 * wx;
values[2][0] = 2 * xz - 2 * wy;
values[2][1] = 2 * yz - 2 * wx;
values[2][2] = ww - xx - yy + zz;
}
public Vector3D multiply(Vector3D vector){
double [][] vect = new double [3][1];
vect[0][0] = vector.getX();
vect[1][0] = vector.getY();
vect[2][0] = vector.getZ();
double [][] result = multiplyMatrices(values, vect);
return new Vector3D(result[0][0], result[1][0], result[2][0]);
}
private double[][] multiplyMatrices(double[][] m1, double[][] m2) {
double[][] result = null;
if (m1[0].length == m2.length) {
int rowCount1 = m1.length;
int colCount1 = m1[0].length;
int rowCount2 = m2[0].length;
result = new double[rowCount1][rowCount2];
for (int i = 0; i < rowCount1; i++) {
for (int j = 0; j < rowCount2; j++) {
result[i][j] = 0;
for (int k = 0; k < colCount1; k++) {
result[i][j] += m1[i][k] * m2[k][j];
}
}
}
} else {
int rowCount = m1.length;
int colCount = m1[0].length;
result = new double[rowCount][colCount];
for (int i = 0; i < m1.length; i++) {
for (int j = 0; j < m1[0].length; j++) {
result[i][j] = 0;
}
}
}
return result;
}
@Override
public String toString() {
StringBuffer sb = new StringBuffer("Matrix = ");
for(int row = 0 ; row<values.length; row++){
sb.append ("[ ");
for(int col = 0 ; col<values[0].length; col++){
sb.append(Double.toString(values[row][col]));
if(col<values.length-1){
sb.append(" | ");
}
}
sb.append("] ");
}
return sb.toString();
}
}
}
答案 0 :(得分：0)
没关系。找到了。我在公式中有一个错误来构建旋转矩阵。它现在按预期工作。 我正在做一个心理记录，以便将来使用维基百科中的公式而不是其他随机网站。
相应部分应为
values[0][0] = ww + xx - yy - zz;
values[0][1] = 2 * xy - 2 * wz;
values[0][2] = 2 * xz + 2 * wy;
values[1][0] = 2 * xy + 2 * wz;
values[1][1] = ww - xx + yy - zz;
values[1][2] = 2 * yz - 2 * wx; //CORRECTED SIGN
values[2][0] = 2 * xz - 2 * wy;
values[2][1] = 2 * yz + 2 * wx; //CORRECTED SIGN
values[2][2] = ww - xx - yy + zz;
在主要方法结束时，我也使用了错误的向量y和z：
Matrix rotationXMatrixZX = new Matrix(rotZX);
rotatedX = xVect.rotate(rotationXMatrixZX);
rotatedY = yVect.rotate(rotationXMatrixZX); // Corrected used y-vector
rotatedZ = zVect.rotate(rotationXMatrixZX); // Corrected used z-vector