123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237 |
- /*===============================================================================
- Copyright (C) 2022 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 sdk@immersal.com for licensing requests.
- ===============================================================================*/
- using UnityEngine;
- using UnityEngine.XR.ARFoundation;
- using UnityEngine.XR.ARSubsystems;
- using System;
- using System.Runtime.InteropServices;
- using Unity.Collections.LowLevel.Unsafe;
- namespace Immersal.AR
- {
- public class ARHelper {
- public static Matrix4x4 SwitchHandedness(Matrix4x4 b)
- {
- Matrix4x4 D = Matrix4x4.identity;
- D.m00 = -1;
- return D * b * D;
- }
- public static Quaternion SwitchHandedness(Quaternion b)
- {
- Matrix4x4 m = SwitchHandedness(Matrix4x4.Rotate(b));
- return m.rotation;
- }
- public static Vector3 SwitchHandedness(Vector3 b)
- {
- Matrix4x4 m = SwitchHandedness(Matrix4x4.TRS(b, Quaternion.identity, Vector3.one));
- return m.GetColumn(3);
- }
- public static void DoubleQuaternionToDoubleMatrix3x3(out double[] m, double[] q)
- {
- 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;
- }
- public static void GetIntrinsics(out Vector4 intrinsics)
- {
- intrinsics = Vector4.zero;
- XRCameraIntrinsics intr;
- ARCameraManager manager = ImmersalSDK.Instance?.cameraManager;
- if (manager != null && manager.TryGetIntrinsics(out intr))
- {
- intrinsics.x = intr.focalLength.x;
- intrinsics.y = intr.focalLength.y;
- intrinsics.z = intr.principalPoint.x;
- intrinsics.w = intr.principalPoint.y;
- }
- }
- public static void GetRotation(ref Quaternion rot)
- {
- float angle = 0f;
- switch (Screen.orientation)
- {
- case ScreenOrientation.Portrait:
- angle = 90f;
- break;
- case ScreenOrientation.LandscapeLeft:
- angle = 180f;
- break;
- case ScreenOrientation.LandscapeRight:
- angle = 0f;
- break;
- case ScreenOrientation.PortraitUpsideDown:
- angle = -90f;
- break;
- default:
- angle = 0f;
- break;
- }
- rot *= Quaternion.Euler(0f, 0f, angle);
- }
- #if PLATFORM_LUMIN && UNITY_2020_1
- public static void GetPlaneDataFast(ref IntPtr pixels, XRCameraImage image)
- {
- XRCameraImagePlane plane = image.GetPlane(0); // use the Y plane
- #else
- public static void GetPlaneDataFast(ref IntPtr pixels, XRCpuImage image)
- {
- XRCpuImage.Plane plane = image.GetPlane(0); // use the Y plane
- #endif
- int width = image.width, height = image.height;
- if (width == plane.rowStride)
- {
- unsafe
- {
- pixels = (IntPtr)NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(plane.data);
- }
- }
- else
- {
- unsafe
- {
- ulong handle;
- byte[] data = new byte[width * height];
- byte* srcPtr = (byte*)NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(plane.data);
- byte* dstPtr = (byte*)UnsafeUtility.PinGCArrayAndGetDataAddress(data, out handle);
- if (width > 0 && height > 0) {
- UnsafeUtility.MemCpyStride(dstPtr, width, srcPtr, plane.rowStride, width, height);
- }
- pixels = (IntPtr)dstPtr;
- UnsafeUtility.ReleaseGCObject(handle);
- }
- }
- }
-
- #if PLATFORM_LUMIN && UNITY_2020_1
- public static void GetPlaneData(out byte[] pixels, XRCameraImage image)
- {
- XRCameraImagePlane plane = image.GetPlane(0); // use the Y plane
- #else
- public static void GetPlaneData(out byte[] pixels, XRCpuImage image)
- {
- XRCpuImage.Plane plane = image.GetPlane(0); // use the Y plane
- #endif
- int width = image.width, height = image.height;
- pixels = new byte[width * height];
- if (width == plane.rowStride)
- {
- plane.data.CopyTo(pixels);
- }
- else
- {
- unsafe
- {
- ulong handle;
- byte* srcPtr = (byte*)NativeArrayUnsafeUtility.GetUnsafePtr(plane.data);
- byte* dstPtr = (byte*)UnsafeUtility.PinGCArrayAndGetDataAddress(pixels, out handle);
- if (width > 0 && height > 0) {
- UnsafeUtility.MemCpyStride(dstPtr, width, srcPtr, plane.rowStride, width, height);
- }
- UnsafeUtility.ReleaseGCObject(handle);
- }
- }
- }
- #if PLATFORM_LUMIN && UNITY_2020_1
- public static void GetPlaneDataRGB(out byte[] pixels, XRCameraImage image)
- {
- var conversionParams = new XRCameraImageConversionParams
- #else
- public static void GetPlaneDataRGB(out byte[] pixels, XRCpuImage image)
- {
- var conversionParams = new XRCpuImage.ConversionParams
- #endif
- {
- inputRect = new RectInt(0, 0, image.width, image.height),
- outputDimensions = new Vector2Int(image.width, image.height),
- outputFormat = TextureFormat.RGB24,
- #if PLATFORM_LUMIN && UNITY_2020_1
- transformation = CameraImageTransformation.None
- #else
- transformation = XRCpuImage.Transformation.None
- #endif
- };
- int size = image.GetConvertedDataSize(conversionParams);
- pixels = new byte[size];
- GCHandle bufferHandle = GCHandle.Alloc(pixels, GCHandleType.Pinned);
- image.Convert(conversionParams, bufferHandle.AddrOfPinnedObject(), pixels.Length);
- bufferHandle.Free();
- }
- public static bool TryGetTrackingQuality(out int quality)
- {
- quality = default;
- if (ImmersalSDK.Instance?.arSession == null)
- return false;
-
- var arSubsystem = ImmersalSDK.Instance?.arSession.subsystem;
-
- if (arSubsystem != null && arSubsystem.running)
- {
- switch (arSubsystem.trackingState)
- {
- case TrackingState.Tracking:
- quality = 4;
- break;
- case TrackingState.Limited:
- quality = 1;
- break;
- case TrackingState.None:
- quality = 0;
- break;
- }
- }
- return true;
- }
- }
- }
|