ARHelper.cs 6.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237
  1. /*===============================================================================
  2. Copyright (C) 2022 Immersal - Part of Hexagon. All Rights Reserved.
  3. This file is part of the Immersal SDK.
  4. The Immersal SDK cannot be copied, distributed, or made available to
  5. third-parties for commercial purposes without written permission of Immersal Ltd.
  6. Contact sdk@immersal.com for licensing requests.
  7. ===============================================================================*/
  8. using UnityEngine;
  9. using UnityEngine.XR.ARFoundation;
  10. using UnityEngine.XR.ARSubsystems;
  11. using System;
  12. using System.Runtime.InteropServices;
  13. using Unity.Collections.LowLevel.Unsafe;
  14. namespace Immersal.AR
  15. {
  16. public class ARHelper {
  17. public static Matrix4x4 SwitchHandedness(Matrix4x4 b)
  18. {
  19. Matrix4x4 D = Matrix4x4.identity;
  20. D.m00 = -1;
  21. return D * b * D;
  22. }
  23. public static Quaternion SwitchHandedness(Quaternion b)
  24. {
  25. Matrix4x4 m = SwitchHandedness(Matrix4x4.Rotate(b));
  26. return m.rotation;
  27. }
  28. public static Vector3 SwitchHandedness(Vector3 b)
  29. {
  30. Matrix4x4 m = SwitchHandedness(Matrix4x4.TRS(b, Quaternion.identity, Vector3.one));
  31. return m.GetColumn(3);
  32. }
  33. public static void DoubleQuaternionToDoubleMatrix3x3(out double[] m, double[] q)
  34. {
  35. m = new double [] {1, 0, 0, 0, 1, 0, 0, 0, 1}; //identity matrix
  36. // input quaternion should be in WXYZ order
  37. double w = q[0];
  38. double x = q[1];
  39. double y = q[2];
  40. double z = q[3];
  41. double ww = w * w;
  42. double xx = x * x;
  43. double yy = y * y;
  44. double zz = z * z;
  45. double xy = x * y;
  46. double zw = z * w;
  47. double xz = x * z;
  48. double yw = y * w;
  49. double yz = y * z;
  50. double xw = x * w;
  51. double inv = 1.0 / (xx + yy + zz + ww);
  52. m[0] = ( xx - yy - zz + ww) * inv;
  53. m[1] = 2.0 * (xy - zw) * inv;
  54. m[2] = 2.0 * (xz + yw) * inv;
  55. m[3] = 2.0 * (xy + zw) * inv;
  56. m[4] = (-xx + yy - zz + ww) * inv;
  57. m[5] = 2.0 * (yz - xw) * inv;
  58. m[6] = 2.0 * (xz - yw) * inv;
  59. m[7] = 2.0 * (yz + xw) * inv;
  60. m[8] = (-xx - yy + zz + ww) * inv;
  61. }
  62. public static void GetIntrinsics(out Vector4 intrinsics)
  63. {
  64. intrinsics = Vector4.zero;
  65. XRCameraIntrinsics intr;
  66. ARCameraManager manager = ImmersalSDK.Instance?.cameraManager;
  67. if (manager != null && manager.TryGetIntrinsics(out intr))
  68. {
  69. intrinsics.x = intr.focalLength.x;
  70. intrinsics.y = intr.focalLength.y;
  71. intrinsics.z = intr.principalPoint.x;
  72. intrinsics.w = intr.principalPoint.y;
  73. }
  74. }
  75. public static void GetRotation(ref Quaternion rot)
  76. {
  77. float angle = 0f;
  78. switch (Screen.orientation)
  79. {
  80. case ScreenOrientation.Portrait:
  81. angle = 90f;
  82. break;
  83. case ScreenOrientation.LandscapeLeft:
  84. angle = 180f;
  85. break;
  86. case ScreenOrientation.LandscapeRight:
  87. angle = 0f;
  88. break;
  89. case ScreenOrientation.PortraitUpsideDown:
  90. angle = -90f;
  91. break;
  92. default:
  93. angle = 0f;
  94. break;
  95. }
  96. rot *= Quaternion.Euler(0f, 0f, angle);
  97. }
  98. #if PLATFORM_LUMIN && UNITY_2020_1
  99. public static void GetPlaneDataFast(ref IntPtr pixels, XRCameraImage image)
  100. {
  101. XRCameraImagePlane plane = image.GetPlane(0); // use the Y plane
  102. #else
  103. public static void GetPlaneDataFast(ref IntPtr pixels, XRCpuImage image)
  104. {
  105. XRCpuImage.Plane plane = image.GetPlane(0); // use the Y plane
  106. #endif
  107. int width = image.width, height = image.height;
  108. if (width == plane.rowStride)
  109. {
  110. unsafe
  111. {
  112. pixels = (IntPtr)NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(plane.data);
  113. }
  114. }
  115. else
  116. {
  117. unsafe
  118. {
  119. ulong handle;
  120. byte[] data = new byte[width * height];
  121. byte* srcPtr = (byte*)NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(plane.data);
  122. byte* dstPtr = (byte*)UnsafeUtility.PinGCArrayAndGetDataAddress(data, out handle);
  123. if (width > 0 && height > 0) {
  124. UnsafeUtility.MemCpyStride(dstPtr, width, srcPtr, plane.rowStride, width, height);
  125. }
  126. pixels = (IntPtr)dstPtr;
  127. UnsafeUtility.ReleaseGCObject(handle);
  128. }
  129. }
  130. }
  131. #if PLATFORM_LUMIN && UNITY_2020_1
  132. public static void GetPlaneData(out byte[] pixels, XRCameraImage image)
  133. {
  134. XRCameraImagePlane plane = image.GetPlane(0); // use the Y plane
  135. #else
  136. public static void GetPlaneData(out byte[] pixels, XRCpuImage image)
  137. {
  138. XRCpuImage.Plane plane = image.GetPlane(0); // use the Y plane
  139. #endif
  140. int width = image.width, height = image.height;
  141. pixels = new byte[width * height];
  142. if (width == plane.rowStride)
  143. {
  144. plane.data.CopyTo(pixels);
  145. }
  146. else
  147. {
  148. unsafe
  149. {
  150. ulong handle;
  151. byte* srcPtr = (byte*)NativeArrayUnsafeUtility.GetUnsafePtr(plane.data);
  152. byte* dstPtr = (byte*)UnsafeUtility.PinGCArrayAndGetDataAddress(pixels, out handle);
  153. if (width > 0 && height > 0) {
  154. UnsafeUtility.MemCpyStride(dstPtr, width, srcPtr, plane.rowStride, width, height);
  155. }
  156. UnsafeUtility.ReleaseGCObject(handle);
  157. }
  158. }
  159. }
  160. #if PLATFORM_LUMIN && UNITY_2020_1
  161. public static void GetPlaneDataRGB(out byte[] pixels, XRCameraImage image)
  162. {
  163. var conversionParams = new XRCameraImageConversionParams
  164. #else
  165. public static void GetPlaneDataRGB(out byte[] pixels, XRCpuImage image)
  166. {
  167. var conversionParams = new XRCpuImage.ConversionParams
  168. #endif
  169. {
  170. inputRect = new RectInt(0, 0, image.width, image.height),
  171. outputDimensions = new Vector2Int(image.width, image.height),
  172. outputFormat = TextureFormat.RGB24,
  173. #if PLATFORM_LUMIN && UNITY_2020_1
  174. transformation = CameraImageTransformation.None
  175. #else
  176. transformation = XRCpuImage.Transformation.None
  177. #endif
  178. };
  179. int size = image.GetConvertedDataSize(conversionParams);
  180. pixels = new byte[size];
  181. GCHandle bufferHandle = GCHandle.Alloc(pixels, GCHandleType.Pinned);
  182. image.Convert(conversionParams, bufferHandle.AddrOfPinnedObject(), pixels.Length);
  183. bufferHandle.Free();
  184. }
  185. public static bool TryGetTrackingQuality(out int quality)
  186. {
  187. quality = default;
  188. if (ImmersalSDK.Instance?.arSession == null)
  189. return false;
  190. var arSubsystem = ImmersalSDK.Instance?.arSession.subsystem;
  191. if (arSubsystem != null && arSubsystem.running)
  192. {
  193. switch (arSubsystem.trackingState)
  194. {
  195. case TrackingState.Tracking:
  196. quality = 4;
  197. break;
  198. case TrackingState.Limited:
  199. quality = 1;
  200. break;
  201. case TrackingState.None:
  202. quality = 0;
  203. break;
  204. }
  205. }
  206. return true;
  207. }
  208. }
  209. }