Example #1
0
        public Status GetTileAndPolyByRef(long refid, ref MeshTile tile, ref Poly poly)
        {
            if (refid == 0)
            {
                return(Status.Failure);
            }
            long salt, it, ip;

            DecodePolyId(refid, out salt, out it, out ip);
            if (it >= _maxTiles)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            if (_tiles[it].Salt != salt || _tiles[it].Header == null)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            if (ip >= _tiles[it].Header.PolyCount)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            tile = _tiles[it];
            poly = _tiles[it].Polys[ip];
            return(Status.Success);
        }
 public float GetCost(float pax, float pay, float paz, float pbx, float pby, float pbz,
                      long prevRef, MeshTile prevTile, Poly prevPoly,
                      long curRef, MeshTile curTile, Poly curPoly,
                      long nextRef, MeshTile nextTile, Poly nextPoly)
 {
     return(Helper.VDist(pax, pay, paz, pbx, pby, pbz) * _areaCost[curPoly.Area]);
 }
Example #3
0
        private long FindNearestPolyInTile(MeshTile tile, float centerx, float centery, float centerz, float extentsx, float extentsy, float extentsz, ref float[] nearestPt)
        {
            float[] bmin, bmax;
            bmin = Helper.VSub(centerx, centery, centerz, extentsx, extentsy, extentsz);
            bmax = Helper.VAdd(centerx, centery, centerz, extentsx, extentsy, extentsz);

            long[] polys     = new long[128];
            int    polyCount = QueryPolygonsInTile(tile, bmin[0], bmin[1], bmin[2], bmax[0], bmax[1], bmax[2], ref polys, 128);

            long  nearest            = 0;
            float nearestDistanceSqr = float.MaxValue;

            for (int i = 0; i < polyCount; i++)
            {
                long    refId         = polys[i];
                float[] closestPtPoly = new float[3];
                ClosestPointOnPolyInTile(tile, DecodePolyIdPoly(refId), centerx, centery, centerz, ref closestPtPoly);
                float d = Helper.VDistSqr(centerx, centery, centerz, closestPtPoly[0], closestPtPoly[1], closestPtPoly[2]);
                if (d < nearestDistanceSqr)
                {
                    if (nearestPt != null)
                    {
                        Array.Copy(closestPtPoly, nearestPt, 3);
                    }
                    nearestDistanceSqr = d;
                    nearest            = refId;
                }
            }
            return(nearest);
        }
Example #4
0
        public Status GetPolyArea(long refId, ref short resultArea)
        {
            long salt, it, ip;

            if (refId == 0)
            {
                return(Status.Failure);
            }

            DecodePolyId(refId, out salt, out it, out ip);
            if (it >= _maxTiles)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            if (_tiles[it].Salt != salt || _tiles[it].Header == null)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            MeshTile tile = _tiles[it];

            if (ip >= _tiles[it].Header.PolyCount)
            {
                return(Status.Failure | Status.InvalidParam);
            }

            Poly poly = tile.Polys[ip];

            resultArea = poly.Area;

            return(Status.Success);
        }
Example #5
0
        public Status RestoreTileState(MeshTile tile, TileState tileState)
        {
            if (tileState.Magic != Helper.NavMeshMagic)
            {
                return(Status.Failure | Status.WrongMagic);
            }
            if (tileState.Version != Helper.NavMeshVersion)
            {
                return(Status.Failure | Status.WrongVersion);
            }
            if (tileState.Ref != GetTileRef(tile))
            {
                return(Status.Failure | Status.InvalidParam);
            }

            for (int i = 0; i < tile.Header.PolyCount; i++)
            {
                Poly      p = tile.Polys[i];
                PolyState s = tileState.PolyStates[i];
                p.Flags = s.Flags;
                p.Area  = s.Area;
            }

            return(Status.Success);
        }
Example #6
0
        public Status SetPolyFlags(long refId, int flags)
        {
            long salt, it, ip;

            if (refId == 0)
            {
                return(Status.Failure);
            }

            DecodePolyId(refId, out salt, out it, out ip);
            if (it >= _maxTiles)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            if (_tiles[it].Salt != salt || _tiles[it].Header == null)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            MeshTile tile = _tiles[it];

            if (ip >= _tiles[it].Header.PolyCount)
            {
                return(Status.Failure | Status.InvalidParam);
            }

            Poly poly = tile.Polys[ip];

            poly.Flags = flags;
            return(Status.Success);
        }
