/// <summary>
/// Looks at every node using a depth first search to see if the bounding plane they describe is the same as the one passed in.
/// </summary>
/// <param name="plane"></param>
/// <returns></returns>
private List <Node> FindAllNodesWithPlane(BoundedPlane plane)
{
List <Node> found = new List <Node>();
List <Node> toSearch = new List <Node>();
if (m_RootNode != null)
{
toSearch.Add(m_RootNode);
}
while (toSearch.Count > 0)
{
Node currentNode = toSearch[0];
toSearch.RemoveAt(0);
if (currentNode.Children[0] != null)
{
toSearch.Add(currentNode.Children[0]);
}
if (currentNode.Children[1] != null)
{
toSearch.Add(currentNode.Children[1]);
}
if (currentNode.Plane.Bounds.Center == plane.Bounds.Center &&
currentNode.Plane.Bounds.Extents == plane.Bounds.Extents &&
currentNode.Plane.Bounds.Rotation == plane.Bounds.Rotation)
{
found.Add(currentNode);
}
}
return(found);
}
private void SendPlaneEvent(int type, BoundedPlane Plane)
{
if (dataSender != null)
{
dataSender.SendPlane(connectionProvider, type, Plane);
}
}
} // PlaneAddedHandler() //
protected virtual void PlaneUpdatedHandler(BoundedPlane plane)
{
if (m_PlanePrefab)
{
CreateOrUpdateGameObject(plane);
}
} // PlaneUpdatedHandler() //
void UpdateMapPosOnY(BoundedPlane plane)
{
var pos = _mapRoot.position;
// _mapRoot.position = new Vector3(pos.x, pos.y, pos.z);
_mapRoot.position = new Vector3(pos.x, plane.min.y, pos.z);
}
//-----------------------------------------------------------------------------------------------
#region Public Static Functions
/// <summary>
/// Gets the possible types a plane might be. This does not take into account the current floor or ceiling height.
/// </summary>
/// <param name="bounds"></param>
/// <param name="upNormalThreshold"></param>
/// <returns></returns>
public static PlaneTypes GetPossibleType(BoundedPlane bounds, float upNormalThreshold = 0.9f)
{
PlaneTypes type;
Vector3 surfaceNormal = bounds.Plane.normal;
// Determine what type of plane this is.
// Use the upNormalThreshold to help determine if we have a horizontal or vertical surface.
if (surfaceNormal.y >= upNormalThreshold)
{
type = PlaneTypes.Floor | PlaneTypes.Table;
}
else if (surfaceNormal.y <= -(upNormalThreshold))
{
type = PlaneTypes.Ceiling | PlaneTypes.Table;
}
else if (Mathf.Abs(surfaceNormal.y) <= (1 - upNormalThreshold))
{
// If the plane is vertical, then classify it as a wall.
type = PlaneTypes.Wall;
}
else
{
// The plane has a strange angle, classify it as 'unknown'.
type = PlaneTypes.Unknown;
}
return(type);
}
public override unsafe TrackableChanges <BoundedPlane> GetChanges(
BoundedPlane defaultPlane,
Allocator allocator)
{
int addedLength, updatedLength, removedLength, elementSize;
void *addedPtr, updatedPtr, removedPtr;
var context = UnityARCore_planeTracking_acquireChanges(
out addedPtr, out addedLength,
out updatedPtr, out updatedLength,
out removedPtr, out removedLength,
out elementSize);
try
{
return(new TrackableChanges <BoundedPlane>(
addedPtr, addedLength,
updatedPtr, updatedLength,
removedPtr, removedLength,
defaultPlane, elementSize,
allocator));
}
finally
{
UnityARCore_planeTracking_releaseChanges(context);
}
}
protected virtual void OnPlaneUpdated(BoundedPlane plane)
{
if (m_PlanePrefab)
{
CreateOrUpdateGameObject(plane);
}
}
public override unsafe TrackableChanges <BoundedPlane> GetChanges(
BoundedPlane defaultPlane,
Allocator allocator)
{
int addedLength, updatedLength, removedLength, elementSize;
void *addedArrayPtr, updatedArrayPtr, removedArrayPtr;
var context = NativeApi.UnityARKit_Planes_AcquireChanges(
out addedArrayPtr, out addedLength,
out updatedArrayPtr, out updatedLength,
out removedArrayPtr, out removedLength,
out elementSize);
try
{
return(new TrackableChanges <BoundedPlane>(
addedArrayPtr, addedLength,
updatedArrayPtr, updatedLength,
removedArrayPtr, removedLength,
defaultPlane, elementSize,
allocator));
}
finally
{
NativeApi.UnityARKit_Planes_ReleaseChanges(context);
}
}
protected virtual void CreateOrUpdateGameObject(BoundedPlane plane)
{
lock (lockObject)
{
GameObject go;
if (!m_Planes.TryGetValue(plane.id, out go))
{
//CommandKeeper.WriteLineDebug("Create or update");
planesRequestTime[plane.id] = Time.time + timeForPlaneToBeCreated;
planesCreationBuffer[plane.id] = plane;
//go = Instantiate(m_PlanePrefab, planesRoot);
//// Make sure we can pick them later
//foreach (var collider in go.GetComponentsInChildren<Collider>())
//{
// collider.gameObject.layer = m_PlaneLayer;
//}
//m_Planes.Add(plane.id, go);
//go.SetActive(planesAreEnabled);
//CommandKeeper.WriteLineDebug("Plane added: " + plane.id);
}
else
{
SetPlanePosition(go, plane);
}
} // lock //
} // CreateOrUpdateGameObject() //
void CheckCoordinates(BoundedPlane plane)
{
_position = plane.center;
_rotation = Quaternion.Inverse(plane.rotation);
_localScale = new Vector3(plane.extents.x, 10, plane.extents.y);
UpdateShaderValues(_position, _localScale, _rotation);
}
void PlaceMap(BoundedPlane plane)
{
if (!_mapTransform.gameObject.activeSelf)
{
_mapTransform.gameObject.SetActive(true);
}
_mapTransform.position = plane.center;
}
void PlaneAddedHandler(BoundedPlane plane)
{
var newPosition = transform.localPosition;
newPosition.y = plane.center.y;
transform.position = newPosition;
ARInterface.planeAdded -= PlaneAddedHandler;
}
private static void AssertExpectedPlane(BoundedPlane plane, Vector3 bounds, Matrix4x4 vertDataTransform, Matrix4x4 meshTransform)
{
float expectedArea = 4.0f * bounds.x * bounds.y;
Vector3 expectedPlaneNormal = meshTransform.TransformDirection(vertDataTransform.TransformDirection(new Vector3(0, 0, 1)));
Vector3 expectedPlaneCenter = meshTransform.TransformPoint(vertDataTransform.TransformPoint(Vector3.zero));
Assert.AreEqual(expectedArea, plane.Area, expectedArea * 0.01f);
AssertAreEqual(expectedPlaneNormal, plane.Plane.normal, 0.01f, "Normal");
AssertAreEqual(expectedPlaneCenter, plane.Bounds.Center, bounds.Length() * 0.01f, "Center");
}
/// <summary>
/// Removes all planes from the tree that match the bounded plane passed in.
/// </summary>
/// <param name="plane"></param>
public void Remove(BoundedPlane plane)
{
List <Node> toRemove = FindAllNodesWithPlane(plane);
while (toRemove.Count > 0)
{
RemoveNode(toRemove[0]);
toRemove.RemoveAt(0);
}
}
protected virtual void OnPlaneRemoved(BoundedPlane plane)
{
GameObject go;
if (m_Planes.TryGetValue(plane.id, out go))
{
Destroy(go);
m_Planes.Remove(plane.id);
}
}
/// <summary>
/// Checks the list of passed in planes for one that might match the passed in bounding plane.
/// </summary>
/// <param name="planes"></param>
/// <param name="plane"></param>
/// <returns></returns>
private SurfacePlane CheckForExistingPlane(List <SurfacePlane> planes, BoundedPlane plane)
{
SurfacePlane bestMatch = null;
float bestAreaDiff = float.MaxValue;
float bestDistance = float.MaxValue;
float bestDistPercent = float.MaxValue;
PlaneTypes type = GetPossibleType(plane, m_UpNormalThreshold);
foreach (SurfacePlane possiblePlane in planes)
{
if ((possiblePlane.PlaneType & type) == 0)
{
//Skip this one.
continue;
}
//What is the area difference?
float areaDiff = Mathf.Abs(possiblePlane.Plane.Area - plane.Area);
float areaDiffPercent = areaDiff / ((possiblePlane.Plane.Area + plane.Area) / 2);
//What is the distance difference?
float distDiff = (possiblePlane.Plane.Bounds.Center - plane.Bounds.Center).sqrMagnitude;
float distChangePercent = distDiff / (possiblePlane.Plane.Bounds.Center.sqrMagnitude + plane.Bounds.Center.sqrMagnitude) / 2;
if (areaDiffPercent >= m_MaxAreaDiffPercent || distDiff > m_MaxDistChange)
{
//The difference in these planes are to different so we can ignore this one.
continue;
}
else if (areaDiffPercent < bestAreaDiff && distDiff < bestDistance)
{
bestMatch = possiblePlane;
bestAreaDiff = areaDiffPercent;
bestDistPercent = distChangePercent;
distDiff = bestDistance;
}
else if (areaDiffPercent < bestAreaDiff && areaDiffPercent <= bestDistPercent)
{
bestMatch = possiblePlane;
bestAreaDiff = areaDiffPercent;
bestDistPercent = distChangePercent;
distDiff = bestDistance;
}
else if (distDiff < bestDistance && distChangePercent <= areaDiffPercent)
{
bestMatch = possiblePlane;
bestAreaDiff = areaDiffPercent;
bestDistPercent = distChangePercent;
distDiff = bestDistance;
}
}
return(bestMatch);
}
public override bool GetPlane(out BoundedPlane plane)
{
if (!string.IsNullOrEmpty(currentPlane.id))
{
plane = currentPlane;
return(true);
}
plane = new BoundedPlane();
return(false);
}
public BoundedPlane ToBoundedPlane(BoundedPlane defaultPlane)
{
return(new BoundedPlane(
this.id,
this.subsumedById,
this.pose,
this.center,
this.bounds,
GetAlignment(this.pose),
this.trackingState,
defaultPlane.nativePtr));
}
/// <summary>
/// Generate UV2s to mark the boundary vertices and feathering UV coords.
/// </summary>
/// <remarks>
/// The <c>ARPlaneMeshVisualizer</c> has a <c>meshUpdated</c> event that can be used to modify the generated
/// mesh. In this case we'll add UV2s to mark the boundary vertices.
/// This technique avoids having to generate extra vertices for the boundary. It works best when the shape is
/// is fairly uniform and the center vert is close to the barycentric center.
/// </remarks>
/// <param name="mesh">The <c>Mesh</c> generated by <c>ARPlaneMeshVisualizer</c></param>
void GenerateBoundaryUVs(Mesh mesh)
{
int vertexCount = mesh.vertexCount;
// Reuse the list of UVs
s_FeatheringUVs.Clear();
if (s_FeatheringUVs.Capacity < vertexCount)
{
s_FeatheringUVs.Capacity = vertexCount;
}
// Figure out where the plane center is in plane-local space (it's in session-local space)
BoundedPlane boundedPlane = m_ARPlane.boundedPlane;
Pose planePose = boundedPlane.Pose;
Vector3 centerInPlaneSpace = planePose.InverseTransformPosition(boundedPlane.Center);
mesh.GetVertices(s_Vertices);
Vector3 uv = new Vector3(0, 0, 0);
float shortestUVMapping = float.MaxValue;
// Assume the last vertex is the center vertex.
for (int i = 0; i < vertexCount - 1; i++)
{
float vertexDist = Vector3.Distance(planePose.InverseTransformPosition(s_Vertices[i]), centerInPlaneSpace);
// Remap the UV so that a UV of "1" marks the feathering boudary.
// The ratio of featherBoundaryDistance/edgeDistance is the same as featherUV/edgeUV.
// Rearrange to get the edge UV.
float uvMapping = vertexDist / (vertexDist - featheringWidth);
uv.x = uvMapping;
// All the UV mappings will be different. In the shader we need to know the UV value we need to fade out by.
// Choose the shortest UV to guarentee we fade out before the border.
// This means the feathering widths will be slightly different, we again rely on a fairly uniform plane.
if (shortestUVMapping > uvMapping)
{
shortestUVMapping = uvMapping;
}
s_FeatheringUVs.Add(uv);
}
m_FeatheredPlaneMaterial.SetFloat("_ShortestUVMapping", shortestUVMapping);
// Add the center vertex UV
uv.Set(0, 0, 0);
s_FeatheringUVs.Add(uv);
mesh.SetUVs(1, s_FeatheringUVs);
mesh.UploadMeshData(false);
}
protected virtual void PlaneRemovedHandler(BoundedPlane plane)
{
if (firstPlaneId == null)
{
Debug.LogError("PlaneRemove without firstPlaneId");
return;
}
if (firstPlaneId == plane.id)
{
Debug.LogWarning("PlaneRemove of firstPlaneId. Infinit Plane stays as it were before being removed");
}
}
//---------------------------------------------------------------------------------------
#region Public Functions
/// <summary>
/// Adds a bounding plane and user data to the tree. (Note if they are already in the tree they will not be added again as they already exist.)
/// </summary>
/// <param name="plane"></param>
/// <param name="data"></param>
public void Add(BoundedPlane plane, T data)
{
Node newNode = new Node(plane, data);
if (m_RootNode == null)
{
m_RootNode = newNode;
}
else
{
RecursiveInsert(m_RootNode, newNode);
}
}
ARPlane AddPlane(BoundedPlane boundedPlane)
{
var go = CreateGameObject();
var plane = go.GetComponent <ARPlane>();
if (plane == null)
{
plane = go.AddComponent <ARPlane>();
}
m_Planes.Add(boundedPlane.Id, plane);
return(plane);
}
public override TrackableChanges <BoundedPlane> GetChanges(BoundedPlane defaultPlane, Allocator allocator)
{
var changes = new TrackableChanges <BoundedPlane>(m_AddedPlanes.Count, m_UpdatedPlanes.Count, m_RemovedPlanes.Count, allocator);
var numAddedPlanes = m_AddedPlanes.Count;
if (numAddedPlanes > 0)
{
Utils.EnsureCapacity(ref m_ConversionBuffer, numAddedPlanes);
var i = 0;
foreach (var plane in m_AddedPlanes.Values)
{
m_ConversionBuffer[i++] = BoundedPlaneFromMRPlane(plane);
}
NativeArray <BoundedPlane> .Copy(m_ConversionBuffer, changes.added, numAddedPlanes);
m_AddedPlanes.Clear();
}
var numUpdatedPlanes = m_UpdatedPlanes.Count;
if (numUpdatedPlanes > 0)
{
Utils.EnsureCapacity(ref m_ConversionBuffer, numUpdatedPlanes);
var i = 0;
foreach (var plane in m_UpdatedPlanes.Values)
{
m_ConversionBuffer[i++] = BoundedPlaneFromMRPlane(plane);
}
NativeArray <BoundedPlane> .Copy(m_ConversionBuffer, changes.updated, numUpdatedPlanes);
m_UpdatedPlanes.Clear();
}
var numRemovedPlanes = m_RemovedPlanes.Count;
if (numRemovedPlanes > 0)
{
Utils.EnsureCapacity(ref m_IdConversionBuffer, numRemovedPlanes);
var i = 0;
foreach (var id in m_RemovedPlanes)
{
m_IdConversionBuffer[i++] = new TrackableId(id.subId1, id.subId2);
}
NativeArray <TrackableId> .Copy(m_IdConversionBuffer, changes.removed, numRemovedPlanes);
m_RemovedPlanes.Clear();
}
return(changes);
}
public override bool GetPlane(out BoundedPlane plane)
{
RaycastHit hit;
if (Physics.Raycast(Camera.main.ScreenPointToRay(Input.mousePosition), out hit, Mathf.Infinity, 1 << LayerMask.NameToLayer("ARGameObject")))
{
plane = new BoundedPlane()
{
Center = hit.point, Pose = new Pose(hit.point, hit.transform.rotation), Size = new Vector2(10f, 10f)
};
return(true);
}
plane = new BoundedPlane();
return(false);
}
protected virtual void PlaneAddedHandler(BoundedPlane plane)
{
if (firstPlaneId != null)
{
return;
}
firstPlaneId = plane.id;
firstPlane = plane;
if (DEBUG)
{
Debug.Log("PlaneAdded c:" + firstPlane.center + " exts:" + firstPlane.extents);
}
state.SetState(State.FIRST_PLANE_DETECTED);
onFirstPlaneDetected.Invoke(plane);
}
void PlaceMap(BoundedPlane plane)
{
_pointScript.enabled = false;
_planeScript.enabled = false;
var oldMask = _camera.cullingMask;
var newMask = oldMask & ~(1 << 8);
_camera.cullingMask = newMask;
if (!_mapTransform.gameObject.activeSelf)
{
_mapTransform.gameObject.SetActive(true);
}
_mapTransform.position = plane.center;
}
/// <summary>
/// Marshals BoundedPlane data returned from a DLL API call into a managed BoundedPlane array
/// and then frees the memory that was allocated within the DLL.
/// </summary>
/// <remarks>Disabling warning 618 when calling Marshal.SizeOf(), because
/// Unity does not support .Net 4.5.1+ for using the preferred Marshal.SizeOf(T) method."/>, </remarks>
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);
}
static BoundedPlane BoundedPlaneFromMRPlane(MRPlane mrPlane)
{
var id = new TrackableId(mrPlane.id.subId1, mrPlane.id.subId2);
PlaneAlignment alignment;
switch (mrPlane.alignment)
{
case MarsPlaneAlignment.HorizontalUp:
alignment = PlaneAlignment.HorizontalUp;
break;
case MarsPlaneAlignment.HorizontalDown:
alignment = PlaneAlignment.HorizontalDown;
break;
case MarsPlaneAlignment.Vertical:
alignment = PlaneAlignment.Vertical;
break;
case MarsPlaneAlignment.NonAxis:
alignment = PlaneAlignment.NotAxisAligned;
break;
default:
alignment = PlaneAlignment.None;
break;
}
var bp = new BoundedPlane(id,
TrackableId.invalidId,
mrPlane.pose,
mrPlane.center,
mrPlane.extents,
alignment,
TrackingState.Tracking,
#if ARSUBSYSTEMS_3_OR_NEWER
IntPtr.Zero,
PlaneClassification.None);
#else
IntPtr.Zero);
#endif
return(bp);
}
public void SendPlane(IConnectionProvider connectionProvider, int type, BoundedPlane Plane)
{
writer.BeginMessage(RemoteMessage.ARPlaneData);
writer.Write((DateTime.UtcNow - ARRemotePath.connectedTime).TotalSeconds);
writer.Write(Plane.Center.x);
writer.Write(Plane.Center.y);
writer.Write(Plane.Center.z);
writer.Write(Plane.Pose.position.x);
writer.Write(Plane.Pose.position.y);
writer.Write(Plane.Pose.position.z);
writer.Write(Plane.Pose.rotation.x);
writer.Write(Plane.Pose.rotation.y);
writer.Write(Plane.Pose.rotation.z);
writer.Write(Plane.Pose.rotation.w);
writer.Write(Plane.Size.x);
writer.Write(Plane.Size.y);
writer.Write((int)Plane.Alignment);
writer.Write(type);
byte[] t = StructToByteArray <TrackableId>(Plane.Id);
IdToArray idToArray = ByteArrayToType <IdToArray>(t);
writer.Write(idToArray.id0);
writer.Write(idToArray.id1);
byte[] subsumedToByte = StructToByteArray <TrackableId>(Plane.SubsumedById);
IdToArray subsumedToArray = ByteArrayToType <IdToArray>(subsumedToByte);
writer.Write(subsumedToArray.id0);
writer.Write(subsumedToArray.id1);
writer.EndMessage(stream);
connectionProvider.SendMessage(stream);
}
protected virtual void PlaneUpdatedHandler(BoundedPlane plane)
{
if (firstPlaneId == null)
{
Debug.LogError("PlaneUpdate without firstPlaneId");
return;
}
if (firstPlaneId == plane.id)
{
// Update plane position & rotation - until selecting a point
firstPlane = plane;
if (DEBUG)
{
Debug.Log("PlaneUpd c:" + firstPlane.center + " exts:" + firstPlane.extents);
}
onFirstPlaneUpdated.Invoke(plane);
}
}
