예제 #1
0
        public void VoxelizeInput(Pathfinding.Util.GraphTransform graphTransform, Bounds graphSpaceBounds)
        {
            AstarProfiler.StartProfile("Build Navigation Mesh");

            AstarProfiler.StartProfile("Voxelizing - Step 1");

            // Transform from voxel space to graph space.
            // then scale from voxel space (one unit equals one voxel)
            // Finally add min
            Matrix4x4 voxelMatrix = Matrix4x4.TRS(graphSpaceBounds.min, Quaternion.identity, Vector3.one) * Matrix4x4.Scale(new Vector3(cellSize, cellHeight, cellSize));

            transformVoxel2Graph = new Pathfinding.Util.GraphTransform(voxelMatrix);

            // Transform from voxel space to world space
            // add half a voxel to fix rounding
            transform = graphTransform * voxelMatrix * Matrix4x4.TRS(new Vector3(0.5f, 0, 0.5f), Quaternion.identity, Vector3.one);

            int maximumVoxelYCoord = (int)(graphSpaceBounds.size.y / cellHeight);

            AstarProfiler.EndProfile("Voxelizing - Step 1");

            AstarProfiler.StartProfile("Voxelizing - Step 2 - Init");

            // Cosine of the slope limit in voxel space (some tweaks are needed because the voxel space might be stretched out along the y axis)
            float slopeLimit = Mathf.Cos(Mathf.Atan(Mathf.Tan(maxSlope * Mathf.Deg2Rad) * (cellSize / cellHeight)));

            // Temporary arrays used for rasterization
            var clipperOrig = new VoxelPolygonClipper(3);
            var clipperX1   = new VoxelPolygonClipper(7);
            var clipperX2   = new VoxelPolygonClipper(7);
            var clipperZ1   = new VoxelPolygonClipper(7);
            var clipperZ2   = new VoxelPolygonClipper(7);

            if (inputMeshes == null)
            {
                throw new System.NullReferenceException("inputMeshes not set");
            }

            // Find the largest lengths of vertex arrays and check for meshes which can be skipped
            int maxVerts = 0;

            for (int m = 0; m < inputMeshes.Count; m++)
            {
                maxVerts = System.Math.Max(inputMeshes[m].vertices.Length, maxVerts);
            }

            // Create buffer, here vertices will be stored multiplied with the local-to-voxel-space matrix
            var verts = new Vector3[maxVerts];

            AstarProfiler.EndProfile("Voxelizing - Step 2 - Init");

            AstarProfiler.StartProfile("Voxelizing - Step 2");

            // This loop is the hottest place in the whole rasterization process
            // it usually accounts for around 50% of the time
            for (int m = 0; m < inputMeshes.Count; m++)
            {
                RasterizationMesh mesh = inputMeshes[m];
                var meshMatrix         = mesh.matrix;

                // Flip the orientation of all faces if the mesh is scaled in such a way
                // that the face orientations would change
                // This happens for example if a mesh has a negative scale along an odd number of axes
                // e.g it happens for the scale (-1, 1, 1) but not for (-1, -1, 1) or (1,1,1)
                var flipOrientation = VectorMath.ReversesFaceOrientations(meshMatrix);

                Vector3[] vs         = mesh.vertices;
                int[]     tris       = mesh.triangles;
                int       trisLength = mesh.numTriangles;

                // Transform vertices first to world space and then to voxel space
                for (int i = 0; i < vs.Length; i++)
                {
                    verts[i] = transform.InverseTransform(meshMatrix.MultiplyPoint3x4(vs[i]));
                }

                int mesharea = mesh.area;

                for (int i = 0; i < trisLength; i += 3)
                {
                    Vector3 p1 = verts[tris[i]];
                    Vector3 p2 = verts[tris[i + 1]];
                    Vector3 p3 = verts[tris[i + 2]];

                    if (flipOrientation)
                    {
                        var tmp = p1;
                        p1 = p3;
                        p3 = tmp;
                    }

                    int minX = (int)(Utility.Min(p1.x, p2.x, p3.x));
                    int minZ = (int)(Utility.Min(p1.z, p2.z, p3.z));

                    int maxX = (int)System.Math.Ceiling(Utility.Max(p1.x, p2.x, p3.x));
                    int maxZ = (int)System.Math.Ceiling(Utility.Max(p1.z, p2.z, p3.z));

                    minX = Mathf.Clamp(minX, 0, voxelArea.width - 1);
                    maxX = Mathf.Clamp(maxX, 0, voxelArea.width - 1);
                    minZ = Mathf.Clamp(minZ, 0, voxelArea.depth - 1);
                    maxZ = Mathf.Clamp(maxZ, 0, voxelArea.depth - 1);

                    // Check if the mesh is completely out of bounds
                    if (minX >= voxelArea.width || minZ >= voxelArea.depth || maxX <= 0 || maxZ <= 0)
                    {
                        continue;
                    }

                    Vector3 normal;

                    int area;

                    //AstarProfiler.StartProfile ("Rasterize...");

                    normal = Vector3.Cross(p2 - p1, p3 - p1);

                    float cosSlopeAngle = Vector3.Dot(normal.normalized, Vector3.up);

                    if (cosSlopeAngle < slopeLimit)
                    {
                        area = UnwalkableArea;
                    }
                    else
                    {
                        area = 1 + mesharea;
                    }

                    clipperOrig[0] = p1;
                    clipperOrig[1] = p2;
                    clipperOrig[2] = p3;
                    clipperOrig.n  = 3;

                    for (int x = minX; x <= maxX; x++)
                    {
                        clipperOrig.ClipPolygonAlongX(ref clipperX1, 1f, -x + 0.5f);

                        if (clipperX1.n < 3)
                        {
                            continue;
                        }

                        clipperX1.ClipPolygonAlongX(ref clipperX2, -1F, x + 0.5F);

                        if (clipperX2.n < 3)
                        {
                            continue;
                        }

                        float clampZ1 = clipperX2.z[0];
                        float clampZ2 = clipperX2.z[0];
                        for (int q = 1; q < clipperX2.n; q++)
                        {
                            float val = clipperX2.z[q];
                            clampZ1 = System.Math.Min(clampZ1, val);
                            clampZ2 = System.Math.Max(clampZ2, val);
                        }

                        int clampZ1I = Mathf.Clamp((int)System.Math.Round(clampZ1), 0, voxelArea.depth - 1);
                        int clampZ2I = Mathf.Clamp((int)System.Math.Round(clampZ2), 0, voxelArea.depth - 1);

                        for (int z = clampZ1I; z <= clampZ2I; z++)
                        {
                            clipperX2.ClipPolygonAlongZWithYZ(ref clipperZ1, 1F, -z + 0.5F);

                            if (clipperZ1.n < 3)
                            {
                                continue;
                            }

                            clipperZ1.ClipPolygonAlongZWithY(ref clipperZ2, -1F, z + 0.5F);
                            if (clipperZ2.n < 3)
                            {
                                continue;
                            }

                            float sMin = clipperZ2.y[0];
                            float sMax = clipperZ2.y[0];
                            for (int q = 1; q < clipperZ2.n; q++)
                            {
                                float val = clipperZ2.y[q];
                                sMin = System.Math.Min(sMin, val);
                                sMax = System.Math.Max(sMax, val);
                            }

                            int maxi = (int)System.Math.Ceiling(sMax);

                            // Skip span if below or above the bounding box
                            if (maxi >= 0 && sMin <= maximumVoxelYCoord)
                            {
                                // Make sure mini >= 0
                                int mini = System.Math.Max(0, (int)sMin);

                                // Make sure the span is at least 1 voxel high
                                maxi = System.Math.Max(mini + 1, maxi);

                                voxelArea.AddLinkedSpan(z * voxelArea.width + x, (uint)mini, (uint)maxi, area, voxelWalkableClimb);
                            }
                        }
                    }
                }
                //AstarProfiler.EndFastProfile(0);
                //AstarProfiler.EndProfile ("Rasterize...");
            }
            AstarProfiler.EndProfile("Voxelizing - Step 2");
        }