Example #7
0
        public void GetTileAndPolyByRefUnsafe(long refId, out MeshTile tile, out Poly poly)
        {
            long salt, it, ip;

            DecodePolyId(refId, out salt, out it, out ip);
            tile = _tiles[it];
            poly = _tiles[it].Polys[ip];
        }
Example #8
0
        private void BaseOffMeshLinks(MeshTile tile)
        {
            if (tile == null)
            {
                return;
            }
            long baseId = GetPolyRefBase(tile);

            for (int i = 0; i < tile.Header.OffMeshConCount; i++)
            {
                OffMeshConnection con = tile.OffMeshCons[i];
                Poly poly             = tile.Polys[con.Poly];

                float[] ext       = { con.Rad, tile.Header.WalkableClimb, con.Rad };
                int     p         = 0;
                float[] nearestPt = new float[3];
                long    refId     = FindNearestPolyInTile(tile, con.Pos[p + 0], con.Pos[p + 1], con.Pos[p + 2], ext[0], ext[1],
                                                          ext[2], ref nearestPt);
                if (refId <= 0)
                {
                    continue;
                }
                if (((nearestPt[0] - con.Pos[p + 0]) * (nearestPt[0] - con.Pos[p + 0])) +
                    ((nearestPt[2] - con.Pos[p + 2]) * (nearestPt[2] - con.Pos[p + 2])) > (con.Rad * con.Rad))
                {
                    continue;
                }

                int v = poly.Verts[0] * 3;
                Array.Copy(nearestPt, 0, tile.Verts, v, 3);

                long idx = AllocLink(tile);
                if (idx != NullLink)
                {
                    Link link = tile.Links[idx];
                    link.Ref       = refId;
                    link.Edge      = 0;
                    link.Side      = 0xff;
                    link.BMin      = link.BMax = 0;
                    link.Next      = poly.FirstLink;
                    poly.FirstLink = idx;
                }

                long tidx = AllocLink(tile);
                if (tidx != NullLink)
                {
                    int  landPolyIdx = (int)DecodePolyIdTile(refId);
                    Poly landPoly    = tile.Polys[landPolyIdx];
                    Link link        = tile.Links[tidx];
                    link.Ref           = baseId | con.Poly;
                    link.Edge          = 0xff;
                    link.Side          = 0xff;
                    link.BMin          = link.BMax = 0;
                    link.Next          = landPoly.FirstLink;
                    landPoly.FirstLink = tidx;
                }
            }
        }
Example #9
0
        private long AllocLink(MeshTile tile)
        {
            if (tile.LinksFreeList == NullLink)
            {
                return(NullLink);
            }
            long link = tile.LinksFreeList;

            tile.LinksFreeList = tile.Links[link].Next;
            return(link);
        }
Example #10
0
        public Status GetOffMeshConnectionPolyEndPoints(long prevRef, long polyRef, ref float[] startPos, ref float[] endPos)
        {
            long salt, it, ip;

            if (polyRef == 0)
            {
                return(Status.Failure);
            }

            DecodePolyId(polyRef, out salt, out it, out ip);
            if (it >= _maxTiles)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            if (_tiles[it].Salt != salt || _tiles[it].Header == null)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            MeshTile tile = _tiles[it];

            if (ip >= _tiles[it].Header.PolyCount)
            {
                return(Status.Failure | Status.InvalidParam);
            }

            Poly poly = tile.Polys[ip];

            if (poly.Type != NavMeshBuilder.PolyTypeOffMeshConnection)
            {
                return(Status.Failure);
            }

            int idx0 = 0, idx1 = 1;

            for (long i = poly.FirstLink; i != NullLink; i = tile.Links[i].Next)
            {
                if (tile.Links[i].Edge == 0)
                {
                    if (tile.Links[i].Ref != prevRef)
                    {
                        idx0 = 1;
                        idx1 = 0;
                    }
                    break;
                }
            }

            Array.Copy(tile.Verts, poly.Verts[idx0] * 3, startPos, 0, 3);
            Array.Copy(tile.Verts, poly.Verts[idx1] * 3, endPos, 0, 3);

            return(Status.Success);
        }
