public long Request(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter) { int slot = -1; for (int i = 0; i < MaxQueue; i++) { if (_queue[i].refId == PathQInvalid) { slot = i; break; } } if (slot == -1) { return(PathQInvalid); } long refId = _nextHandle++; if (_nextHandle == PathQInvalid) { _nextHandle++; } PathQuery q = _queue[slot]; q.refId = refId; Helper.VCopy(ref q.startPos, startPos); q.startRef = startRef; Helper.VCopy(ref q.endPos, endPos); q.endRef = endRef; q.status = 0; q.npath = 0; q.filter = filter; q.keepAlive = 0; return(refId); }
public void Update(long refId, float[] pos, float collisionQueryRange, NavMeshQuery navQuery, QueryFilter filter) { int MaxSegsPerPoly = NavMeshBuilder.VertsPerPoly * 3; if (refId <= 0) { Reset(); return; } Helper.VCopy(ref _center, pos); long[] parentPoly = null; navQuery.FindLocalNeighbourhood(refId, pos, collisionQueryRange, filter, ref _polys, ref parentPoly, ref _npolys, MaxLocalPolys); _nsegs = 0; float[] segs = new float[MaxSegsPerPoly * 6]; int nsegs = 0; long[] parentrefs = null; for (int j = 0; j < _npolys; j++) { navQuery.GetPolyWallSegments(_polys[j], filter, ref segs, ref parentrefs, ref nsegs, MaxSegsPerPoly); for (int k = 0; k < nsegs; k++) { int s = k * 6; float tseg; float distSqr = Helper.DistancePtSegSqr2D(pos[0], pos[1], pos[2], segs[s + 0], segs[s + 1], segs[s + 2], segs[s + 3], segs[s + 4], segs[s + 5], out tseg); if (distSqr > collisionQueryRange * collisionQueryRange) { continue; } float[] tempS = new float[6]; Array.Copy(segs, s, tempS, 0, 6); AddSegment(distSqr, tempS); } } }