예제 #2
0
        public void VoxelizeInput()
        {
            AstarProfiler.StartProfile("Build Navigation Mesh");

            AstarProfiler.StartProfile("Voxelizing - Step 1");

            Vector3 min = forcedBounds.min;

            voxelOffset = min;

            int maximumVoxelYCoord = (int)(forcedBounds.size.y / cellHeight);

            // Scale factor from world space to voxel space
            float ics = 1F / cellSize;
            float ich = 1F / cellHeight;

            AstarProfiler.EndProfile("Voxelizing - Step 1");

            AstarProfiler.StartProfile("Voxelizing - Step 2 - Init");

            float slopeLimit = Mathf.Cos(Mathf.Atan(Mathf.Tan(maxSlope * Mathf.Deg2Rad) * (ich * cellSize)));

            // Temporary arrays used for rasterization
            float[] vTris    = new float[3 * 3];
            float[] vOut     = new float[7 * 3];
            float[] vRow     = new float[7 * 3];
            float[] vCellOut = new float[7 * 3];
            float[] vCell    = new float[7 * 3];

            if (inputMeshes == null)
            {
                throw new System.NullReferenceException("inputMeshes not set");
            }

            //Find the largest lengths of vertex arrays and check for meshes which can be skipped
            int maxVerts = 0;

            for (int m = 0; m < inputMeshes.Count; m++)
            {
                maxVerts = System.Math.Max(inputMeshes[m].vertices.Length, maxVerts);
            }

            //Create buffer, here vertices will be stored multiplied with the local-to-voxel-space matrix
            Vector3[] verts = new Vector3[maxVerts];

            // First subtract min, then scale to voxel space (one unit equals one voxel along the x and z axes)
            // then subtract half a voxel to fix rounding
            Matrix4x4 voxelMatrix = Matrix4x4.TRS(-new Vector3(0.5f, 0, 0.5f), Quaternion.identity, Vector3.one) * Matrix4x4.Scale(new Vector3(ics, ich, ics)) * Matrix4x4.TRS(-min, Quaternion.identity, Vector3.one);

            AstarProfiler.EndProfile("Voxelizing - Step 2 - Init");

            AstarProfiler.StartProfile("Voxelizing - Step 2");

            // This loop is the hottest place in the whole rasterization process
            // it usually accounts for around 50% of the time
            for (int m = 0; m < inputMeshes.Count; m++)
            {
                RasterizationMesh mesh = inputMeshes[m];

                Matrix4x4 matrix = mesh.matrix;
                matrix = voxelMatrix * matrix;

                // Flip the orientation of all faces if the mesh is scaled in such a way
                // that the face orientations would change
                // This happens for example if a mesh has a negative scale along an odd number of axes
                // e.g it happens for the scale (-1, 1, 1) but not for (-1, -1, 1) or (1,1,1)
                var flipOrientation = VectorMath.ReversesFaceOrientations(matrix);

                Vector3[] vs         = mesh.vertices;
                int[]     tris       = mesh.triangles;
                int       trisLength = tris.Length;

                for (int i = 0; i < vs.Length; i++)
                {
                    verts[i] = matrix.MultiplyPoint3x4(vs[i]);
                }

                //AstarProfiler.StartFastProfile(0);

                int mesharea = mesh.area;

                for (int i = 0; i < trisLength; i += 3)
                {
                    Vector3 p1;
                    Vector3 p2;
                    Vector3 p3;

                    int minX;
                    int minZ;
                    int maxX;
                    int maxZ;

                    p1 = verts[tris[i]];
                    p2 = verts[tris[i + 1]];
                    p3 = verts[tris[i + 2]];

                    if (flipOrientation)
                    {
                        var tmp = p1;
                        p1 = p3;
                        p3 = tmp;
                    }

                    minX = (int)(Utility.Min(p1.x, p2.x, p3.x));
                    minZ = (int)(Utility.Min(p1.z, p2.z, p3.z));

                    maxX = (int)System.Math.Ceiling(Utility.Max(p1.x, p2.x, p3.x));
                    maxZ = (int)System.Math.Ceiling(Utility.Max(p1.z, p2.z, p3.z));

                    minX = Mathf.Clamp(minX, 0, voxelArea.width - 1);
                    maxX = Mathf.Clamp(maxX, 0, voxelArea.width - 1);
                    minZ = Mathf.Clamp(minZ, 0, voxelArea.depth - 1);
                    maxZ = Mathf.Clamp(maxZ, 0, voxelArea.depth - 1);



                    if (minX >= voxelArea.width || minZ >= voxelArea.depth || maxX <= 0 || maxZ <= 0)
                    {
                        continue;
                    }

                    // Debug code
                    //Debug.DrawLine (p1*cellSize+min+Vector3.up*0.2F,p2*cellSize+voxelOffset+Vector3.up*0.1F,Color.red);
                    //Debug.DrawLine (p1*cellSize+min,p2*cellSize+voxelOffset,Color.red, 1);
                    //Debug.DrawLine (p2*cellSize+min,p3*cellSize+voxelOffset,Color.red, 1);
                    //Debug.DrawLine (p3*cellSize+min,p1*cellSize+voxelOffset,Color.red, 1);

                    Vector3 normal;

                    int area;

                    //AstarProfiler.StartProfile ("Rasterize...");

                    normal = Vector3.Cross(p2 - p1, p3 - p1);

                    float dot = Vector3.Dot(normal.normalized, Vector3.up);

                    if (dot < slopeLimit)
                    {
                        area = UnwalkableArea;
                    }
                    else
                    {
                        area = 1 + mesharea;
                    }

                    Utility.CopyVector(vTris, 0, p1);
                    Utility.CopyVector(vTris, 3, p2);
                    Utility.CopyVector(vTris, 6, p3);

                    for (int x = minX; x <= maxX; x++)
                    {
                        int nrow = clipper.ClipPolygon(vTris, 3, vOut, 1F, -x + 0.5F, 0);

                        if (nrow < 3)
                        {
                            continue;
                        }

                        nrow = clipper.ClipPolygon(vOut, nrow, vRow, -1F, x + 0.5F, 0);

                        if (nrow < 3)
                        {
                            continue;
                        }

                        float clampZ1 = vRow[2];
                        float clampZ2 = vRow[2];
                        for (int q = 1; q < nrow; q++)
                        {
                            float val = vRow[q * 3 + 2];
                            clampZ1 = System.Math.Min(clampZ1, val);
                            clampZ2 = System.Math.Max(clampZ2, val);
                        }

                        int clampZ1I = Mathf.Clamp((int)System.Math.Round(clampZ1), 0, voxelArea.depth - 1);
                        int clampZ2I = Mathf.Clamp((int)System.Math.Round(clampZ2), 0, voxelArea.depth - 1);


                        for (int z = clampZ1I; z <= clampZ2I; z++)
                        {
                            //AstarProfiler.StartFastProfile(1);
                            int ncell = clipper.ClipPolygon(vRow, nrow, vCellOut, 1F, -z + 0.5F, 2);

                            if (ncell < 3)
                            {
                                //AstarProfiler.EndFastProfile(1);
                                continue;
                            }

                            ncell = clipper.ClipPolygonY(vCellOut, ncell, vCell, -1F, z + 0.5F, 2);

                            if (ncell < 3)
                            {
                                //AstarProfiler.EndFastProfile(1);
                                continue;
                            }

                            //AstarProfiler.EndFastProfile(1);
                            //AstarProfiler.StartFastProfile(2);
                            float sMin = vCell[1];
                            float sMax = vCell[1];
                            for (int q = 1; q < ncell; q++)
                            {
                                float val = vCell[q * 3 + 1];
                                sMin = System.Math.Min(sMin, val);
                                sMax = System.Math.Max(sMax, val);
                            }

                            //AstarProfiler.EndFastProfile(2);
                            int maxi = (int)System.Math.Ceiling(sMax);

                            // Skip span if below or above the bounding box
                            if (maxi >= 0 && sMin <= maximumVoxelYCoord)
                            {
                                // Make sure mini >= 0
                                int mini = System.Math.Max(0, (int)sMin);

                                // Make sure the span is at least 1 voxel high
                                maxi = System.Math.Max(mini + 1, maxi);

                                voxelArea.AddLinkedSpan(z * voxelArea.width + x, (uint)mini, (uint)maxi, area, voxelWalkableClimb);
                            }
                        }
                    }
                }
                //AstarProfiler.EndFastProfile(0);
                //AstarProfiler.EndProfile ("Rasterize...");
            }
            AstarProfiler.EndProfile("Voxelizing - Step 2");
        }