Example #1
0
        private void ClosestPointOnPolyInTile(MeshTile tile, long ip, float posx, float posy, float posz, ref float[] closestPt)
        {
            Poly poly = tile.Polys[ip];

            if (poly.Type == NavMeshBuilder.PolyTypeOffMeshConnection)
            {
                int   v0 = poly.Verts[0] * 3;
                int   v1 = poly.Verts[1] * 3;
                float d0 = Helper.VDist(posx, posy, posz, tile.Verts[v0 + 0], tile.Verts[v0 + 1], tile.Verts[v0 + 2]);
                float d1 = Helper.VDist(posx, posy, posz, tile.Verts[v1 + 0], tile.Verts[v1 + 1], tile.Verts[v1 + 2]);
                float u  = d0 / (d0 + d1);
                Helper.VLerp(ref closestPt, tile.Verts[v0 + 0], tile.Verts[v0 + 1], tile.Verts[v0 + 2], tile.Verts[v1 + 0], tile.Verts[v1 + 1], tile.Verts[v1 + 2], u);
                return;
            }

            PolyDetail pd = tile.DetailMeshes[ip];

            float[] verts = new float[NavMeshBuilder.VertsPerPoly * 3];
            float[] edged = new float[NavMeshBuilder.VertsPerPoly];
            float[] edget = new float[NavMeshBuilder.VertsPerPoly];
            int     nv    = poly.VertCount;

            for (int i = 0; i < nv; i++)
            {
                Array.Copy(tile.Verts, poly.Verts[i] * 3, verts, i * 3, 3);
            }

            closestPt[0] = posx;
            closestPt[1] = posy;
            closestPt[2] = posz;

            if (!Helper.DistancePtPolyEdgesSqr(posx, posy, posz, verts, nv, ref edged, ref edget))
            {
                float dmin = float.MaxValue;
                int   imin = -1;
                for (int i = 0; i < nv; i++)
                {
                    if (edged[i] < dmin)
                    {
                        dmin = edged[i];
                        imin = i;
                    }
                }
                int va = imin * 3;
                int vb = ((imin + 1) % nv) * 3;
                Helper.VLerp(ref closestPt, verts[va + 0], verts[va + 1], verts[va + 2], verts[vb + 0], verts[vb + 1], verts[vb + 2], edget[imin]);
            }

            for (int j = 0; j < pd.TriCount; j++)
            {
                int     t = (int)(pd.TriBase + j) * 4;
                float[] v = new float[9];
                for (int k = 0; k < 3; k++)
                {
                    if (tile.DetailTris[t + k] < poly.VertCount)
                    {
                        Array.Copy(tile.Verts, poly.Verts[tile.DetailTris[t + k]] * 3, v, k * 3, 3);
                        //v[k] = tile.Verts[poly.Verts[tile.DetailTris[t + k]]*3];
                    }
                    else
                    {
                        Array.Copy(tile.DetailVerts, (pd.VertBase + (tile.DetailTris[t + k] - poly.VertCount)) * 3, v, k * 3, 3);
                        //v[k] = tile.DetailVerts[(pd.VertBase + (tile.DetailTris[t + k] - poly.VertCount))*3];
                    }
                }
                float h = 0;
                if (Helper.ClosestHeightPointTriangle(posx, posy, posz, v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8], ref h))
                {
                    closestPt[1] = h;
                    break;
                }
            }
        }
