// Retrace portals between corners and register if type of polygon changes
            public static int RetracePortals(UnityEngine.Experimental.AI.NavMeshQuery query, int startIndex, int endIndex,
                                             Unity.Collections.NativeSlice <UnityEngine.Experimental.AI.PolygonId> path, int n, Vector3 termPos,
                                             ref Unity.Collections.NativeArray <UnityEngine.Experimental.AI.NavMeshLocation> straightPath,
                                             ref Unity.Collections.NativeArray <StraightPathFlags> straightPathFlags, int maxStraightPath)
            {
                for (var k = startIndex; k < endIndex - 1; ++k)
                {
                    var type1 = query.GetPolygonType(path[k]);
                    var type2 = query.GetPolygonType(path[k + 1]);
                    if (type1 != type2)
                    {
                        Vector3 l, r;
                        var     status = query.GetPortalPoints(path[k], path[k + 1], out l, out r);
                        Unity.Mathematics.float3 cpa1, cpa2;
                        GeometryUtils.SegmentSegmentCPA(out cpa1, out cpa2, l, r, straightPath[n - 1].position, termPos);
                        straightPath[n] = query.CreateLocation(cpa1, path[k + 1]);

                        straightPathFlags[n] = type2 == UnityEngine.Experimental.AI.NavMeshPolyTypes.OffMeshConnection ? StraightPathFlags.OffMeshConnection : 0;
                        if (++n == maxStraightPath)
                        {
                            return(maxStraightPath);
                        }
                    }
                }

                straightPath[n]      = query.CreateLocation(termPos, path[endIndex]);
                straightPathFlags[n] = query.GetPolygonType(path[endIndex]) == UnityEngine.Experimental.AI.NavMeshPolyTypes.OffMeshConnection
                                           ? StraightPathFlags.OffMeshConnection
                                           : 0;
                return(++n);
            }
            public static UnityEngine.Experimental.AI.PathQueryStatus FindStraightPath(UnityEngine.Experimental.AI.NavMeshQuery query, Vector3 startPos, Vector3 endPos,
                                                                                       Unity.Collections.NativeSlice <UnityEngine.Experimental.AI.PolygonId> path, int pathSize,
                                                                                       ref Unity.Collections.NativeArray <UnityEngine.Experimental.AI.NavMeshLocation> straightPath,
                                                                                       ref Unity.Collections.NativeArray <StraightPathFlags> straightPathFlags,
                                                                                       ref Unity.Collections.NativeArray <float> vertexSide, ref int straightPathCount,
                                                                                       int maxStraightPath)
            {
                if (!query.IsValid(path[0]))
                {
                    straightPath[0] = new UnityEngine.Experimental.AI.NavMeshLocation(); // empty terminator
                    return(UnityEngine.Experimental.AI.PathQueryStatus.Failure);         // | PathQueryStatus.InvalidParam;
                }

                straightPath[0] = query.CreateLocation(startPos, path[0]);

                straightPathFlags[0] = StraightPathFlags.Start;

                var apexIndex = 0;
                var n         = 1;

                if (pathSize > 1)
                {
                    var startPolyWorldToLocal = query.PolygonWorldToLocalMatrix(path[0]);
                    var apex       = startPolyWorldToLocal.MultiplyPoint(startPos);
                    var left       = new Vector3(0, 0, 0); // Vector3.zero accesses a static readonly which does not work in burst yet
                    var right      = new Vector3(0, 0, 0);
                    var leftIndex  = -1;
                    var rightIndex = -1;

                    for (var i = 1; i <= pathSize; ++i)
                    {
                        var polyWorldToLocal = query.PolygonWorldToLocalMatrix(path[apexIndex]);

                        Vector3 vl, vr;
                        if (i == pathSize)
                        {
                            vl = vr = polyWorldToLocal.MultiplyPoint(endPos);
                        }
                        else
                        {
                            var success = query.GetPortalPoints(path[i - 1], path[i], out vl, out vr);
                            if (!success)
                            {
                                return(UnityEngine.Experimental.AI.PathQueryStatus.Failure); // | PathQueryStatus.InvalidParam;
                            }

                            vl = polyWorldToLocal.MultiplyPoint(vl);
                            vr = polyWorldToLocal.MultiplyPoint(vr);
                        }

                        vl = vl - apex;
                        vr = vr - apex;

                        // Ensure left/right ordering
                        if (PathUtils.Perp2D(vl, vr) < 0)
                        {
                            PathUtils.Swap(ref vl, ref vr);
                        }

                        // Terminate funnel by turning
                        if (PathUtils.Perp2D(left, vr) < 0)
                        {
                            var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
                            var termPos          = polyLocalToWorld.MultiplyPoint(apex + left);

                            n = PathUtils.RetracePortals(query, apexIndex, leftIndex, path, n, termPos, ref straightPath, ref straightPathFlags, maxStraightPath);
                            if (vertexSide.Length > 0)
                            {
                                vertexSide[n - 1] = -1;
                            }

                            //Debug.Log("LEFT");

                            if (n == maxStraightPath)
                            {
                                straightPathCount = n;
                                return(UnityEngine.Experimental.AI.PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
                            }

                            apex = polyWorldToLocal.MultiplyPoint(termPos);
                            left.Set(0, 0, 0);
                            right.Set(0, 0, 0);
                            i = apexIndex = leftIndex;
                            continue;
                        }

                        if (PathUtils.Perp2D(right, vl) > 0)
                        {
                            var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
                            var termPos          = polyLocalToWorld.MultiplyPoint(apex + right);

                            n = PathUtils.RetracePortals(query, apexIndex, rightIndex, path, n, termPos, ref straightPath, ref straightPathFlags, maxStraightPath);
                            if (vertexSide.Length > 0)
                            {
                                vertexSide[n - 1] = 1;
                            }

                            //Debug.Log("RIGHT");

                            if (n == maxStraightPath)
                            {
                                straightPathCount = n;
                                return(UnityEngine.Experimental.AI.PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
                            }

                            apex = polyWorldToLocal.MultiplyPoint(termPos);
                            left.Set(0, 0, 0);
                            right.Set(0, 0, 0);
                            i = apexIndex = rightIndex;
                            continue;
                        }

                        // Narrow funnel
                        if (PathUtils.Perp2D(left, vl) >= 0)
                        {
                            left      = vl;
                            leftIndex = i;
                        }

                        if (PathUtils.Perp2D(right, vr) <= 0)
                        {
                            right      = vr;
                            rightIndex = i;
                        }
                    }
                }

                // Remove the the next to last if duplicate point - e.g. start and end positions are the same
                // (in which case we have get a single point)
                if (n > 0 && straightPath[n - 1].position == endPos)
                {
                    n--;
                }

                n = PathUtils.RetracePortals(query, apexIndex, pathSize - 1, path, n, endPos, ref straightPath, ref straightPathFlags, maxStraightPath);
                if (vertexSide.Length > 0)
                {
                    vertexSide[n - 1] = 0;
                }

                if (n == maxStraightPath)
                {
                    straightPathCount = n;
                    return(UnityEngine.Experimental.AI.PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
                }

                // Fix flag for final path point
                straightPathFlags[n - 1] = StraightPathFlags.End;

                straightPathCount = n;
                return(UnityEngine.Experimental.AI.PathQueryStatus.Success);
            }