/// <summary> /// Set the grid index as having been observed from the given direction. /// </summary> /// <param name="gridIndex">Grid index to observe.</param> private void _ViewGridIndex(Tango3DReconstruction.GridIndex gridIndex) { // This update may occur somewhat later than the actual time of the camera pose observation. Vector3 dir = Camera.main.transform.forward; dir = new Vector3(dir.x, 0.0f, dir.z).normalized; Vector3[] directions = new Vector3[] { new Vector3(0, 0, 1), new Vector3(0, 0, -1), new Vector3(1, 0, 0), new Vector3(-1, 0, 0) }; for (int i = 0; i < 4; i++) { // If the camera is facing one of 4 directions (every 90 degrees) within a 45 degree spread, // set that direction as seen. float dot = Vector3.Dot(dir, directions[i]); if (dot > m_minDirectionCheck) { // Bitwise OR new and old directions to show that the mesh has been observed from the new direction. byte direction = (byte)(1 << i); m_meshes[gridIndex].m_directions = (byte)(m_meshes[gridIndex].m_directions | direction); break; } } }
/// <summary> /// Unity Update callback. /// </summary> public void Update() { List <Tango3DReconstruction.GridIndex> needsResize = new List <Tango3DReconstruction.GridIndex>(); int it; int startTimeMS = (int)(Time.realtimeSinceStartup * 1000); for (it = 0; it < m_gridIndexToUpdate.Count; ++it) { Tango3DReconstruction.GridIndex gridIndex = m_gridIndexToUpdate[it]; if (_GoneOverTimeBudget(startTimeMS)) { Debug.Log(string.Format( "TangoDynamicMesh.Update() ran over budget with {0}/{1} grid indexes processed.", it, m_gridIndexToUpdate.Count)); break; } _UpdateMeshAtGridIndex(gridIndex, needsResize); m_gridUpdateBacklog.Remove(gridIndex); } // While we have time left over, go through backlog of unprocessed indices. int numBacklogGridIndicesProcessed = 0; if (!_GoneOverTimeBudget(startTimeMS)) { List <Tango3DReconstruction.GridIndex> processedBacklog = new List <Tango3DReconstruction.GridIndex>(); foreach (Tango3DReconstruction.GridIndex gridIndex in m_gridUpdateBacklog) { _UpdateMeshAtGridIndex(gridIndex, needsResize); processedBacklog.Add(gridIndex); ++numBacklogGridIndicesProcessed; if (_GoneOverTimeBudget(startTimeMS)) { break; } } m_gridUpdateBacklog.ExceptWith(processedBacklog); } m_debugRemeshingTime = Time.realtimeSinceStartup - (startTimeMS * 0.001f); m_debugRemeshingCount = it + numBacklogGridIndicesProcessed; // Any leftover grid indices also need to get processed next frame. while (it < m_gridIndexToUpdate.Count) { needsResize.Add(m_gridIndexToUpdate[it]); ++it; } m_gridIndexToUpdate = needsResize; }
/// <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; } }
/// <summary> /// Update the bounding box. /// </summary> /// <param name="gridIndex">Grid index to include in bounds.</param> private void _UpdateBounds(Tango3DReconstruction.GridIndex gridIndex) { float gridIndexSize = m_tangoApplication.m_3drResolutionMeters * 16; Vector3 pointToCompare = gridIndexSize * new Vector3(gridIndex.x, gridIndex.y, gridIndex.z); Vector3 min = m_bounds.min; Vector3 max = m_bounds.max; if (m_bounds.min.x > pointToCompare.x) { min.x = pointToCompare.x; } if (m_bounds.min.y > pointToCompare.y) { min.y = pointToCompare.y; } if (m_bounds.min.z > pointToCompare.z) { min.z = pointToCompare.z; } if (m_bounds.max.x < pointToCompare.x) { max.x = pointToCompare.x; } if (m_bounds.max.y < pointToCompare.y) { max.y = pointToCompare.y; } if (m_bounds.max.z < pointToCompare.z) { max.z = pointToCompare.z; } m_bounds.SetMinMax(min, max); }
/// <summary> /// Unity Update callback. /// </summary> public void Update() { List <Tango3DReconstruction.GridIndex> needsResize = new List <Tango3DReconstruction.GridIndex>(); int it; int startTimeMS = (int)(Time.realtimeSinceStartup * 1000); for (it = 0; it < m_gridIndexToUpdate.Count; ++it) { Tango3DReconstruction.GridIndex gridIndex = m_gridIndexToUpdate[it]; if ((Time.realtimeSinceStartup * 1000) - startTimeMS > TIME_BUDGET_MS) { Debug.Log(string.Format( "TangoDynamicMesh.Update() ran over budget with {0}/{1} grid indexes processed.", it, m_gridIndexToUpdate.Count)); break; } TangoSingleDynamicMesh dynamicMesh; if (!m_meshes.TryGetValue(gridIndex, out dynamicMesh)) { // build a dynamic mesh as a child of this game object. GameObject newObj = new GameObject(); newObj.transform.parent = transform; newObj.name = string.Format("{0},{1},{2}", gridIndex.x, gridIndex.y, gridIndex.z); dynamicMesh = newObj.AddComponent <TangoSingleDynamicMesh>(); dynamicMesh.m_vertices = new Vector3[INITIAL_VERTEX_COUNT]; if (m_tangoApplication.m_3drGenerateTexCoord) { dynamicMesh.m_uv = new Vector2[INITIAL_VERTEX_COUNT]; } if (m_tangoApplication.m_3drGenerateColor) { dynamicMesh.m_colors = new Color32[INITIAL_VERTEX_COUNT]; } dynamicMesh.m_triangles = new int[INITIAL_INDEX_COUNT]; // Update debug info too. m_debugTotalVertices = dynamicMesh.m_vertices.Length; m_debugTotalTriangles = dynamicMesh.m_triangles.Length; // Add the other necessary objects MeshFilter meshFilter = newObj.AddComponent <MeshFilter>(); dynamicMesh.m_mesh = meshFilter.mesh; if (m_meshRenderer != null) { MeshRenderer meshRenderer = newObj.AddComponent <MeshRenderer>(); #if UNITY_5 meshRenderer.shadowCastingMode = m_meshRenderer.shadowCastingMode; meshRenderer.receiveShadows = m_meshRenderer.receiveShadows; meshRenderer.sharedMaterials = m_meshRenderer.sharedMaterials; meshRenderer.useLightProbes = m_meshRenderer.useLightProbes; meshRenderer.reflectionProbeUsage = m_meshRenderer.reflectionProbeUsage; meshRenderer.probeAnchor = m_meshRenderer.probeAnchor; #elif UNITY_4_6 meshRenderer.castShadows = m_meshRenderer.castShadows; meshRenderer.receiveShadows = m_meshRenderer.receiveShadows; meshRenderer.sharedMaterials = m_meshRenderer.sharedMaterials; meshRenderer.useLightProbes = m_meshRenderer.useLightProbes; meshRenderer.lightProbeAnchor = m_meshRenderer.lightProbeAnchor; #endif } if (m_meshCollider != null) { MeshCollider meshCollider = newObj.AddComponent <MeshCollider>(); meshCollider.convex = m_meshCollider.convex; meshCollider.isTrigger = m_meshCollider.isTrigger; meshCollider.sharedMaterial = m_meshCollider.sharedMaterial; meshCollider.sharedMesh = dynamicMesh.m_mesh; dynamicMesh.m_meshCollider = meshCollider; } m_meshes.Add(gridIndex, dynamicMesh); } // Last frame the mesh needed more space. Give it more room now. if (dynamicMesh.m_needsToGrow) { int newVertexSize = (int)(dynamicMesh.m_vertices.Length * GROWTH_FACTOR); int newTriangleSize = (int)(dynamicMesh.m_triangles.Length * GROWTH_FACTOR); newTriangleSize -= newTriangleSize % 3; // Remove the old size, add the new size. m_debugTotalVertices += newVertexSize - dynamicMesh.m_vertices.Length; m_debugTotalTriangles += newTriangleSize - dynamicMesh.m_triangles.Length; dynamicMesh.m_vertices = new Vector3[newVertexSize]; if (m_tangoApplication.m_3drGenerateTexCoord) { dynamicMesh.m_uv = new Vector2[newVertexSize]; } if (m_tangoApplication.m_3drGenerateColor) { dynamicMesh.m_colors = new Color32[newVertexSize]; } dynamicMesh.m_triangles = new int[newTriangleSize]; dynamicMesh.m_needsToGrow = false; } int numVertices; int numTriangles; Tango3DReconstruction.Status status = m_tangoApplication.Tango3DRExtractMeshSegment( gridIndex, dynamicMesh.m_vertices, null, dynamicMesh.m_colors, dynamicMesh.m_triangles, out numVertices, out numTriangles); if (status != Tango3DReconstruction.Status.INSUFFICIENT_SPACE && status != Tango3DReconstruction.Status.SUCCESS) { Debug.Log("Tango3DR extraction failed, status code = " + status + Environment.StackTrace); continue; } else if (status == Tango3DReconstruction.Status.INSUFFICIENT_SPACE) { // We already spent the time extracting this mesh, let's not spend any more time this frame // to extract the mesh. Debug.Log(string.Format( "TangoDynamicMesh.Update() extraction ran out of space with room for {0} vertexes, {1} indexes.", dynamicMesh.m_vertices.Length, dynamicMesh.m_triangles.Length)); dynamicMesh.m_needsToGrow = true; needsResize.Add(gridIndex); } // Make any leftover triangles degenerate. for (int triangleIt = numTriangles * 3; triangleIt < dynamicMesh.m_triangles.Length; ++triangleIt) { dynamicMesh.m_triangles[triangleIt] = 0; } if (dynamicMesh.m_uv != null) { // Add texture coordinates. for (int vertexIt = 0; vertexIt < numVertices; ++vertexIt) { Vector3 vertex = dynamicMesh.m_vertices[vertexIt]; dynamicMesh.m_uv[vertexIt].x = vertex.x * UV_PER_METERS; dynamicMesh.m_uv[vertexIt].y = (vertex.z + vertex.y) * UV_PER_METERS; } } dynamicMesh.m_mesh.Clear(); dynamicMesh.m_mesh.vertices = dynamicMesh.m_vertices; dynamicMesh.m_mesh.uv = dynamicMesh.m_uv; dynamicMesh.m_mesh.colors32 = dynamicMesh.m_colors; dynamicMesh.m_mesh.triangles = dynamicMesh.m_triangles; if (m_tangoApplication.m_3drGenerateNormal) { dynamicMesh.m_mesh.RecalculateNormals(); } if (dynamicMesh.m_meshCollider != null) { // Force the mesh collider to update too. dynamicMesh.m_meshCollider.sharedMesh = null; dynamicMesh.m_meshCollider.sharedMesh = dynamicMesh.m_mesh; } } m_debugRemeshingTime = Time.realtimeSinceStartup - (startTimeMS * 0.001f); m_debugRemeshingCount = it; // Any leftover grid indices also need to get processed next frame. while (it < m_gridIndexToUpdate.Count) { needsResize.Add(m_gridIndexToUpdate[it]); ++it; } m_gridIndexToUpdate = needsResize; }
/// <summary> /// Extract and update (or create, if it doesn't exist) the mesh at the given grid index. /// </summary> /// <param name="gridIndex">Grid index.</param> /// <param name="needsResize">List to which indices needing a future resize will be added.</param> private void _UpdateMeshAtGridIndex(Tango3DReconstruction.GridIndex gridIndex, List <Tango3DReconstruction.GridIndex> needsResize) { TangoSingleDynamicMesh dynamicMesh; if (!m_meshes.TryGetValue(gridIndex, out dynamicMesh)) { // build a dynamic mesh as a child of this game object. GameObject newObj = new GameObject(); newObj.transform.parent = transform; newObj.name = string.Format("{0},{1},{2}", gridIndex.x, gridIndex.y, gridIndex.z); newObj.layer = gameObject.layer; dynamicMesh = newObj.AddComponent <TangoSingleDynamicMesh>(); dynamicMesh.m_vertices = new Vector3[INITIAL_VERTEX_COUNT]; dynamicMesh.boxes = new GameObject[INITIAL_VERTEX_COUNT]; if (m_tangoApplication.m_3drGenerateTexCoord) { dynamicMesh.m_uv = new Vector2[INITIAL_VERTEX_COUNT]; } if (m_tangoApplication.m_3drGenerateColor) { dynamicMesh.m_colors = new Color32[INITIAL_VERTEX_COUNT]; } dynamicMesh.m_triangles = new int[INITIAL_INDEX_COUNT]; // Update debug info too. m_debugTotalVertices = dynamicMesh.m_vertices.Length; m_debugTotalTriangles = dynamicMesh.m_triangles.Length; // Add the other necessary objects MeshFilter meshFilter = newObj.AddComponent <MeshFilter>(); dynamicMesh.m_mesh = meshFilter.mesh; if (m_meshRenderer != null) { MeshRenderer meshRenderer = newObj.AddComponent <MeshRenderer>(); #if UNITY_5 meshRenderer.shadowCastingMode = m_meshRenderer.shadowCastingMode; meshRenderer.receiveShadows = m_meshRenderer.receiveShadows; meshRenderer.sharedMaterials = m_meshRenderer.sharedMaterials; meshRenderer.useLightProbes = m_meshRenderer.useLightProbes; meshRenderer.reflectionProbeUsage = m_meshRenderer.reflectionProbeUsage; meshRenderer.probeAnchor = m_meshRenderer.probeAnchor; #elif UNITY_4_6 meshRenderer.castShadows = m_meshRenderer.castShadows; meshRenderer.receiveShadows = m_meshRenderer.receiveShadows; meshRenderer.sharedMaterials = m_meshRenderer.sharedMaterials; meshRenderer.useLightProbes = m_meshRenderer.useLightProbes; meshRenderer.lightProbeAnchor = m_meshRenderer.lightProbeAnchor; #endif } if (m_meshCollider != null) { MeshCollider meshCollider = newObj.AddComponent <MeshCollider>(); meshCollider.convex = m_meshCollider.convex; meshCollider.isTrigger = m_meshCollider.isTrigger; meshCollider.sharedMaterial = m_meshCollider.sharedMaterial; meshCollider.sharedMesh = dynamicMesh.m_mesh; dynamicMesh.m_meshCollider = meshCollider; } m_meshes.Add(gridIndex, dynamicMesh); _UpdateBounds(gridIndex); } // Skip updating this grid index if it is considered completed. if (m_enableSelectiveMeshing) { if (dynamicMesh.m_completed) { return; } _ObserveGridIndex(gridIndex, dynamicMesh); } // Last frame the mesh needed more space. Give it more room now. if (dynamicMesh.m_needsToGrow) { int newVertexSize = (int)(dynamicMesh.m_vertices.Length * GROWTH_FACTOR); int newTriangleSize = (int)(dynamicMesh.m_triangles.Length * GROWTH_FACTOR); newTriangleSize -= newTriangleSize % 3; // Remove the old size, add the new size. m_debugTotalVertices += newVertexSize - dynamicMesh.m_vertices.Length; m_debugTotalTriangles += newTriangleSize - dynamicMesh.m_triangles.Length; dynamicMesh.m_vertices = new Vector3[newVertexSize]; dynamicMesh.boxes = new GameObject[newVertexSize]; if (m_tangoApplication.m_3drGenerateTexCoord) { dynamicMesh.m_uv = new Vector2[newVertexSize]; } if (m_tangoApplication.m_3drGenerateColor) { dynamicMesh.m_colors = new Color32[newVertexSize]; } dynamicMesh.m_triangles = new int[newTriangleSize]; dynamicMesh.m_needsToGrow = false; } int numVertices; int numTriangles; Tango3DReconstruction.Status status = m_tangoApplication.Tango3DRExtractMeshSegment( gridIndex, dynamicMesh.m_vertices, null, dynamicMesh.m_colors, dynamicMesh.m_triangles, out numVertices, out numTriangles); if (status != Tango3DReconstruction.Status.INSUFFICIENT_SPACE && status != Tango3DReconstruction.Status.SUCCESS) { Debug.Log("Tango3DR extraction failed, status code = " + status + Environment.StackTrace); return; } else if (status == Tango3DReconstruction.Status.INSUFFICIENT_SPACE) { // We already spent the time extracting this mesh, let's not spend any more time this frame // to extract the mesh. Debug.Log(string.Format( "TangoDynamicMesh.Update() extraction ran out of space with room for {0} vertexes, {1} indexes.", dynamicMesh.m_vertices.Length, dynamicMesh.m_triangles.Length)); dynamicMesh.m_needsToGrow = true; needsResize.Add(gridIndex); } // Make any leftover triangles degenerate. for (int triangleIt = numTriangles * 3; triangleIt < dynamicMesh.m_triangles.Length; ++triangleIt) { dynamicMesh.m_triangles[triangleIt] = 0; } if (dynamicMesh.m_uv != null) { // Add texture coordinates. for (int vertexIt = 0; vertexIt < numVertices; ++vertexIt) { Vector3 vertex = dynamicMesh.m_vertices[vertexIt]; dynamicMesh.m_uv[vertexIt].x = vertex.x * UV_PER_METERS; dynamicMesh.m_uv[vertexIt].y = (vertex.z + vertex.y) * UV_PER_METERS; } } for (int k = 0; k < dynamicMesh.m_vertices.Length; k++) { if (dynamicMesh.boxes [k] != null) { Destroy(dynamicMesh.boxes [k]); } dynamicMesh.boxes [k] = GameObject.CreatePrimitive(PrimitiveType.Cube); dynamicMesh.boxes [k].transform.position = dynamicMesh.m_vertices [k]; dynamicMesh.boxes [k].transform.localScale = new Vector3(0.02f, 0.02f, 0.02f); Color ObjectColor = Color.green; Material materialColored = new Material(Shader.Find("Diffuse")); materialColored.color = ObjectColor; dynamicMesh.boxes [k].GetComponent <Renderer> ().material = materialColored; } dynamicMesh.m_mesh.Clear(); dynamicMesh.m_mesh.vertices = dynamicMesh.m_vertices; dynamicMesh.m_mesh.uv = dynamicMesh.m_uv; dynamicMesh.m_mesh.colors32 = dynamicMesh.m_colors; dynamicMesh.m_mesh.triangles = dynamicMesh.m_triangles; if (m_tangoApplication.m_3drGenerateNormal) { dynamicMesh.m_mesh.RecalculateNormals(); } if (dynamicMesh.m_meshCollider != null) { // Force the mesh collider to update too. dynamicMesh.m_meshCollider.sharedMesh = null; dynamicMesh.m_meshCollider.sharedMesh = dynamicMesh.m_mesh; } }