Java 四元数乘法的结果不产生预期的局部坐标系旋转

当将两个四元数相乘并将结果旋转应用到本地右手坐标系时，我得到了意想不到的结果。X指向前方，Y指向右侧，Z向下

请参阅下面我的JavaSCCE

因此，我试图首先应用一个Z旋转90度偏航，然后围绕局部X轴旋转90度。 我试图通过将表示这两个旋转的两个四元数相乘，从结果中创建一个旋转矩阵，并将其应用于坐标系的3个单位向量来实现这一点，但我得到的结果没有意义。i、 e.它们不代表从这两个旋转中获得的坐标系

我尝试过改变四元数的乘法顺序，但这无助于看到在SCCE的main方法中注释掉的代码行。 我还尝试从全局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();
        }
    }   
}

---

别客气。找到了。我在构建旋转矩阵的公式中有一个错误。现在它的工作原理与预期一致。 我在心里记下，将来使用维基百科的公式，而不是其他一些随机的网站

各部分应为：

        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