Beispiel #1
0
        public static MatrixFixed dR_by_dqz(Quaternion q)
        {
            float q0 = q.R();
            float qx = q.X();
            float qy = q.Y();
            float qz = q.Z();

            float[] a = new float[9];
            a[0] = -2*qz;
            a[1] = -2*q0;
            a[2] = 2*qx;

            a[3] = 2*q0;
            a[4] = -2*qz;
            a[5] = 2*qy;

            a[6] = 2*qx;
            a[7] = 2*qy;
            a[8] = 2*qz;

            MatrixFixed M = new MatrixFixed(3,3);
            return M;
        }
Beispiel #2
0
        public static MatrixFixed dq3_by_dq2(Quaternion q1)
        {
            float[] a = new float[16];
            
            a[0] = q1.R();
            a[1] = -q1.X();
            a[2] = -q1.Y();
            a[3] = -q1.Z();

            a[4] = q1.X();
            a[5] = q1.R();
            a[6] = q1.Z();
            a[7] = -q1.Y();

            a[8] = q1.Y();
            a[9] = -q1.Z();
            a[10] = q1.R();
            a[11] = q1.X();

            a[12] = q1.Z();
            a[13] = q1.Y();
            a[14] = -q1.X();
            a[15] = q1.R();

            MatrixFixed M = new MatrixFixed(a);
            return M;
        }