Example #2
0
        public NavMeshBuilder(NavMeshCreateParams param)
        {
            if (param.Nvp > VertsPerPoly)
            {
                throw new ArgumentException("Too many Verts per Poly for NavMeshBuilder");
            }
            if (param.VertCount >= 0xffff)
            {
                throw new ArgumentException("Too many total verticies for NavMeshBuilder");
            }
            if (param.VertCount == 0 || param.Verts == null)
            {
                throw new ArgumentException("No vertices, cannot generate nav mesh");
            }
            if (param.PolyCount == 0 || param.Polys == null)
            {
                throw new ArgumentException("No Polygons, cannot generate nav mesh");
            }

            int nvp = param.Nvp;

            short[] offMeshConClass       = new short[0];
            int     storedOffMeshConCount = 0;
            int     offMeshConLinkCount   = 0;

            if (param.OffMeshConCount > 0)
            {
                offMeshConClass = new short[param.OffMeshConCount * 2];

                float hmin = float.MaxValue;
                float hmax = float.MinValue;

                if (param.DetailVerts != null && param.DetailVertsCount > 0)
                {
                    for (int i = 0; i < param.DetailVertsCount; i++)
                    {
                        int h = i * 3 + 1;
                        hmin = Math.Min(hmin, param.DetailVerts[h]);
                        hmax = Math.Max(hmax, param.DetailVerts[h]);
                    }
                }
                else
                {
                    for (int i = 0; i < param.VertCount; i++)
                    {
                        int   iv = i * 3;
                        float h  = param.BMin[1] + param.Verts[iv + 1] * param.Ch;
                        hmin = Math.Min(hmin, h);
                        hmax = Math.Max(hmax, h);
                    }
                }
                hmin -= param.WalkableClimb;
                hmax += param.WalkableClimb;
                float[] bmin = new float[3], bmax = new float[3];
                Array.Copy(param.BMin, bmin, 3);
                Array.Copy(param.BMax, bmax, 3);

                bmin[1] = hmin;
                bmax[1] = hmax;

                for (int i = 0; i < param.OffMeshConCount; i++)
                {
                    int p0 = (i * 2 + 0) * 3;
                    int p1 = (i * 2 + 1) * 3;
                    offMeshConClass[i * 2 + 0] = ClassifyOffMeshPoint(param.OffMeshConVerts[p0 + 0],
                                                                      param.OffMeshConVerts[p0 + 1],
                                                                      param.OffMeshConVerts[p0 + 2], bmin, bmax);
                    offMeshConClass[i * 2 + 1] = ClassifyOffMeshPoint(param.OffMeshConVerts[p1 + 0],
                                                                      param.OffMeshConVerts[p1 + 1],
                                                                      param.OffMeshConVerts[p1 + 2], bmin, bmax);

                    if (offMeshConClass[i * 2 + 0] == 0xff)
                    {
                        if (param.OffMeshConVerts[p0 + 1] < bmin[1] || param.OffMeshConVerts[p0 + 1] > bmax[1])
                        {
                            offMeshConClass[i * 2 + 0] = 0;
                        }
                    }

                    if (offMeshConClass[i * 2 + 0] == 0xff)
                    {
                        offMeshConLinkCount++;
                    }
                    if (offMeshConClass[i * 2 + 1] == 0xff)
                    {
                        offMeshConLinkCount++;
                    }

                    if (offMeshConClass[i * 2 + 0] == 0xff)
                    {
                        storedOffMeshConCount++;
                    }
                }
            }

            int totPolyCount = param.PolyCount + storedOffMeshConCount;
            int totVertCount = param.VertCount + storedOffMeshConCount * 2;

            int edgeCount   = 0;
            int portalCount = 0;

            for (int i = 0; i < param.PolyCount; i++)
            {
                int p = i * 2 * nvp;
                for (int j = 0; j < nvp; j++)
                {
                    if (param.Polys[p + j] == PolyMesh.MeshNullIdx)
                    {
                        break;
                    }
                    edgeCount++;

                    if ((param.Polys[p + nvp + j] & 0x8000) != 0)
                    {
                        int dir = param.Polys[p + nvp + j] & 0xf;
                        if (dir != 0xf)
                        {
                            portalCount++;
                        }
                    }
                }
            }

            int maxLinkCount = edgeCount + portalCount * 2 + offMeshConLinkCount * 2;

            int uniqueDetailVertCount = 0;
            int detailTryCount        = 0;

            if (param.DetailMeshes != null)
            {
                detailTryCount = param.DetailTriCount;
                for (int i = 0; i < param.PolyCount; i++)
                {
                    int p   = i * nvp * 2;
                    int ndv = (int)param.DetailMeshes[i * 4 + 1];
                    int nv  = 0;
                    for (int j = 0; j < nvp; j++)
                    {
                        if (param.Polys[p + j] == PolyMesh.MeshNullIdx)
                        {
                            break;
                        }
                        nv++;
                    }
                    ndv -= nv;
                    uniqueDetailVertCount += ndv;
                }
            }
            else
            {
                uniqueDetailVertCount = 0;
                detailTryCount        = 0;
                for (int i = 0; i < param.PolyCount; i++)
                {
                    int p  = i * nvp * 2;
                    int nv = 0;
                    for (int j = 0; j < nvp; j++)
                    {
                        if (param.Polys[p + j] == PolyMesh.MeshNullIdx)
                        {
                            break;
                        }
                        nv++;
                    }
                    detailTryCount += nv - 2;
                }
            }

            // Initialize the header and all nav data.
            Header = new MeshHeader
            {
                Magic           = Helper.NavMeshMagic,
                Version         = Helper.NavMeshVersion,
                X               = param.TileX,
                Y               = param.TileY,
                Layer           = param.TileLayer,
                UserId          = param.UserId,
                PolyCount       = totPolyCount,
                VertCount       = totVertCount,
                MaxLinkCount    = maxLinkCount,
                DetailMeshCount = param.PolyCount,
                DetailVertCount = uniqueDetailVertCount,
                DetailTriCount  = detailTryCount,
                BVQuantFactor   = 1.0f / param.Cs,
                OffMeshBase     = param.PolyCount,
                WalkableHeight  = param.WalkableHeight,
                WalkableRadius  = param.WalkableRadius,
                WalkableClimb   = param.WalkableClimb,
                OffMeshConCount = storedOffMeshConCount,
                BVNodeCount     = param.BuildBvTree ? param.PolyCount * 2 : 0,
                BMin            = new float[3],
                BMax            = new float[3]
            };
            Array.Copy(param.BMin, Header.BMin, 3);
            Array.Copy(param.BMax, Header.BMax, 3);

            NavVerts = new float[totVertCount * 3];
            NavPolys = new Poly[totPolyCount];
            for (int i = 0; i < totPolyCount; i++)
            {
                NavPolys[i] = new Poly();
            }
            NavLinks = new Link[maxLinkCount];
            for (int i = 0; i < maxLinkCount; i++)
            {
                NavLinks[i] = new Link();
            }
            NavDMeshes = new PolyDetail[param.PolyCount];
            for (int i = 0; i < param.PolyCount; i++)
            {
                NavDMeshes[i] = new PolyDetail();
            }
            NavDVerts = new float[3 * uniqueDetailVertCount];
            NavDTris  = new short[4 * detailTryCount];
            NavBvTree = param.BuildBvTree ? new BVNode[param.PolyCount * 2] : new BVNode[0];
            if (param.BuildBvTree)
            {
                for (int i = 0; i < param.PolyCount * 2; i++)
                {
                    NavBvTree[i] = new BVNode();
                }
            }
            OffMeshCons = new OffMeshConnection[storedOffMeshConCount];
            for (int i = 0; i < storedOffMeshConCount; i++)
            {
                OffMeshCons[i] = new OffMeshConnection();
            }


            int offMeshVertsBase = param.VertCount;
            int offMeshPolyBase  = param.PolyCount;

            // store vertices
            // Mesh
            for (int i = 0; i < param.VertCount; i++)
            {
                int iv = i * 3;
                int v  = i * 3;
                NavVerts[v + 0] = param.BMin[0] + param.Verts[iv + 0] * param.Cs;
                NavVerts[v + 1] = param.BMin[1] + param.Verts[iv + 1] * param.Ch;
                NavVerts[v + 2] = param.BMin[2] + param.Verts[iv + 2] * param.Cs;
            }
            // off-link
            int n = 0;

            for (int i = 0; i < param.OffMeshConCount; i++)
            {
                if (offMeshConClass[i * 2 + 0] == 0xff)
                {
                    int linkv = i * 2 * 3;
                    int v     = (offMeshVertsBase + n * 2) * 3;
                    Array.Copy(param.OffMeshConVerts, linkv, NavVerts, v, 3);
                    Array.Copy(param.OffMeshConVerts, linkv + 3, NavVerts, v + 3, 3);
                    n++;
                }
            }

            // store polygons
            // mesh
            int src = 0;

            for (int i = 0; i < param.PolyCount; i++)
            {
                Poly p = NavPolys[i];
                p.VertCount = 0;
                p.Flags     = param.PolyFlags[i];
                p.Area      = param.PolyAreas[i];
                p.Type      = PolyTypeGround;
                for (int j = 0; j < nvp; j++)
                {
                    if (param.Polys[src + j] == PolyMesh.MeshNullIdx)
                    {
                        break;
                    }
                    p.Verts[j] = param.Polys[src + j];
                    if ((param.Polys[src + nvp + j] & 0x8000) != 0)
                    {
                        int dir = param.Polys[src + nvp + j] & 0xf;
                        if (dir == 0xf)
                        {
                            p.Neis[j] = 0;
                        }
                        else if (dir == 0)
                        {
                            p.Neis[j] = ExtLink | 4;
                        }
                        else if (dir == 1)
                        {
                            p.Neis[j] = ExtLink | 2;
                        }
                        else if (dir == 2)
                        {
                            p.Neis[j] = ExtLink | 0;
                        }
                        else if (dir == 3)
                        {
                            p.Neis[j] = ExtLink | 6;
                        }
                    }
                    else
                    {
                        p.Neis[j] = param.Polys[src + nvp + j] + 1;
                    }
                    p.VertCount++;
                }
                src += nvp * 2;
            }
            // off mesh
            n = 0;
            for (int i = 0; i < param.OffMeshConCount; i++)
            {
                if (offMeshConClass[i * 2 + 0] == 0xff)
                {
                    Poly p = NavPolys[offMeshPolyBase + n];
                    p.VertCount = 2;
                    p.Verts[0]  = (offMeshVertsBase + n * 2 + 0);
                    p.Verts[1]  = (offMeshVertsBase + n * 2 + 1);
                    p.Flags     = param.OffMeshConFlags[i];
                    p.Area      = (short)param.OffMeshConAreas[i];
                    p.Type      = PolyTypeOffMeshConnection;
                    n++;
                }
            }

            // Store detail meshes and verts
            if (param.DetailMeshes != null)
            {
                int vbase = 0;
                for (int i = 0; i < param.PolyCount; i++)
                {
                    PolyDetail dtl = NavDMeshes[i];
                    int        vb  = (int)param.DetailMeshes[i * 4 + 0];
                    int        ndv = (int)param.DetailMeshes[i * 4 + 1];
                    int        nv  = NavPolys[i].VertCount;
                    dtl.VertBase  = vbase;
                    dtl.VertCount = (short)(ndv - nv);
                    dtl.TriBase   = param.DetailMeshes[i * 4 + 2];
                    dtl.TriCount  = (short)param.DetailMeshes[i * 4 + 3];
                    if (ndv - nv > 0)
                    {
                        Array.Copy(param.DetailVerts, (vb + nv) * 3, NavDVerts, vbase * 3, (ndv - nv) * 3);
                        vbase += (short)(ndv - nv);
                    }
                }
                Array.Copy(param.DetailTris, NavDTris, param.DetailTriCount * 4);
            }
            else
            {
                // Create dummy detail mesh
                int tbase = 0;
                for (int i = 0; i < param.PolyCount; i++)
                {
                    PolyDetail dtl = NavDMeshes[i];
                    int        nv  = NavPolys[i].VertCount;
                    dtl.VertBase  = 0;
                    dtl.VertCount = 0;
                    dtl.TriBase   = tbase;
                    dtl.TriCount  = (short)(nv - 2);
                    for (int j = 2; j < nv; j++)
                    {
                        int t = tbase * 4;
                        NavDTris[t + 0] = 0;
                        NavDTris[t + 1] = (short)(j - 1);
                        NavDTris[t + 2] = (short)j;
                        NavDTris[t + 3] = (1 << 2);
                        if (j == 2)
                        {
                            NavDTris[t + 3] |= (1 << 0);
                        }
                        if (j == nv - 1)
                        {
                            NavDTris[t + 3] |= (1 << 4);
                        }
                        tbase++;
                    }
                }
            }

            // Store and create BVTree
            if (param.BuildBvTree)
            {
                CreateBVTree(param.Verts, param.VertCount, param.Polys, param.PolyCount, nvp, param.Cs, param.Ch, param.PolyCount * 2);
            }

            // store off-mesh connections
            n = 0;
            for (int i = 0; i < param.OffMeshConCount; i++)
            {
                if (offMeshConClass[i * 2 + 0] == 0xff)
                {
                    OffMeshConnection con = OffMeshCons[n];
                    con.Poly = offMeshPolyBase + n;
                    int endPts = i * 2 * 3;
                    Array.Copy(param.OffMeshConVerts, endPts, con.Pos, 0, 3);
                    Array.Copy(param.OffMeshConVerts, endPts + 3, con.Pos, 3, 3);
                    con.Rad   = param.OffMeshConRad[i];
                    con.Flags = param.OffMeshConDir[i] > 0 ? OffMeshConBiDir : (short)0;
                    con.Side  = offMeshConClass[i * 2 + 1];
                    if (param.OffMeshConUserId != null)
                    {
                        con.UserId = param.OffMeshConUserId[i];
                    }
                    n++;
                }
            }
        }