Newer
Older
HoloAnatomy / Assets / HoloToolkit / SpatialMapping / Scripts / SpatialProcessing / PlaneFinding.cs
SURFACEBOOK2\jackwynne on 25 May 2018 14 KB v1
  1. // Copyright (c) Microsoft Corporation. All rights reserved.
  2. // Licensed under the MIT License. See LICENSE in the project root for license information.
  3.  
  4. using System;
  5. using System.Collections.Generic;
  6. using System.Runtime.InteropServices;
  7. using UnityEngine;
  8.  
  9. namespace HoloToolkit.Unity.SpatialMapping
  10. {
  11. [StructLayout(LayoutKind.Sequential)]
  12. public struct OrientedBoundingBox
  13. {
  14. public Vector3 Center;
  15. public Vector3 Extents;
  16. public Quaternion Rotation;
  17. };
  18.  
  19. [StructLayout(LayoutKind.Sequential)]
  20. public struct BoundedPlane
  21. {
  22. public Plane Plane;
  23. public OrientedBoundingBox Bounds;
  24. public float Area;
  25.  
  26. /// <summary>
  27. /// Builds the bounded plane to match the obb defined by xform
  28. /// </summary>
  29. public BoundedPlane(Transform xform)
  30. {
  31. Plane = new Plane(xform.forward, xform.position);
  32. Bounds = new OrientedBoundingBox()
  33. {
  34. Center = xform.position,
  35. Extents = xform.localScale / 2,
  36. Rotation = xform.rotation
  37. };
  38. Area = Bounds.Extents.x * Bounds.Extents.y;
  39. }
  40. };
  41.  
  42. public class PlaneFinding
  43. {
  44. #region Public APIs
  45.  
  46. /// <summary>
  47. /// PlaneFinding is an expensive task that should not be run from Unity's main thread as it
  48. /// will stall the thread and cause a frame rate dip. Instead, the PlaneFinding APIs should be
  49. /// exclusively called from background threads. Unfortunately, Unity's built-in data types
  50. /// (such as MeshFilter) are not thread safe and cannot be accessed from background threads.
  51. /// The MeshData struct exists to work-around this limitation. When you want to find planes
  52. /// in a collection of MeshFilter objects, start by constructing a list of MeshData structs
  53. /// from those MeshFilters. You can then take the resulting list of MeshData structs, and
  54. /// safely pass it to the FindPlanes() API from a background thread.
  55. /// </summary>
  56. public struct MeshData
  57. {
  58. public Matrix4x4 Transform;
  59. public Vector3[] Verts;
  60. public Vector3[] Normals;
  61. public Int32[] Indices;
  62.  
  63. public MeshData(MeshFilter meshFilter)
  64. {
  65. Transform = meshFilter.transform.localToWorldMatrix;
  66. Verts = meshFilter.sharedMesh.vertices;
  67. Normals = meshFilter.sharedMesh.normals;
  68. Indices = meshFilter.sharedMesh.triangles;
  69. }
  70. }
  71.  
  72. /// <summary>
  73. /// Finds small planar patches that are contained within individual meshes. The output of this
  74. /// API can then be passed to MergeSubPlanes() in order to find larger planar surfaces that
  75. /// potentially span across multiple meshes.
  76. /// </summary>
  77. /// <param name="meshes">
  78. /// List of meshes to run the plane finding algorithm on.
  79. /// </param>
  80. /// <param name="snapToGravityThreshold">
  81. /// Planes whose normal vectors are within this threshold (in degrees) from vertical/horizontal
  82. /// will be snapped to be perfectly gravity aligned. When set to something other than zero, the
  83. /// bounding boxes for each plane will be gravity aligned as well, rather than rotated for an
  84. /// optimally tight fit. Pass 0.0 for this parameter to completely disable the gravity alignment
  85. /// logic.
  86. /// </param>
  87. public static BoundedPlane[] FindSubPlanes(List<MeshData> meshes, float snapToGravityThreshold = 0.0f)
  88. {
  89. StartPlaneFinding();
  90.  
  91. try
  92. {
  93. int planeCount;
  94. IntPtr planesPtr;
  95. IntPtr pinnedMeshData = PinMeshDataForMarshalling(meshes);
  96. DLLImports.FindSubPlanes(meshes.Count, pinnedMeshData, snapToGravityThreshold, out planeCount, out planesPtr);
  97. return MarshalBoundedPlanesFromIntPtr(planesPtr, planeCount);
  98. }
  99. finally
  100. {
  101. FinishPlaneFinding();
  102. }
  103. }
  104.  
  105. /// <summary>
  106. /// Takes the subplanes returned by one or more previous calls to FindSubPlanes() and merges
  107. /// them together into larger planes that can potentially span across multiple meshes.
  108. /// Overlapping subplanes that have similar plane equations will be merged together to form
  109. /// larger planes.
  110. /// </summary>
  111. /// <param name="subPlanes">
  112. /// The output from one or more previous calls to FindSubPlanes().
  113. /// </param>
  114. /// <param name="snapToGravityThreshold">
  115. /// Planes whose normal vectors are within this threshold (in degrees) from vertical/horizontal
  116. /// will be snapped to be perfectly gravity aligned. When set to something other than zero, the
  117. /// bounding boxes for each plane will be gravity aligned as well, rather than rotated for an
  118. /// optimally tight fit. Pass 0.0 for this parameter to completely disable the gravity alignment
  119. /// logic.
  120. /// </param>
  121. /// <param name="minArea">
  122. /// While merging subplanes together, any candidate merged plane whose constituent mesh
  123. /// triangles have a total area less than this threshold are ignored.
  124. /// </param>
  125. public static BoundedPlane[] MergeSubPlanes(BoundedPlane[] subPlanes, float snapToGravityThreshold = 0.0f, float minArea = 0.0f)
  126. {
  127. StartPlaneFinding();
  128.  
  129. try
  130. {
  131. int planeCount;
  132. IntPtr planesPtr;
  133. DLLImports.MergeSubPlanes(subPlanes.Length, PinObject(subPlanes), minArea, snapToGravityThreshold, out planeCount, out planesPtr);
  134. return MarshalBoundedPlanesFromIntPtr(planesPtr, planeCount);
  135. }
  136. finally
  137. {
  138. FinishPlaneFinding();
  139. }
  140. }
  141.  
  142. /// <summary>
  143. /// Convenience wrapper that executes FindSubPlanes followed by MergeSubPlanes via a single
  144. /// call into native code (which improves performance by avoiding a bunch of unnecessary data
  145. /// marshalling and a managed-to-native transition).
  146. /// </summary>
  147. /// <param name="meshes">
  148. /// List of meshes to run the plane finding algorithm on.
  149. /// </param>
  150. /// <param name="snapToGravityThreshold">
  151. /// Planes whose normal vectors are within this threshold (in degrees) from vertical/horizontal
  152. /// will be snapped to be perfectly gravity aligned. When set to something other than zero, the
  153. /// bounding boxes for each plane will be gravity aligned as well, rather than rotated for an
  154. /// optimally tight fit. Pass 0.0 for this parameter to completely disable the gravity alignment
  155. /// logic.
  156. /// </param>
  157. /// <param name="minArea">
  158. /// While merging subplanes together, any candidate merged plane whose constituent mesh
  159. /// triangles have a total area less than this threshold are ignored.
  160. /// </param>
  161. public static BoundedPlane[] FindPlanes(List<MeshData> meshes, float snapToGravityThreshold = 0.0f, float minArea = 0.0f)
  162. {
  163. StartPlaneFinding();
  164.  
  165. try
  166. {
  167. int planeCount;
  168. IntPtr planesPtr;
  169. IntPtr pinnedMeshData = PinMeshDataForMarshalling(meshes);
  170. DLLImports.FindPlanes(meshes.Count, pinnedMeshData, minArea, snapToGravityThreshold, out planeCount, out planesPtr);
  171. return MarshalBoundedPlanesFromIntPtr(planesPtr, planeCount);
  172. }
  173. finally
  174. {
  175. FinishPlaneFinding();
  176. }
  177. }
  178.  
  179. #endregion
  180.  
  181. #region Internal
  182.  
  183. private static bool findPlanesRunning = false;
  184. private static System.Object findPlanesLock = new System.Object();
  185. private static DLLImports.ImportedMeshData[] reusedImportedMeshesForMarshalling;
  186. private static List<GCHandle> reusedPinnedMemoryHandles = new List<GCHandle>();
  187.  
  188. /// <summary>
  189. /// Validate that no other PlaneFinding API call is currently in progress. As a performance
  190. /// optimization to avoid unnecessarily thrashing the garbage collector, each call into the
  191. /// PlaneFinding DLL reuses a couple of static data structures. As a result, we can't handle
  192. /// multiple concurrent calls into these APIs.
  193. /// </summary>
  194. private static void StartPlaneFinding()
  195. {
  196. lock (findPlanesLock)
  197. {
  198. if (findPlanesRunning)
  199. {
  200. throw new Exception("PlaneFinding is already running. You can not call these APIs from multiple threads.");
  201. }
  202. findPlanesRunning = true;
  203. }
  204. }
  205.  
  206. /// <summary>
  207. /// Cleanup after finishing a PlaneFinding API call by unpinning any memory that was pinned
  208. /// for the call into the driver, and then reset the findPlanesRunning bool.
  209. /// </summary>
  210. private static void FinishPlaneFinding()
  211. {
  212. UnpinAllObjects();
  213. findPlanesRunning = false;
  214. }
  215.  
  216. /// <summary>
  217. /// Pins the specified object so that the backing memory can not be relocated, adds the pinned
  218. /// memory handle to the tracking list, and then returns that address of the pinned memory so
  219. /// that it can be passed into the DLL to be access directly from native code.
  220. /// </summary>
  221. private static IntPtr PinObject(System.Object obj)
  222. {
  223. GCHandle h = GCHandle.Alloc(obj, GCHandleType.Pinned);
  224. reusedPinnedMemoryHandles.Add(h);
  225. return h.AddrOfPinnedObject();
  226. }
  227.  
  228. /// <summary>
  229. /// Unpins all of the memory previously pinned by calls to PinObject().
  230. /// </summary>
  231. private static void UnpinAllObjects()
  232. {
  233. for (int i = 0; i < reusedPinnedMemoryHandles.Count; ++i)
  234. {
  235. reusedPinnedMemoryHandles[i].Free();
  236. }
  237. reusedPinnedMemoryHandles.Clear();
  238. }
  239.  
  240. /// <summary>
  241. /// Copies the supplied mesh data into the reusedMeshesForMarhsalling array. All managed arrays
  242. /// are pinned so that the marshalling only needs to pass a pointer and the native code can
  243. /// reference the memory in place without needing the marshaller to create a complete copy of
  244. /// the data.
  245. /// </summary>
  246. private static IntPtr PinMeshDataForMarshalling(List<MeshData> meshes)
  247. {
  248. // if we have a big enough array reuse it, otherwise create new
  249. if (reusedImportedMeshesForMarshalling == null || reusedImportedMeshesForMarshalling.Length < meshes.Count)
  250. {
  251. reusedImportedMeshesForMarshalling = new DLLImports.ImportedMeshData[meshes.Count];
  252. }
  253.  
  254. for (int i = 0; i < meshes.Count; ++i)
  255. {
  256. reusedImportedMeshesForMarshalling[i] = new DLLImports.ImportedMeshData
  257. {
  258. transform = meshes[i].Transform,
  259. vertCount = meshes[i].Verts.Length,
  260. indexCount = meshes[i].Indices.Length,
  261. verts = PinObject(meshes[i].Verts),
  262. normals = PinObject(meshes[i].Normals),
  263. indices = PinObject(meshes[i].Indices),
  264. };
  265. }
  266.  
  267. return PinObject(reusedImportedMeshesForMarshalling);
  268. }
  269.  
  270. /// <summary>
  271. /// Marshals BoundedPlane data returned from a DLL API call into a managed BoundedPlane array
  272. /// and then frees the memory that was allocated within the DLL.
  273. /// </summary>
  274. /// <remarks>Disabling warning 618 when calling Marshal.SizeOf(), because
  275. /// Unity does not support .Net 4.5.1+ for using the preferred Marshal.SizeOf(T) method."/>, </remarks>
  276. private static BoundedPlane[] MarshalBoundedPlanesFromIntPtr(IntPtr outArray, int size)
  277. {
  278. BoundedPlane[] resArray = new BoundedPlane[size];
  279. #pragma warning disable 618
  280. int structsize = Marshal.SizeOf(typeof(BoundedPlane));
  281. #pragma warning restore 618
  282. IntPtr current = outArray;
  283. for (int i = 0; i < size; i++)
  284. {
  285. #pragma warning disable 618
  286. resArray[i] = (BoundedPlane)Marshal.PtrToStructure(current, typeof(BoundedPlane));
  287. #pragma warning restore 618
  288. current = (IntPtr)((long)current + structsize);
  289. }
  290. Marshal.FreeCoTaskMem(outArray);
  291. return resArray;
  292. }
  293.  
  294. /// <summary>
  295. /// Raw PlaneFinding.dll imports
  296. /// </summary>
  297. private class DLLImports
  298. {
  299. [StructLayout(LayoutKind.Sequential)]
  300. public struct ImportedMeshData
  301. {
  302. public Matrix4x4 transform;
  303. public Int32 vertCount;
  304. public Int32 indexCount;
  305. public IntPtr verts;
  306. public IntPtr normals;
  307. public IntPtr indices;
  308. };
  309.  
  310. [DllImport("PlaneFinding")]
  311. public static extern void FindPlanes(
  312. [In] int meshCount,
  313. [In] IntPtr meshes,
  314. [In] float minArea,
  315. [In] float snapToGravityThreshold,
  316. [Out] out int planeCount,
  317. [Out] out IntPtr planesPtr);
  318.  
  319. [DllImport("PlaneFinding")]
  320. public static extern void FindSubPlanes(
  321. [In] int meshCount,
  322. [In] IntPtr meshes,
  323. [In] float snapToGravityThreshold,
  324. [Out] out int planeCount,
  325. [Out] out IntPtr planesPtr);
  326.  
  327. [DllImport("PlaneFinding")]
  328. public static extern void MergeSubPlanes(
  329. [In] int subPlaneCount,
  330. [In] IntPtr subPlanes,
  331. [In] float minArea,
  332. [In] float snapToGravityThreshold,
  333. [Out] out int planeCount,
  334. [Out] out IntPtr planesPtr);
  335. }
  336.  
  337. #endregion
  338. }
  339. }