Example #11
0
        public long GetTileRefAt(int x, int y, int layer)
        {
            int      h    = ComputeTileHash(x, y, _tileLutMask);
            MeshTile tile = _posLookup[h];

            while (tile != null)
            {
                if (tile.Header != null && tile.Header.X == x && tile.Header.Y == y && tile.Header.Layer == layer)
                {
                    return(GetTileRef(tile));
                }
                tile = tile.Next;
            }
            return(0);
        }
Example #12
0
        public long GetPolyRefBase(MeshTile tile)
        {
            if (tile == null)
            {
                return(0);
            }
            long it = -1;

            for (int i = 0; i < _tiles.Length; i++)
            {
                if (_tiles[i] == tile)
                {
                    it = i;
                }
            }
            return(EncodePolyId(tile.Salt, it, 0));
        }
Example #13
0
        public Status StoreTileState(MeshTile tile, out TileState tileState)
        {
            tileState            = new TileState();
            tileState.Magic      = Helper.NavMeshMagic;
            tileState.Version    = Helper.NavMeshVersion;
            tileState.Ref        = GetTileRef(tile);
            tileState.PolyStates = new PolyState[tile.Header.PolyCount];
            for (int i = 0; i < tile.Header.PolyCount; i++)
            {
                Poly p = tile.Polys[i];
                tileState.PolyStates[i]       = new PolyState();
                tileState.PolyStates[i].Flags = p.Flags;
                tileState.PolyStates[i].Area  = p.Area;
            }

            return(Status.Success);
        }
Example #14
0
        public int GetTilesAt(int x, int y, ref MeshTile[] tiles, int maxTiles)
        {
            int n = 0;

            int      h    = ComputeTileHash(x, y, _tileLutMask);
            MeshTile tile = _posLookup[h];

            while (tile != null)
            {
                if (tile.Header != null && tile.Header.X == x && tile.Header.Y == y)
                {
                    if (n < maxTiles)
                    {
                        tiles[n++] = tile;
                    }
                }
                tile = tile.Next;
            }
            return(n);
        }
Example #15
0
        public Status Init(NavMeshParams param)
        {
            _param = param;
            Array.Copy(param.Orig, _orig, 3);
            _tileWidth  = param.TileWidth;
            _tileHeight = param.TileHeight;

            _maxTiles    = param.MaxTiles;
            _tileLutSize = (int)Helper.NextPow2(param.MaxTiles / 4);
            if (_tileLutSize <= 0)
            {
                _tileLutSize = 1;
            }
            _tileLutMask = _tileLutSize - 1;

            _tiles     = new MeshTile[_maxTiles];
            _posLookup = new MeshTile[_tileLutSize];

            for (int i = 0; i < _tileLutSize; i++)
            {
                _posLookup[i] = new MeshTile();
            }

            _nextFree = null;
            for (int i = _maxTiles - 1; i >= 0; i--)
            {
                _tiles[i] = new MeshTile {
                    Salt = 1, Next = _nextFree
                };
                _nextFree = _tiles[i];
            }

            _tileBits = Helper.Ilog2(Helper.NextPow2(param.MaxTiles));
            _polyBits = Helper.Ilog2(Helper.NextPow2(param.MaxPolys));
            _saltBits = Math.Min(31, 32 - _tileBits - _polyBits);
            if (_saltBits < 10)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            return(Status.Success);
        }
Example #16
0
        private void ConnectIntLinks(MeshTile tile)
        {
            if (tile == null)
            {
                return;
            }
            long baseId = GetPolyRefBase(tile);

            for (int i = 0; i < tile.Header.PolyCount; i++)
            {
                Poly poly = tile.Polys[i];
                poly.FirstLink = NullLink;

                if (poly.Type == NavMeshBuilder.PolyTypeOffMeshConnection)
                {
                    continue;
                }

                for (int j = poly.VertCount - 1; j >= 0; j--)
                {
                    if (poly.Neis[j] == 0 || (poly.Neis[j] & NavMeshBuilder.ExtLink) != 0)
                    {
                        continue;
                    }

                    long idx = AllocLink(tile);
                    if (idx != NullLink)
                    {
                        Link link = tile.Links[idx];
                        link.Ref  = baseId | (poly.Neis[j] - 1);
                        link.Edge = (short)j;
                        link.Side = 0xff;
                        link.BMin = link.BMax = 0;

                        link.Next      = poly.FirstLink;
                        poly.FirstLink = idx;
                    }
                }
            }
        }
