public Fix64Vec2 FindNearestPointOnEdge(Fix64Vec2 testPosition, Fix64Vec2 pa, Fix64Vec2 pb, Fix64 width)
{
Fix64Vec2 pointOnEdge = Fix64Math.FindNearestPointOnLine(testPosition, pa, pb);
Fix64 distanceToA = Fix64Vec2.Distance(pointOnEdge, pa);
Fix64 edgeWidth = Fix64Vec2.Distance(pa, pb);
Fix64 distanceToB = edgeWidth - distanceToA;
Fix64Vec2 ba = pb - pa;
ba = ba.normalized;
Fix64 halfWidth = width / Fix64.two;
if (distanceToA < halfWidth)
{
pointOnEdge = pa + ba * halfWidth;
}
else if (distanceToB < halfWidth)
{
pointOnEdge = pb - ba * halfWidth;
}
return(pointOnEdge);
}
public bool NextCorner1(Fix64Vec3 position, Fix64 width, ref Fix64Vec3 corner, ref int index)
{
//if corner not found stay at current position
corner = position;
if (Status != ParallelNavMeshPathStatus.Valid)
{
return(false);
}
if (index < 0)
{
index = startIndex;
}
if (index == MAX_CORNER_COUNT)
{
//reached last polygon, invalidate path
index = -1;
Status = ParallelNavMeshPathStatus.Invalid;
return(false);
}
Fix64Vec2 testPosition = new Fix64Vec2(position.x, position.z);
int polygonIndex = polygonIndexes[index];
PNavPolygon currentPolygon = island.graph.polygons[polygonIndex];
int nextIndex = index + 1;
if (nextIndex >= MAX_CORNER_COUNT)
{
//reached final polygon
index = MAX_CORNER_COUNT;
corner = Destination;
return(false);
}
int nextPolygonIndex = polygonIndexes[nextIndex];
PNavPolygon nextPolygon = island.graph.polygons[nextPolygonIndex];
PNavEdge edge = FindEdge(currentPolygon, nextPolygon, width);
if (edge != null)
{
Fix64Vec2 pointOnEdge = Fix64Math.FindNearestPointOnLine(testPosition, edge.pointA, edge.pointB);
Fix64 distanceToA = Fix64Vec2.Distance(pointOnEdge, edge.pointA);
Fix64 distanceToB = edge.width - distanceToA;
Fix64Vec2 ba = edge.pointB - edge.pointA;
ba = ba.normalized;
Fix64 halfWidth = width / Fix64.two;
if (distanceToA < halfWidth)
{
pointOnEdge = edge.pointA + ba * halfWidth;
}
else if (distanceToB < halfWidth)
{
pointOnEdge = edge.pointB - ba * halfWidth;
}
corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y);
index = nextIndex;
return(true);
}
return(false);
}