AHRSInterface.AHRSInterface.DataReceivedEventHandler C# (CSharp) Method

        void DataReceivedEventHandler(int active_channels)
        {
            //            AppendStatusText("Got Data\r\n", Color.Green);

            float[] Matrix = new float[9];
            float x = (float) sensor.q1;
            float y =  (float)sensor.q2;

            float z =  (float)sensor.q3;
            float a =  (float)sensor.q4;

            Matrix[0] = (a * a) + (x * x) - (y * y) - (z * z);
            Matrix[1] = (2 * x * y) + (2 * a * z);
            Matrix[2] = (2 * x * z) - (2 * a * y);
            //Column 2
            Matrix[3] = (2 * x * y) - (2 * a * z);
            Matrix[4] = (a * a) - (x * x) + (y * y) - (z * z);
            Matrix[5] = (2 * y * z) + (2 * a * x);
            //Column 3
            Matrix[6] = (2 * x * z) + (2 * a * y);
            Matrix[7] = (2 * y * z) - (2 * a * x);
            Matrix[8] = (a * a) - (x * x) - (y * y) + (z * z);
            //Column 4

            // Convert from NED to XYZ
            float[] num = new float[9] {
             (float)(2 * System.Math.Pow(sensor.q1,2)  -1 +2 * System.Math.Pow(sensor.q2,2)) ,
             (float)(2*(sensor.q2 *sensor.q3 + sensor.q1 * sensor.q4)),
             (float)(2*(sensor.q2*sensor.q4 -sensor.q1 * sensor.q3)),

             (float)(2*(sensor.q2*sensor.q3 - sensor.q1*sensor.q4)),
             (float)(2*System.Math.Pow(sensor.q1,2) -1 +2*System.Math.Pow(sensor.q3,2)),
             (float)(2*(sensor.q3*sensor.q4 + sensor.q1*sensor.q2)),

            (float)(2*(sensor.q2*sensor.q4 + sensor.q1*sensor.q3)),
            (float)(2*(sensor.q3*sensor.q4 - sensor.q1*sensor.q2)),
            (float)(2*System.Math.Pow(sensor.q1,2) -1 +2*System.Math.Pow(sensor.q4,2))};

            float[] Quaternion =new float[] {x,y,z,a};

            float R11 = 2 * Quaternion[0] * Quaternion[0] - 1 + 2 * Quaternion[1] * Quaternion[1];
            float R12 = 2 * (Quaternion[1] * Quaternion[2] + Quaternion[0] * Quaternion[3]);
            float R13 = 2 * (Quaternion[1] * Quaternion[3] - Quaternion[0] * Quaternion[2]);
            float R21 = 2 * (Quaternion[1] * Quaternion[2] - Quaternion[0] * Quaternion[3]);
            float R22 = 2 * Quaternion[0] * Quaternion[0] - 1 + 2 * Quaternion[2] * Quaternion[2];
            float R23 = 2 * (Quaternion[2] * Quaternion[3] + Quaternion[0] * Quaternion[1]);
            float R31 = 2 * (Quaternion[1] * Quaternion[3] + Quaternion[0] * Quaternion[2]);
            float R32 = 2 * (Quaternion[2] * Quaternion[3] - Quaternion[0] * Quaternion[1]);
            float R33 = 2 * Quaternion[0] * Quaternion[0] - 1 + 2 * Quaternion[3] * Quaternion[3];

            float[] nnn= new float[] {  R11, R12, R13,
                                        R21, R22, R23,
                                        R31, R32, R33 };

            form_3DcuboidA.RotationMatrix = nnn;

            //form_3DcuboidA.CameraView =;
        }