Example #17
0
        public MeshTile GetTileByRef(long refId)
        {
            if (refId == 0)
            {
                return(null);
            }

            long tileIndex = DecodePolyIdTile(refId);
            long tileSalt  = DecodePolyIdSalt(refId);

            if ((int)tileIndex >= _maxTiles)
            {
                return(null);
            }
            MeshTile tile = _tiles[tileIndex];

            if (tile.Salt != tileSalt)
            {
                return(null);
            }
            return(tile);
        }
Example #18
0
        private void UnconnectExtLinks(ref MeshTile tile, ref MeshTile target)
        {
            if (tile == null || target == null)
            {
                return;
            }

            long targetNum = DecodePolyIdTile(GetTileRef(target));

            for (int i = 0; i < tile.Header.PolyCount; i++)
            {
                Poly poly = tile.Polys[i];
                long j    = poly.FirstLink;
                long pj   = NullLink;
                while (j != NullLink)
                {
                    if (tile.Links[j].Side != 0xff && DecodePolyIdTile(tile.Links[j].Ref) == targetNum)
                    {
                        long nj = tile.Links[j].Next;
                        if (pj == NullLink)
                        {
                            poly.FirstLink = nj;
                        }
                        else
                        {
                            tile.Links[pj].Next = nj;
                        }
                        FreeLink(ref tile, j);
                        j = nj;
                    }
                    else
                    {
                        pj = j;
                        j  = tile.Links[j].Next;
                    }
                }
            }
        }
Example #19
0
        public OffMeshConnection GetOffMeshConnectionByRef(long refId)
        {
            long salt, it, ip;

            if (refId == 0)
            {
                return(null);
            }

            DecodePolyId(refId, out salt, out it, out ip);
            if (it >= _maxTiles)
            {
                return(null);
            }
            if (_tiles[it].Salt != salt || _tiles[it].Header == null)
            {
                return(null);
            }
            MeshTile tile = _tiles[it];

            if (ip >= _tiles[it].Header.PolyCount)
            {
                return(null);
            }

            Poly poly = tile.Polys[ip];

            if (poly.Type != NavMeshBuilder.PolyTypeOffMeshConnection)
            {
                return(null);
            }

            long idx = ip - tile.Header.OffMeshBase;

            return(tile.OffMeshCons[idx]);
        }
Example #20
0
        private int FindConnectingPolys(float vax, float vay, float vaz, float vbx, float vby, float vbz, MeshTile tile,
                                        int side, ref long[] con, ref float[] conarea, int maxcon)
        {
            if (tile == null)
            {
                return(0);
            }
            float[] amin = new float[2], amax = new float[2];
            Helper.CalcSlabEndPoints(vax, vay, vaz, vbx, vby, vbz, ref amin, ref amax, side);
            float apos = Helper.GetSlabCoord(vax, vay, vaz, side);

            float[] bmin   = new float[2], bmax = new float[2];
            int     m      = NavMeshBuilder.ExtLink | side;
            int     n      = 0;
            long    baseId = GetPolyRefBase(tile);

            for (int i = 0; i < tile.Header.PolyCount; i++)
            {
                Poly poly = tile.Polys[i];
                int  nv   = poly.VertCount;
                for (int j = 0; j < nv; j++)
                {
                    if (poly.Neis[j] != m)
                    {
                        continue;
                    }

                    int   vc   = poly.Verts[j] * 3;
                    int   vd   = poly.Verts[(j + 1) % nv] * 3;
                    float bpos = Helper.GetSlabCoord(tile.Verts[vc + 0], tile.Verts[vc + 1], tile.Verts[vc + 2], side);

                    if (Math.Abs(apos - bpos) > 0.01f)
                    {
                        continue;
                    }

                    Helper.CalcSlabEndPoints(tile.Verts[vc + 0], tile.Verts[vc + 1], tile.Verts[vc + 2], tile.Verts[vd + 0], tile.Verts[vd + 1], tile.Verts[vd + 2], ref bmin, ref bmax, side);

                    if (!Helper.OverlapSlabs(amin, amax, bmin, bmax, 0.01f, tile.Header.WalkableClimb))
                    {
                        continue;
                    }

                    if (n < maxcon)
                    {
                        conarea[n * 2 + 0] = Math.Max(amin[0], bmin[0]);
                        conarea[n * 2 + 1] = Math.Min(amax[0], bmax[0]);
                        con[n]             = baseId | i;
                        n++;
                    }
                    break;
                }
            }
            return(n);
        }
