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);
}