Unity中的旋转和矩阵操作

Definition for Matrix4x4 is from unity
                Matrix4x4 matrix = new Matrix4x4();
                matrix.SetRow(0, new Vector4(m[3], m[6], m[9], m[12] / scaleMultiplier));//经过实际测试前三个都是0，从第四个开始才跟transform中的数据一样，跟openIGTLink官方的协议说明不同，按实际的来，所以从m[3]开始
                matrix.SetRow(1, new Vector4(m[4], m[7], m[10], m[13] / scaleMultiplier));
                matrix.SetRow(2, new Vector4(m[5], m[8], m[11], m[14] / scaleMultiplier));
                matrix.SetRow(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));

在Unity中使用Matrix4x4.Inverse(matrix)求得一个矩阵得逆矩阵

 IEnumerator ReceiveTransformMessage(byte[] data, string transformName)
    {
        // Find Game Objects with Transform Name and determine if they should be updated
        string objectName;
        foreach (GameObject gameObject in GameObjects)
        {
            // Could be a bit more efficient
            if (gameObject.name.Length > 20)
            {
                objectName = gameObject.name.Substring(0, 20);
            }
            else
            {
                objectName = gameObject.name;
            }
 
            if (objectName.Equals(transformName) & gameObject.GetComponent().ReceiveTransform)
            {
                // Transform Matrix starts from byte 58 until 106
                // Extract transform matrix
                byte[] matrixBytes = new byte[4];
                float[] m = new float[15];
                for (int i = 0; i < 15; i++)
                {
                    Buffer.BlockCopy(data, 58 + i * 4, matrixBytes, 0, 4);
 
                    if (BitConverter.IsLittleEndian)
                    {
                        Array.Reverse(matrixBytes);
                    }
 
                    m[i] = BitConverter.ToSingle(matrixBytes, 0);
 //                   Debug.Log($"m{i},is {m[i]}");
 
                }
                // Slicer units are in millimeters, Unity is in meters, so convert accordingly
                // Definition for Matrix4x4 is from unity
                Matrix4x4 matrix = new Matrix4x4();
                /*            matrix.SetRow(0, new Vector4(m[0], m[3], m[6], m[9] / scaleMultiplier));
                            matrix.SetRow(1, new Vector4(m[1], m[4], m[7], m[10] / scaleMultiplier));
                            matrix.SetRow(2, new Vector4(m[2], m[5], m[8], m[11] / scaleMultiplier));*/
                matrix.SetRow(0, new Vector4(m[3], m[6], m[9], m[12] / scaleMultiplier));//经过实际测试前三个都是0，从第四个开始才跟transform中的数据一样，跟openIGTLink官方的协议说明不同，按实际的来，所以从m[3]开始
                matrix.SetRow(1, new Vector4(m[4], m[7], m[10], m[13] / scaleMultiplier));
                matrix.SetRow(2, new Vector4(m[5], m[8], m[11], m[14] / scaleMultiplier));
                matrix.SetRow(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
 
                Matrix4x4 IJKToRAS = new Matrix4x4();
                IJKToRAS.SetRow(0, new Vector4(-1.0f, 0, 0, 0));
                IJKToRAS.SetRow(1, new Vector4(0, -1.0f, 0, 2));
                IJKToRAS.SetRow(2, new Vector4(0, 0, 1.0f, 0));
                IJKToRAS.SetRow(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
                //在Unity中使用Matrix4x4.Inverse(matrix)求得一个矩阵得逆矩阵
                Matrix4x4 matrixRAS = matrix * IJKToRAS;//暂时没用上
                
                Vector3 translation = matrix.GetColumn(3);//上面matrix.SetRow已经转为以m为单位了
                tro = new Vector3(m[12], m[13], m[14]);//平移记录给全局变量，继续保持以mm为单位，为了在NDI设备中方位合成计算方便
                rro[0, 0] = matrix.m00; rro[0, 1] = matrix.m01; rro[0, 2] = matrix.m02; //旋转记录给全局变量
                rro[1, 0] = matrix.m10; rro[1, 1] = matrix.m11; rro[1, 2] = matrix.m12;//旋转记录给全局变量
                rro[2, 0] = matrix.m20; rro[2, 1] = matrix.m21; rro[2, 2] = matrix.m22;//旋转记录给全局变量
                if (tibiacontrolclass.targetflag)//如果ControllerTibia中点了button，tibiacontrolclass.targetflag变为true,表面现象是显示target plane位置
                               {
                    //                                  gameObject.transform.localPosition = new Vector3(-translation.x + TrackerOriginPosition.cubeposition.x, translation.y + TrackerOriginPosition.cubeposition.y, translation.z + TrackerOriginPosition.cubeposition.z);
                    gameObject.transform.localPosition = TrackerOriginPosition.cuberotation * new Vector3(translation.y, translation.z, -translation.x) + TrackerOriginPosition.cubeposition;
                    ReferenceInGlass = new Vector3(translation.y, translation.z, -translation.x);//从tracker设备坐标系下转换到glass坐标系下，标记到全局变量给ReceivePointMessage函数用
                    Debug.Log($"Tracker is {TrackerOriginPosition.cubeposition};Reference is {-translation.x},{translation.y},{translation.z};sum is {gameObject.transform.localPosition}");
                               }
                               else
                                {
                                   gameObject.transform.localPosition = new Vector3(-translation.x, translation.y, translation.z);
                                   Debug.Log($"Reference is {gameObject.transform.localPosition}");
                                }
//                gameObject.transform.localPosition = new Vector3(-translation.x, translation.y, translation.z);
//               Debug.Log($"Reference is {gameObject.transform.localPosition}");
                Vector3 eulerAngles = matrix.rotation.eulerAngles;
                gameObject.transform.localRotation = Quaternion.Euler(eulerAngles.x, -eulerAngles.y, -eulerAngles.z);
 //               Debug.Log($"Rotation is {eulerAngles.x},{eulerAngles.y},{eulerAngles.z}");
            }
            else if (objectName.Equals(transformName) & gameObject.GetComponent().ReceiveImage)//官方协议是错误的，因此图片传输用OpenIGTLink没法进行下去。
            {
 
            }
        }
        // Place this inside the loop if you only want to perform one loop per update cycle
        yield return null;
    }