PlaneFinding.cs

Go to the documentation of this file.

// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License. See LICENSE in the project root for license information.

using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using UnityEngine;

namespace HoloToolkit.Unity.SpatialMapping
{
    [StructLayout(LayoutKind.Sequential)]
    public struct OrientedBoundingBox
    {
        public Vector3 Center;
        public Vector3 Extents;
        public Quaternion Rotation;
    };

    [StructLayout(LayoutKind.Sequential)]
    public struct BoundedPlane
    {
        public Plane Plane;
        public OrientedBoundingBox Bounds;
        public float Area;

        public BoundedPlane(Transform xform)
        {
            Plane = new Plane(xform.forward, xform.position);
            Bounds = new OrientedBoundingBox()
            {
                Center = xform.position,
                Extents = xform.localScale / 2,
                Rotation = xform.rotation
            };
            Area = Bounds.Extents.x * Bounds.Extents.y;
        }
    };

    public class PlaneFinding
    {
        #region Public APIs

        public struct MeshData
        {
            public Matrix4x4 Transform;
            public Vector3[] Verts;
            public Vector3[] Normals;
            public Int32[] Indices;

            public MeshData(MeshFilter meshFilter)
            {
                Transform = meshFilter.transform.localToWorldMatrix;
                Verts = meshFilter.sharedMesh.vertices;
                Normals = meshFilter.sharedMesh.normals;
                Indices = meshFilter.sharedMesh.triangles;
            }
        }

        public static BoundedPlane[] FindSubPlanes(List<MeshData> meshes, float snapToGravityThreshold = 0.0f)
        {
            StartPlaneFinding();

            try
            {
                int planeCount;
                IntPtr planesPtr;
                IntPtr pinnedMeshData = PinMeshDataForMarshalling(meshes);
                DLLImports.FindSubPlanes(meshes.Count, pinnedMeshData, snapToGravityThreshold, out planeCount, out planesPtr);
                return MarshalBoundedPlanesFromIntPtr(planesPtr, planeCount);
            }
            finally
            {
                FinishPlaneFinding();
            }
        }

        public static BoundedPlane[] MergeSubPlanes(BoundedPlane[] subPlanes, float snapToGravityThreshold = 0.0f, float minArea = 0.0f)
        {
            StartPlaneFinding();

            try
            {
                int planeCount;
                IntPtr planesPtr;
                DLLImports.MergeSubPlanes(subPlanes.Length, PinObject(subPlanes), minArea, snapToGravityThreshold, out planeCount, out planesPtr);
                return MarshalBoundedPlanesFromIntPtr(planesPtr, planeCount);
            }
            finally
            {
                FinishPlaneFinding();
            }
        }

        public static BoundedPlane[] FindPlanes(List<MeshData> meshes, float snapToGravityThreshold = 0.0f, float minArea = 0.0f)
        {
            StartPlaneFinding();

            try
            {
                int planeCount;
                IntPtr planesPtr;
                IntPtr pinnedMeshData = PinMeshDataForMarshalling(meshes);
                DLLImports.FindPlanes(meshes.Count, pinnedMeshData, minArea, snapToGravityThreshold, out planeCount, out planesPtr);
                return MarshalBoundedPlanesFromIntPtr(planesPtr, planeCount);
            }
            finally
            {
                FinishPlaneFinding();
            }
        }

        #endregion

        #region Internal

        private static bool findPlanesRunning = false;
        private static System.Object findPlanesLock = new System.Object();
        private static DLLImports.ImportedMeshData[] reusedImportedMeshesForMarshalling;
        private static List<GCHandle> reusedPinnedMemoryHandles = new List<GCHandle>();

        private static void StartPlaneFinding()
        {
            lock (findPlanesLock)
            {
                if (findPlanesRunning)
                {
                    throw new Exception("PlaneFinding is already running. You can not call these APIs from multiple threads.");
                }
                findPlanesRunning = true;
            }
        }

        private static void FinishPlaneFinding()
        {
            UnpinAllObjects();
            findPlanesRunning = false;
        }

        private static IntPtr PinObject(System.Object obj)
        {
            GCHandle h = GCHandle.Alloc(obj, GCHandleType.Pinned);
            reusedPinnedMemoryHandles.Add(h);
            return h.AddrOfPinnedObject();
        }

        private static void UnpinAllObjects()
        {
            for (int i = 0; i < reusedPinnedMemoryHandles.Count; ++i)
            {
                reusedPinnedMemoryHandles[i].Free();
            }
            reusedPinnedMemoryHandles.Clear();
        }

        private static IntPtr PinMeshDataForMarshalling(List<MeshData> meshes)
        {
            // if we have a big enough array reuse it, otherwise create new
            if (reusedImportedMeshesForMarshalling == null || reusedImportedMeshesForMarshalling.Length < meshes.Count)
            {
                reusedImportedMeshesForMarshalling = new DLLImports.ImportedMeshData[meshes.Count];
            }

            for (int i = 0; i < meshes.Count; ++i)
            {
                reusedImportedMeshesForMarshalling[i] = new DLLImports.ImportedMeshData
                {
                    transform = meshes[i].Transform,
                    vertCount = meshes[i].Verts.Length,
                    indexCount = meshes[i].Indices.Length,
                    verts = PinObject(meshes[i].Verts),
                    normals = PinObject(meshes[i].Normals),
                    indices = PinObject(meshes[i].Indices),
                };
            }

            return PinObject(reusedImportedMeshesForMarshalling);
        }

        private static BoundedPlane[] MarshalBoundedPlanesFromIntPtr(IntPtr outArray, int size)
        {
            BoundedPlane[] resArray = new BoundedPlane[size];
#pragma warning disable 618
            int structsize = Marshal.SizeOf(typeof(BoundedPlane));
#pragma warning restore 618
            IntPtr current = outArray;
            for (int i = 0; i < size; i++)
            {
#pragma warning disable 618
                resArray[i] = (BoundedPlane)Marshal.PtrToStructure(current, typeof(BoundedPlane));
#pragma warning restore 618
                current = (IntPtr)((long)current + structsize);
            }
            Marshal.FreeCoTaskMem(outArray);
            return resArray;
        }

        private class DLLImports
        {
            [StructLayout(LayoutKind.Sequential)]
            public struct ImportedMeshData
            {
                public Matrix4x4 transform;
                public Int32 vertCount;
                public Int32 indexCount;
                public IntPtr verts;
                public IntPtr normals;
                public IntPtr indices;
            };

            [DllImport("PlaneFinding")]
            public static extern void FindPlanes(
                [In] int meshCount,
                [In] IntPtr meshes,
                [In] float minArea,
                [In] float snapToGravityThreshold,
                [Out] out int planeCount,
                [Out] out IntPtr planesPtr);

            [DllImport("PlaneFinding")]
            public static extern void FindSubPlanes(
                [In] int meshCount,
                [In] IntPtr meshes,
                [In] float snapToGravityThreshold,
                [Out] out int planeCount,
                [Out] out IntPtr planesPtr);

            [DllImport("PlaneFinding")]
            public static extern void MergeSubPlanes(
                [In] int subPlaneCount,
                [In] IntPtr subPlanes,
                [In] float minArea,
                [In] float snapToGravityThreshold,
                [Out] out int planeCount,
                [Out] out IntPtr planesPtr);
        }

        #endregion
    }
}