/// <summary>
/// Gets the highest point on the dynamic mesh through at a given position.
///
/// Raycast against a subset of TangoSingleDynamicMesh colliders and find the highest point. The subset
/// is defined by all the meshes intersected by a downward-pointing ray that passes through a position.
/// </summary>
/// <returns>The highest raycast hit point.</returns>
/// <param name="position">The position to cast a ray through.</param>
/// <param name="maxDistance">The max distance of the ray.</param>
public Vector3 GetHighestRaycastHitPoint(Vector3 position, float maxDistance)
{
if (GetComponent<Collider>() == null)
{
return position;
}
Vector3 topHitPoint = position;
Ray ray = new Ray(position + (Vector3.up * (maxDistance / 2)), Vector3.down);
// Find the starting grid index X and Y components.
float gridIndexSize = m_tangoApplication.m_3drResolutionMeters * 16;
int gridIndexX = Mathf.FloorToInt(position.x / gridIndexSize);
int gridIndexY = Mathf.FloorToInt(position.z / gridIndexSize);
// Find the top and bottom grid indices that are overlapped by a raycast downward through the position.
int topZ = Mathf.FloorToInt(ray.origin.y / gridIndexSize);
int btmZ = Mathf.FloorToInt((ray.origin.y - maxDistance) / gridIndexSize);
// Perform a raycast on each TangoSingleDynamicMesh collider the ray passes through.
for (int i = btmZ; i <= topZ; i++)
{
Tango3DReconstruction.GridIndex newGridIndex = new Tango3DReconstruction.GridIndex();
newGridIndex.x = gridIndexX;
newGridIndex.y = gridIndexY;
newGridIndex.z = i;
// Find the mesh associated with the grid index if available. Raycast to the attached collider.
TangoSingleDynamicMesh singleDynamicMesh;
if (m_meshes.TryGetValue(newGridIndex, out singleDynamicMesh))
{
Collider c = singleDynamicMesh.GetComponent<Collider>();
RaycastHit hit;
if (c.Raycast(ray, out hit, maxDistance))
{
// Update the highest position if the new raycast hit is above. Reject the hit if the normal is orthogonal to the up
// direction (to prevent the object from unintentionally climbing up walls).
if ((hit.point.y > topHitPoint.y) && (Vector3.Dot(hit.normal, Vector3.up) > 0.1f))
{
topHitPoint = hit.point;
}
}
}
}
return topHitPoint;
}
/// <summary>
/// When the grid index has been updated, also determine whether it should be considered completed
/// based on its neighboring grid indices, number of observations, and mesh completeness.
///
/// When checking a grid index for completeness, the observation count of neighboring grid indices is checked.
/// If all grid indices contained in one of the configurations have a sufficient number of observations,
/// the grid index is considered complete.
/// </summary>
/// <param name="gridIndex">Grid index to observe.</param>
/// <param name="singleMesh">TangoSingleDynamicMesh to update and observe.</param>
private void _ObserveGridIndex(Tango3DReconstruction.GridIndex gridIndex, TangoSingleDynamicMesh singleMesh)
{
// Increment the observations made for this grid index.
singleMesh.m_observations++;
// Add observation based on the direction of the observation.
_ViewGridIndex(gridIndex);
// Exit if the grid index has not been observed from all 8 directions.
if (singleMesh.m_directions != DIRECTIONS_COMPLETE)
{
return;
}
// Run through each grid index configuration and check if the grid index is complete.
for (int i = 0; i < m_gridIndexConfigs.Length; i++)
{
Vector3[] config = m_gridIndexConfigs[i];
bool neighborsObserved = true;
foreach (Vector3 nPosition in config)
{
Tango3DReconstruction.GridIndex neighbor = new Tango3DReconstruction.GridIndex();
neighbor.x = (Int32)(nPosition.x + gridIndex.x);
neighbor.y = (Int32)(nPosition.y + gridIndex.y);
neighbor.z = (Int32)(nPosition.z + gridIndex.z);
TangoSingleDynamicMesh nSingleMesh;
if (m_meshes.TryGetValue(neighbor, out nSingleMesh))
{
if (nSingleMesh.m_observations < NUM_OBSERVATIONS_TO_COMPLETE)
{
neighborsObserved = false;
break;
}
}
}
// Complete using this configurations of the neighbors with sufficient observations.
if (neighborsObserved)
{
// Add the grid index to the completed list, so it will be skipped during next mesh update.
singleMesh.m_completed = true;
return;
}
}
}
/// <summary>
/// Gets each single dynamic mesh and fills out arrays with properties. Each mesh corresponds to the same index in each array.
/// </summary>
/// <param name="gridIndices">Filled out with grid index of each mesh.</param>
/// <param name="completed">Filled out with completion state of each mesh.</param>
/// <param name="completionScale">Filled out with amount that each mesh has been completed.</param>
/// <param name="directions">Filled out with a byte representation of the observed directions of each mesh.</param>
public void GetSingleMeshProperties(out Tango3DReconstruction.GridIndex[] gridIndices, out bool[] completed, out float[] completionScale, out byte[] directions)
{
int numIndices = m_meshes.Count;
gridIndices = new Tango3DReconstruction.GridIndex[numIndices];
completed = new bool[numIndices];
completionScale = new float[numIndices];
directions = new byte[numIndices];
// Assign mesh properties to each index of the arrays.
m_meshes.Keys.CopyTo(gridIndices, 0);
for (int i = 0; i < numIndices; i++)
{
TangoSingleDynamicMesh mesh = m_meshes[gridIndices[i]];
completed[i] = mesh.m_completed;
completionScale[i] = 1.0f * mesh.m_observations / NUM_OBSERVATIONS_TO_COMPLETE;
directions[i] = mesh.m_directions;
}
}
