99.imdk_unity 上传

Assets/99.imdk_unity/Runtime/Scripts/Util/ExtensionMethods.cs

@@ -0,0 +1,87 @@
+/*===============================================================================
+Copyright (C) 2024 Immersal - Part of Hexagon. All Rights Reserved.
+
+This file is part of the Immersal SDK.
+
+The Immersal SDK cannot be copied, distributed, or made available to
+third-parties for commercial purposes without written permission of Immersal Ltd.
+
+Contact sales@immersal.com for licensing requests.
+===============================================================================*/
+
+using Immersal.XR;
+using UnityEngine;
+
+namespace Immersal.XR
+{
+    public static class ExtensionMethods
+    {
+        public static Matrix4x4 SwitchHandedness(this ref Matrix4x4 b)
+        {
+            Matrix4x4 D = Matrix4x4.identity;
+            D.m00 = -1;
+            b = D * b * D;
+            return b;
+        }
+
+        public static Quaternion SwitchHandedness(this ref Quaternion b)
+        {
+            Matrix4x4 m = Matrix4x4.Rotate(b);
+            m.SwitchHandedness();
+            b = m.rotation;
+            return b;
+        }
+
+        public static float[] ToFloats(this ref Quaternion q)
+        {
+            float[] result = new float[4] { q.x, q.y, q.z, q.w };
+            return result;
+        }
+
+        public static Vector3 SwitchHandedness(this ref Vector3 b)
+        {
+            Matrix4x4 m = Matrix4x4.TRS(b, Quaternion.identity, Vector3.one);
+            m.SwitchHandedness();
+            b = m.GetColumn(3);
+            return b;
+        }
+        
+        public static double[] QuaternionsToDoubleMatrix3x3(this XRMap.MapAlignment ma)
+        {
+            double[] q = new double[] {ma.qw, ma.qx, ma.qy, ma.qz};
+            double[] m = new double [] {1, 0, 0, 0, 1, 0, 0, 0, 1}; //identity matrix
+            
+            // input quaternion should be in WXYZ order
+            double w = q[0];
+            double x = q[1];
+            double y = q[2];
+            double z = q[3];
+			
+            double ww = w * w;
+            double xx = x * x;
+            double yy = y * y;
+            double zz = z * z;
+            
+            double xy = x * y;
+            double zw = z * w;
+            double xz = x * z;
+            double yw = y * w;
+            double yz = y * z;
+            double xw = x * w;
+
+            double inv = 1.0 / (xx + yy + zz + ww);
+
+            m[0] = ( xx - yy - zz + ww) * inv;
+            m[1] = 2.0 * (xy - zw) * inv;
+            m[2] = 2.0 * (xz + yw) * inv;
+            m[3] = 2.0 * (xy + zw) * inv;
+            m[4] = (-xx + yy - zz + ww) * inv;
+            m[5] = 2.0 * (yz - xw) * inv;
+            m[6] = 2.0 * (xz - yw) * inv;
+            m[7] = 2.0 * (yz + xw) * inv;
+            m[8] = (-xx - yy + zz + ww) * inv;
+
+            return m;
+        }
+    }
+}