public bool Raycast(ref NavPoint startPoint, ref Vector3 endPos, NavPolyId prevRef, RaycastOptions options, out RaycastHit hit, Path hitPath) { hit = new RaycastHit(); if (hitPath != null) hitPath.Clear(); //validate input if (startPoint.Polygon == NavPolyId.Null || !nav.IsValidPolyRef(startPoint.Polygon)) return false; if (prevRef != NavPolyId.Null && !nav.IsValidPolyRef(prevRef)) return false; Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON]; NavTile prevTile, curTile, nextTile; NavPoly prevPoly, curPoly, nextPoly; NavPolyId curRef = startPoint.Polygon; nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly); nextTile = prevTile = curTile; nextPoly = prevPoly = curPoly; if (prevRef != NavPolyId.Null) nav.TryGetTileAndPolyByRefUnsafe(prevRef, out prevTile, out prevPoly); while (curRef != NavPolyId.Null) { //collect vertices int nv = 0; for (int i = 0; i < curPoly.VertCount; i++) { verts[nv] = curTile.Verts[curPoly.Verts[i]]; nv++; } float tmin, tmax; int segMin, segMax; if (!Intersection.SegmentPoly2D(startPoint.Position, endPos, verts, nv, out tmin, out tmax, out segMin, out segMax)) { //could not hit the polygon, keep the old t and report hit return true; } hit.EdgeIndex = segMax; //keep track of furthest t so far if (tmax > hit.T) hit.T = tmax; //store visited polygons if (hitPath != null) hitPath.Add(curRef); //ray end is completely inside the polygon if (segMax == -1) { hit.T = float.MaxValue; return true; } //follow neighbors NavPolyId nextRef = NavPolyId.Null; foreach (Link link in curPoly.Links) { //find link which contains the edge if (link.Edge != segMax) continue; //get pointer to the next polygon nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out nextTile, out nextPoly); //skip off-mesh connection if (nextPoly.PolyType == NavPolyType.OffMeshConnection) continue; //TODO QueryFilter //if the link is internal, just return the ref if (link.Side == BoundarySide.Internal) { nextRef = link.Reference; break; } //if the link is at the tile boundary //check if the link spans the whole edge and accept if (link.BMin == 0 && link.BMax == 255) { nextRef = link.Reference; break; } //check for partial edge links int v0 = curPoly.Verts[link.Edge]; int v1 = curPoly.Verts[(link.Edge + 1) % curPoly.VertCount]; Vector3 left = curTile.Verts[v0]; Vector3 right = curTile.Verts[v1]; //check that the intersection lies inside the link portal if (link.Side == BoundarySide.PlusX || link.Side == BoundarySide.MinusX) { //calculate link size float s = 1.0f / 255.0f; float lmin = left.Z + (right.Z - left.Z) * (link.BMin * s); float lmax = left.Z + (right.Z - left.Z) * (link.BMax * s); if (lmin > lmax) { //swap float temp = lmin; lmin = lmax; lmax = temp; } //find z intersection float z = startPoint.Position.Z + (endPos.Z - startPoint.Position.Z) * tmax; if (z >= lmin && z <= lmax) { nextRef = link.Reference; break; } } else if (link.Side == BoundarySide.PlusZ || link.Side == BoundarySide.MinusZ) { //calculate link size float s = 1.0f / 255.0f; float lmin = left.X + (right.X - left.X) * (link.BMin * s); float lmax = left.X + (right.X - left.X) * (link.BMax * s); if (lmin > lmax) { //swap float temp = lmin; lmin = lmax; lmax = temp; } //find x intersection float x = startPoint.Position.X + (endPos.X - startPoint.Position.X) * tmax; if (x >= lmin && x <= lmax) { nextRef = link.Reference; break; } } } if ((options & RaycastOptions.UseCosts) != 0) { //TODO add cost } if (nextRef == NavPolyId.Null) { //no neighbor, we hit a wall //calculate hit normal int a = segMax; int b = (segMax + 1) < nv ? segMax + 1 : 0; Vector3 va = verts[a]; Vector3 vb = verts[b]; float dx = vb.X - va.X; float dz = vb.Z - va.Z; hit.Normal = new Vector3(dz, 0, dx); hit.Normal.Normalize(); return true; } //no hit, advance to neighbor polygon prevRef = curRef; curRef = nextRef; prevTile = curTile; curTile = nextTile; prevPoly = curPoly; curPoly = nextPoly; } return true; }
public bool Raycast(ref NavPoint startPoint, ref Vector3 endPos, RaycastOptions options, out RaycastHit hit, Path hitPath) { return Raycast(ref startPoint, ref endPos, NavPolyId.Null, options, out hit, hitPath); }