Пример #1
0
    /// <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;
    }
Пример #2
0
    /// <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;
            }
        }
    }
Пример #3
0
    /// <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;
        }
    }