FixupCorridor IndexOutOfRangeException

[AqlaSolutions]

I copied your pathfinding code from the example app and sometimes it throws IndexOutOfRangeException: Index was outside the bounds of the array in FixupCorridor. I don't have a specific line, unfortunately. I will email you an snb and coordinates.

Here is the method:

        int FixupCorridor(PolyId[] path, int npath, int maxPath, List<PolyId> visited)
        {
            int furthestPath = -1;
            int furthestVisited = -1;

            //find furhtest common polygon
            for (int i = npath - 1; i >= 0; i--)
            {
                bool found = false;
                for (int j = visited.Count - 1; j >= 0; j--)
                {
                    if (path[i] == visited[j])
                    {
                        furthestPath = i;
                        furthestVisited = j;
                        found = true;
                    }
                }

                if (found)
                    break;
            }

            //if no intersection found, return current path
            if (furthestPath == -1 || furthestVisited == -1)
                return npath;

            //concatenate paths
            //adjust beginning of the buffer to include the visited
            int req = visited.Count - furthestVisited;
            int orig = Math.Min(furthestPath + 1, npath);
            int size = Math.Max(0, npath - orig);
            if (req + size > maxPath)
                size = maxPath - req;
            for (int i = 0; i < size; i++)
                path[req + i] = path[orig + i];

            //store visited
            for (int i = 0; i < req; i++)
                path[i] = visited[(visited.Count - 1) - i];

            return req + size;
        }

        [CanBeNull]
        public SlimVector3[] FindPath(SlimVector3 from, SlimVector3 to)
        {
            try
            {
                var navMeshQuery = _navMeshQuery;
                //Find random start and end points on the poly mesh
                NavPoint startPt;
                if (!GetClosestPoint(navMeshQuery, from, out startPt))
                    return null;
                //Debug.WriteLine("Finding path from " + startPos.Position);
                NavPoint endPt;
                if (!GetClosestPoint(navMeshQuery, to, out endPt))
                    return null;

                //Debug.WriteLine("             to " + endPos.Position);

                //calculate the overall path, which contains an array of polygon references
                int MAX_POLYS = 256;
                var path = new List<PolyId>(MAX_POLYS);
                if (startPt.Position == endPt.Position)
                    return new[] { to };
                navMeshQuery.FindPath(ref startPt, ref endPt, path);
                if (path.Count == 0)
                    return new[] { from, to };

                //find a smooth path over the mesh surface
                int npolys = path.Count;
                PolyId[] polys = path.ToArray();
                SVector3 iterPos = new SVector3();
                SVector3 targetPos = new SVector3();
                navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
                navMeshQuery.ClosestPointOnPoly(polys[npolys - 1], endPt.Position, ref targetPos);
                if ((targetPos - endPt.Position).LengthSquared() > 25f)
                    return null;
                var smoothPath = new List<SVector3>(2048);
                smoothPath.Add(iterPos);

                float STEP_SIZE = 0.5f;
                float SLOP = 0.01f;
                while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
                {
                    //find location to steer towards
                    SVector3 steerPos = new SVector3();
                    int steerPosFlag = 0;
                    PolyId steerPosRef = PolyId.Null;

                    if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, polys, npolys, ref steerPos, ref steerPosFlag, ref steerPosRef))
                        break;

                    bool endOfPath = (steerPosFlag & PathfindingCommon.STRAIGHTPATH_END) != 0 ? true : false;
                    bool offMeshConnection = (steerPosFlag & PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ? true : false;

                    //find movement delta
                    SVector3 delta = steerPos - iterPos;
                    float len = (float)Math.Sqrt(SVector3.Dot(delta, delta));

                    //if steer target is at end of path or off-mesh link
                    //don't move past location
                    if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
                        len = 1;
                    else
                        len = STEP_SIZE / len;

                    SVector3 moveTgt = new SVector3();
                    VMad(ref moveTgt, iterPos, delta, len);

                    //move
                    SVector3 result = new SVector3();
                    List<PolyId> visited = new List<PolyId>(16);
                    navMeshQuery.MoveAlongSurface(new NavPoint(polys[0], iterPos), moveTgt, ref result, visited);
                    npolys = FixupCorridor(polys, npolys, MAX_POLYS, visited);
                    float h = 0;
                    navMeshQuery.GetPolyHeight(polys[0], result, ref h);
                    result.Y = h;
                    iterPos = result;

                    //handle end of path when close enough
                    if (endOfPath && InRange(iterPos, steerPos, SLOP, 1.0f))
                    {
                        //reached end of path
                        iterPos = targetPos;
                        if (smoothPath.Count < smoothPath.Capacity)
                        {
                            smoothPath.Add(iterPos);
                        }
                        break;
                    }

                    //store results
                    if (smoothPath.Count < smoothPath.Capacity)
                    {
                        smoothPath.Add(iterPos);
                    }
                }
                return smoothPath.Select(v => new SlimVector3(v.X, v.Y, v.Z)).ToArray();
            }
            catch (Exception e)
            {
                string message = string.Format("FindPath problem: {0}\r\n\r\nMap = {1}\r\nFrom = {2}\r\nTo = {3}", e, _navMeshQuery, @from, to);
                Log.Error(message);
                Debug.WriteLine(e);
                return null;
            }
        }

        bool InRange(SVector3 v1, SVector3 v2, float r, float h)
        {
            float dx = v2.X - v1.X;
            float dy = v2.Y - v1.Y;
            float dz = v2.Z - v1.Z;
            return (dx * dx + dz * dz) < (r * r) && Math.Abs(dy) < h;
        }

        /// <summary>
        /// Scaled vector addition
        /// </summary>
        /// <param name="dest">Result</param>
        /// <param name="v1">Vector 1</param>
        /// <param name="v2">Vector 2</param>
        /// <param name="s">Scalar</param>
        void VMad(ref SVector3 dest, SVector3 v1, SVector3 v2, float s)
        {
            dest.X = v1.X + v2.X * s;
            dest.Y = v1.Y + v2.Y * s;
            dest.Z = v1.Z + v2.Z * s;
        }

        bool GetSteerTarget(NavMeshQueryWrapper navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, PolyId[] path, int pathSize,
             ref SVector3 steerPos, ref int steerPosFlag, ref PolyId steerPosRef)
        {
            int MAX_STEER_POINTS = 3;
            SVector3[] steerPath = new SVector3[MAX_STEER_POINTS];
            int[] steerPathFlags = new int[MAX_STEER_POINTS];
            PolyId[] steerPathPolys = new PolyId[MAX_STEER_POINTS];
            int nsteerPath = 0;
            navMeshQuery.FindStraightPath(startPos, endPos, path, pathSize,
                steerPath, steerPathFlags, steerPathPolys, ref nsteerPath, MAX_STEER_POINTS, 0);

            if (nsteerPath == 0)
                return false;

            //find vertex far enough to steer to
            int ns = 0;
            while (ns < nsteerPath)
            {
                if ((steerPathFlags[ns] & PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ||
                    !InRange(steerPath[ns], startPos, minTargetDist, 1000.0f))
                    break;

                ns++;
            }

            //failed to find good point to steer to
            if (ns >= nsteerPath)
                return false;

            steerPos = steerPath[ns];
            steerPos.Y = startPos.Y;
            steerPosFlag = steerPathFlags[ns];
            steerPosRef = steerPathPolys[ns];

            return true;
        }

Related to #52

[AqlaSolutions]

I sent snb to robert.rouhani@gmail.com

[Robmaister]

I'll take a look at this right now. Also on a side note, .snb is supposed to denote the binary file format (which has yet to be implemented), the file format you sent me is .snj, representing the JSON format. You can verify this by opening up the file you sent me with a text editor and see that it is both in JSON format and under meta.version, see that "snj" is defined as the version of the file format.

[AqlaSolutions]

Ok. I also emailed you a demo project. So any news?

[Robmaister]

Not yet, been a really busy few weeks, I'm about a week away from being done with my last semester in college, lots of final projects and assignments coming to a head. #61 should have been fixed pending you testing it.

When I get a chance I'll move this into NavMeshQuery and update it to use the new Path class.

[AqlaSolutions]

Ok, then I'll wait a week before testing #61 in case you have anything on this more frequent issue (I need to merge-rebuild-deploy things on our side to test the changes).

[AqlaSolutions]

I updated the code to your latest version and it now throws ArgumentOutOfRangeException ( Index was out of range. Must be non-negative and less than the size of the collection.) in List.set_Item on line path[req + i] = path[orig + i];:

    private int FixupCorridor(SharpNav.Pathfinding.Path path, int maxPath, List<NavPolyId> visited)
    {
        int furthestPath = -1;
        int furthestVisited = -1;

        //find furhtest common polygon
        for (int i = path.Count - 1; i >= 0; i--)
        {
            bool found = false;
            for (int j = visited.Count - 1; j >= 0; j--)
            {
                if (path[i] == visited[j])
                {
                    furthestPath = i;
                    furthestVisited = j;
                    found = true;
                }
            }

            if (found)
                break;
        }

        //if no intersection found, return current path
        if (furthestPath == -1 || furthestVisited == -1)
            return path.Count;

        //concatenate paths
        //adjust beginning of the buffer to include the visited
        int req = visited.Count - furthestVisited;
        int orig = Math.Min(furthestPath + 1, path.Count);
        int size = Math.Max(0, path.Count - orig);
        if (req + size > maxPath)
            size = maxPath - req;
        for (int i = 0; i < size; i++)
            path[req + i] = path[orig + i]; // *** THROWS HERE ***

        //store visited
        for (int i = 0; i < req; i++)
            path[i] = visited[(visited.Count - 1) - i];

        return req + size;
    }

Positions on snb that I emailed you before:

From = (X:2.041 Y:3.622 Z:-7.757) To = (X:6.029624 Y:0.6990567 Z:-2.627516)

X is inverted here. In your code it should be X:-2.041 and X:-6.029624.

Here is a screenshot of variables in Locals (but for other random input points): http://screencast.com/t/xnyCCq4Gb

[Robmaister]

I understand most of the problem but can't reproduce since the JSON file format has changed since you last emailed me the mesh. Could you regenerate the mesh and email it to me again? I'm going to move this function into the Path class and make it use List operations instead of the directly ported array/buffer stuff.

It may work with the changes I'm making before I even get to reproduce the exception on your map.

[AqlaSolutions]

@Robmaister, Ok, I'm going to check it now and email you a new repro if still applicable.

[AqlaSolutions]

Fixed!

[Robmaister]

Great, the solution is less performant than the original, so having a reproducible example will still help in that regard

[AqlaSolutions]

@Robmaister, ok, I've sent you the repro.