Example #21
0
        public Status AddTile(NavMeshBuilder data, int flags, long lastRef, ref long result)
        {
            MeshHeader header = data.Header;

            if (header.Magic != Helper.NavMeshMagic)
            {
                return(Status.Failure | Status.WrongMagic);
            }
            if (header.Version != Helper.NavMeshVersion)
            {
                return(Status.Failure | Status.WrongVersion);
            }

            if (GetTileAt(header.X, header.Y, header.Layer) != null)
            {
                return(Status.Failure);
            }

            MeshTile tile = null;

            if (lastRef == 0)
            {
                if (_nextFree != null)
                {
                    tile      = _nextFree;
                    _nextFree = tile.Next;
                    tile.Next = null;
                }
            }
            else
            {
                int tileIndex = (int)DecodePolyIdTile(lastRef);
                if (tileIndex >= _maxTiles)
                {
                    return(Status.Failure | Status.OutOfMemory);
                }

                MeshTile target = _tiles[tileIndex];
                MeshTile prev   = null;
                tile = _nextFree;
                while (tile != null && tile != target)
                {
                    prev = tile;
                    tile = tile.Next;
                }

                if (tile != target)
                {
                    return(Status.Failure | Status.OutOfMemory);
                }

                if (prev != null)
                {
                    _nextFree = tile.Next;
                }
                else
                {
                    prev.Next = tile.Next;
                }

                tile.Salt = DecodePolyIdSalt(lastRef);
            }

            if (tile == null)
            {
                return(Status.Failure | Status.OutOfMemory);
            }

            // insert tile into the position
            int h = ComputeTileHash(header.X, header.Y, _tileLutMask);

            tile.Next     = _posLookup[h];
            _posLookup[h] = tile;

            tile.Verts        = data.NavVerts;
            tile.Polys        = data.NavPolys;
            tile.Links        = data.NavLinks;
            tile.DetailMeshes = data.NavDMeshes;
            tile.DetailVerts  = data.NavDVerts;
            tile.DetailTris   = data.NavDTris;
            tile.BVTree       = data.NavBvTree;
            tile.OffMeshCons  = data.OffMeshCons;

            tile.LinksFreeList = 0;
            tile.Links[header.MaxLinkCount - 1].Next = NullLink;
            for (int i = 0; i < header.MaxLinkCount - 1; i++)
            {
                tile.Links[i].Next = i + 1;
            }

            tile.Data   = data;
            tile.Header = header;
            tile.Flags  = flags;

            ConnectIntLinks(tile);
            BaseOffMeshLinks(tile);

            int MaxNeis = 32;

            MeshTile[] neis = new MeshTile[MaxNeis];
            int        nneis;

            nneis = GetTilesAt(header.X, header.Y, ref neis, MaxNeis);
            for (int j = 0; j < nneis; j++)
            {
                MeshTile temp = neis[j];
                if (neis[j] != tile)
                {
                    ConnectExtLinks(ref tile, ref temp, -1);
                    ConnectExtLinks(ref temp, ref tile, -1);
                }
                ConnectExtOffMeshLinks(ref tile, ref temp, -1);
                ConnectExtOffMeshLinks(ref temp, ref tile, -1);
            }

            for (int i = 0; i < 8; i++)
            {
                nneis = GetNeighborTilesAt(header.X, header.Y, i, ref neis, MaxNeis);
                for (int j = 0; j < nneis; j++)
                {
                    MeshTile temp = neis[j];
                    ConnectExtLinks(ref tile, ref temp, i);
                    ConnectExtLinks(ref temp, ref tile, Helper.OppositeTile(i));
                    ConnectExtOffMeshLinks(ref tile, ref temp, i);
                    ConnectExtOffMeshLinks(ref temp, ref tile, Helper.OppositeTile(i));
                }
            }

            result = GetTileRef(tile);

            return(Status.Success);
        }
Example #22
0
 private void FreeLink(ref MeshTile tile, long link)
 {
     tile.Links[link].Next = tile.LinksFreeList;
     tile.LinksFreeList    = link;
 }
Example #23
0
        private void ConnectExtOffMeshLinks(ref MeshTile tile, ref MeshTile target, int side)
        {
            if (tile == null)
            {
                return;
            }

            short oppositeSide = (side == -1) ? (short)0xff : (short)Helper.OppositeTile(side);

            for (int i = 0; i < target.Header.OffMeshConCount; i++)
            {
                OffMeshConnection targetCon = target.OffMeshCons[i];
                if (targetCon.Side != oppositeSide)
                {
                    continue;
                }

                Poly targetPoly = target.Polys[targetCon.Poly];
                if (targetPoly.FirstLink == NullLink)
                {
                    continue;
                }

                float[] ext = { targetCon.Rad, target.Header.WalkableClimb, targetCon.Rad };

                int     p         = 3;
                float[] nearestPt = new float[3];
                long    refId     = FindNearestPolyInTile(tile, targetCon.Pos[p + 0], targetCon.Pos[p + 1], targetCon.Pos[p + 2],
                                                          ext[0], ext[1], ext[2], ref nearestPt);
                if (refId <= 0)
                {
                    continue;
                }

                if (((nearestPt[0] - targetCon.Pos[p + 0]) * (nearestPt[0] - targetCon.Pos[p + 0])) + ((nearestPt[2] - targetCon.Pos[p + 2]) * (nearestPt[2] - targetCon.Pos[p + 2])) > (targetCon.Rad * targetCon.Rad))
                {
                    continue;
                }

                int v = targetPoly.Verts[1] * 3;
                Array.Copy(nearestPt, 0, target.Verts, v, 3);

                long idx = AllocLink(target);
                if (idx != NullLink)
                {
                    Link link = target.Links[idx];
                    link.Ref  = refId;
                    link.Edge = 1;
                    link.Side = oppositeSide;
                    link.BMin = link.BMax = 0;

                    link.Next            = targetPoly.FirstLink;
                    targetPoly.FirstLink = idx;
                }

                if ((targetCon.Flags & NavMeshBuilder.OffMeshConBiDir) != 0)
                {
                    long tidx = AllocLink(tile);
                    if (tidx != NullLink)
                    {
                        int  landPolyIdx = (int)DecodePolyIdPoly(refId);
                        Poly landPoly    = tile.Polys[landPolyIdx];
                        Link link        = tile.Links[tidx];
                        link.Ref           = GetPolyRefBase(target) | (targetCon.Poly);
                        link.Edge          = 0xff;
                        link.Side          = side == -1 ? (short)0xff : (short)side;
                        link.BMin          = link.BMax = 0;
                        link.Next          = landPoly.FirstLink;
                        landPoly.FirstLink = tidx;
                    }
                }
            }
        }
Example #24
0
        private int QueryPolygonsInTile(MeshTile tile, float qminx, float qminy, float qminz, float qmaxx, float qmaxy,
                                        float qmaxz, ref long[] polys, int maxPolys)
        {
            if (tile.BVTree != null)
            {
                int     node  = 0;
                int     end   = tile.Header.BVNodeCount;
                float[] tbmin = tile.Header.BMin;
                float[] tbmax = tile.Header.BMax;
                float   qfac  = tile.Header.BVQuantFactor;

                int[] bmin = new int[3], bmax = new int[3];

                float minx = Math.Min(tbmax[0], Math.Max(qminx, tbmin[0])) - tbmin[0];
                float miny = Math.Min(tbmax[1], Math.Max(qminy, tbmin[1])) - tbmin[1];
                float minz = Math.Min(tbmax[2], Math.Max(qminz, tbmin[2])) - tbmin[2];
                float maxx = Math.Min(tbmax[0], Math.Max(qmaxx, tbmin[0])) - tbmin[0];
                float maxy = Math.Min(tbmax[1], Math.Max(qmaxy, tbmin[1])) - tbmin[1];
                float maxz = Math.Min(tbmax[2], Math.Max(qmaxz, tbmin[2])) - tbmin[2];

                bmin[0] = (int)(qfac * minx) & 0xfffe;
                bmin[1] = (int)(qfac * miny) & 0xfffe;
                bmin[2] = (int)(qfac * minz) & 0xfffe;
                bmax[0] = (int)(qfac * maxx + 1) | 1;
                bmax[1] = (int)(qfac * maxy + 1) | 1;
                bmax[2] = (int)(qfac * maxz + 1) | 1;

                long baseId = GetPolyRefBase(tile);
                int  n      = 0;
                while (node < end)
                {
                    bool overlap    = Helper.OverlapQuantBounds(bmin, bmax, tile.BVTree[node].BMin, tile.BVTree[node].BMax);
                    bool isLeafNode = tile.BVTree[node].I >= 0;

                    if (isLeafNode && overlap)
                    {
                        if (n < maxPolys)
                        {
                            polys[n++] = baseId | tile.BVTree[node].I;
                        }
                    }

                    if (overlap || isLeafNode)
                    {
                        node++;
                    }
                    else
                    {
                        int escapeIndex = -tile.BVTree[node].I;
                        node += escapeIndex;
                    }
                }
                return(n);
            }
            else
            {
                float[] bmin   = new float[3], bmax = new float[3];
                int     n      = 0;
                long    baseId = GetPolyRefBase(tile);
                for (int i = 0; i < tile.Header.PolyCount; i++)
                {
                    Poly p = tile.Polys[i];
                    if (p.Type == NavMeshBuilder.PolyTypeOffMeshConnection)
                    {
                        continue;
                    }

                    int v = p.Verts[0 * 3];
                    Array.Copy(tile.Verts, v, bmin, 0, 3);
                    Array.Copy(tile.Verts, v, bmax, 0, 3);
                    for (int j = 1; j < p.VertCount; j++)
                    {
                        v = p.Verts[j] * 3;
                        Helper.VMin(ref bmin, tile.Verts[v + 0], tile.Verts[v + 1], tile.Verts[v + 2]);
                        Helper.VMax(ref bmax, tile.Verts[v + 0], tile.Verts[v + 1], tile.Verts[v + 2]);
                    }
                    if (Helper.OverlapBounds(qminx, qminy, qminz, qmaxx, qmaxy, qmaxz, bmin[0], bmin[1], bmin[2],
                                             bmax[0], bmax[1], bmax[2]))
                    {
                        if (n < maxPolys)
                        {
                            polys[n++] = baseId | i;
                        }
                    }
                }
                return(n);
            }
        }
Example #25
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;
                }
            }
        }
 public bool PassFilter(long refId, MeshTile tile, Poly poly)
 {
     return((poly.Flags & IncludeFlags) != 0 && (poly.Flags & ExcludeFlags) == 0);
 }
Example #27
0
        private void ConnectExtLinks(ref MeshTile tile, ref MeshTile target, int side)
        {
            if (tile == null)
            {
                return;
            }

            for (int i = 0; i < tile.Header.PolyCount; i++)
            {
                Poly poly = tile.Polys[i];

                int nv = poly.VertCount;
                for (int j = 0; j < nv; j++)
                {
                    if ((poly.Neis[j] & NavMeshBuilder.ExtLink) == 0)
                    {
                        continue;
                    }

                    int dir = (int)(poly.Neis[j] & 0xff);
                    if (side != -1 && dir != side)
                    {
                        continue;
                    }

                    int     va   = poly.Verts[j] * 3;
                    int     vb   = poly.Verts[(j + 1) % nv] * 3;
                    long[]  nei  = new long[4];
                    float[] neia = new float[4 * 2];
                    int     nnei = FindConnectingPolys(tile.Verts[va + 0], tile.Verts[va + 1], tile.Verts[va + 2],
                                                       tile.Verts[vb + 0], tile.Verts[vb + 1], tile.Verts[vb + 2], target,
                                                       Helper.OppositeTile(dir), ref nei, ref neia, 4);
                    for (int k = 0; k < nnei; k++)
                    {
                        long idx = AllocLink(tile);
                        if (idx != NullLink)
                        {
                            Link link = tile.Links[idx];
                            link.Ref  = nei[k];
                            link.Edge = (short)j;
                            link.Side = (short)dir;

                            link.Next      = poly.FirstLink;
                            poly.FirstLink = idx;

                            if (dir == 0 || dir == 4)
                            {
                                float tmin = (neia[k * 2 + 0] - tile.Verts[va + 2]) /
                                             (tile.Verts[vb + 2] - tile.Verts[va + 2]);
                                float tmax = (neia[k * 2 + 1] - tile.Verts[va + 2]) /
                                             (tile.Verts[vb + 2] - tile.Verts[va + 2]);
                                if (tmin > tmax)
                                {
                                    float temp = tmin;
                                    tmin = tmax;
                                    tmax = temp;
                                }
                                link.BMin = (short)(Math.Min(1.0f, Math.Max(tmin, 0.0f)) * 255.0f);
                                link.BMax = (short)(Math.Min(1.0f, Math.Max(tmax, 0.0f)) * 255.0f);
                            }
                            else if (dir == 2 || dir == 6)
                            {
                                float tmin = (neia[k * 2 + 0] - tile.Verts[va + 0]) /
                                             (tile.Verts[vb + 0] - tile.Verts[va + 0]);
                                float tmax = (neia[k * 2 + 1] - tile.Verts[va + 0]) /
                                             (tile.Verts[vb + 0] - tile.Verts[va + 0]);
                                if (tmin > tmax)
                                {
                                    float temp = tmin;
                                    tmin = tmax;
                                    tmax = temp;
                                }
                                link.BMin = (short)(Math.Min(1.0f, Math.Max(tmin, 0.0f)) * 255.0f);
                                link.BMax = (short)(Math.Min(1.0f, Math.Max(tmax, 0.0f)) * 255.0f);
                            }
                        }
                    }
                }
            }
        }
Example #28
0
        public Status RemoveTile(long refId, out NavMeshBuilder data)
        {
            data = null;
            if (refId == 0)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            long tileIndex = DecodePolyIdTile(refId);
            long tileSalt  = DecodePolyIdSalt(refId);

            if (tileIndex >= _maxTiles)
            {
                return(Status.Failure | Status.InvalidParam);
            }
            MeshTile tile = _tiles[tileIndex];

            if (tile.Salt != tileSalt)
            {
                return(Status.Failure | Status.InvalidParam);
            }

            int      h    = ComputeTileHash(tile.Header.X, tile.Header.Y, _tileLutMask);
            MeshTile prev = null;
            MeshTile cur  = _posLookup[h];

            while (cur != null)
            {
                if (cur == tile)
                {
                    if (prev != null)
                    {
                        prev.Next = cur.Next;
                    }
                    else
                    {
                        _posLookup[h] = cur.Next;
                    }
                    break;
                }
                prev = cur;
                cur  = cur.Next;
            }

            int MaxNeis = 32;

            MeshTile[] neis = new MeshTile[MaxNeis];
            int        nneis;

            nneis = GetTilesAt(tile.Header.X, tile.Header.Y, ref neis, MaxNeis);
            for (int j = 0; j < nneis; j++)
            {
                if (neis[j] == tile)
                {
                    continue;
                }
                MeshTile temp = neis[j];
                UnconnectExtLinks(ref temp, ref tile);
            }

            for (int i = 0; i < 8; i++)
            {
                nneis = GetNeighborTilesAt(tile.Header.X, tile.Header.Y, i, ref neis, MaxNeis);
                for (int j = 0; j < nneis; j++)
                {
                    MeshTile temp = neis[j];
                    UnconnectExtLinks(ref temp, ref tile);
                }
            }

            // reset tile
            if ((tile.Flags & TileFreeData) != 0)
            {
                tile.Data = null;
            }
            else
            {
                data = tile.Data;
            }

            tile.Header        = null;
            tile.Flags         = 0;
            tile.LinksFreeList = 0;
            tile.Polys         = null;
            tile.Verts         = null;
            tile.Links         = null;
            tile.DetailMeshes  = null;
            tile.DetailVerts   = null;
            tile.DetailTris    = null;
            tile.BVTree        = null;
            tile.OffMeshCons   = null;

            tile.Salt = (tile.Salt + 1) & ((1 << (int)_saltBits) - 1);
            if (tile.Salt == 0)
            {
                tile.Salt++;
            }

            tile.Next = _nextFree;
            _nextFree = tile;

            return(Status.Success);
        }