static int PrepareCornerVerts(PNavEdgeLoop edgeLoop, PNavIsland island, Fix64Vec2[] verts, int[] indexes)
{
List <PNavNode> corners = new List <PNavNode>();
foreach (PNavNode node in edgeLoop.nodes)
{
if (node.isCorner)
{
corners.Add(node);
}
}
int cornersCount = corners.Count;
for (int i = 0; i < cornersCount; i++)
{
PNavNode node = corners[i];
Fix64Vec3 center = node.Center;
verts[i] = new Fix64Vec2(center.x, center.z);
indexes[i] = island.nodes.IndexOf(node);
}
bool isClockwise = IsClockwise(verts, cornersCount);
if (!isClockwise)
{
InverseVerticesAndIndexes(verts, indexes, cornersCount);
}
return(cornersCount);
}
// Update is called once per frame
void Update()
{
transform.Rotate(Vector3.forward * rotateSpeed * Time.deltaTime);
bool hit = false;
Fix64Vec3 start = (Fix64Vec3)transform.position;
Fix64Vec3 movement = (Fix64)castRange * (Fix64Vec3)transform.right;
Fix64 radius = (Fix64)circleRadius;
Fix64Vec3 end = start + movement;
hit = Parallel2D.CircleCast((Fix64Vec2)start, radius, (Fix64Vec2)movement, layerMask, ref hitInfo);
if (hit)
{
castPoint = hitInfo.point;
castNormal = hitInfo.normal;
circleHitPosition = (Fix64Vec2)(start + hitInfo.fraction * movement);
}
else
{
castPoint = (Fix64Vec2)end;
castNormal = Fix64Vec2.zero;
circleHitPosition = castPoint;
}
}
// Update is called once per frame
void Update()
{
transform.Rotate(Vector3.up * rotateSpeed * Time.deltaTime);
bool hit = false;
Fix64Vec3 start = (Fix64Vec3)transform.position;
Fix64Vec3 movement = (Fix64)castRange * (Fix64Vec3)transform.forward;
Fix64 radius = (Fix64)sphereRadius;
Fix64Vec3 end = start + movement;
hit = Parallel3D.SphereCast(start, radius, movement, layerMask, ref hitInfo);
if (hit)
{
castPoint = hitInfo.point;
castNormal = hitInfo.normal;
sphereHitPosition = start + hitInfo.fraction * movement;
}
else
{
castPoint = end;
castNormal = Fix64Vec3.zero;
sphereHitPosition = end;
}
}
void Update()
{
Fix64Vec3 center = (Fix64Vec3)transform.position;
Fix64 radius = (Fix64)sphereRadius;
Parallel3D.OverlapSphere(center, radius, layerMask, result);
}
void Move(Fix64 distanceToDestination, Fix64Vec3 direction, Fix64 deltaTime)
{
Fix64 stepDistance = movementSpeed * deltaTime;
stepDistance = Fix64Math.Min(distanceToDestination, stepDistance);
_pTransform.position += direction.normalized * stepDistance;
}
void RotateToTarget(Fix64Vec3 direction, Fix64 deltaTime)
{
Fix64Quat newRotation = Fix64Quat.LookRotation(direction, Fix64Vec3.up);
newRotation = Fix64Quat.Slerp(_pTransform.rotation, newRotation, rotationSpeed * Fix64.DegreeToRad * deltaTime);
_pTransform.rotation = newRotation;
}
void MoveByPhysics(Fix64 distanceToDestination, Fix64Vec3 direction, Fix64 deltaTime)
{
Fix64 movementSpeedLimit = distanceToDestination / deltaTime;
Fix64 clampedSpeed = Fix64Math.Clamp(movementSpeed, Fix64.zero, movementSpeedLimit);
Fix64Vec3 velocity = direction * clampedSpeed;//Fix64Vec3.ClampLength(direction * movementSpeed, clampedSpeed);
_pRigidbody.LinearVelocity = velocity;
}
public void ParallelFixedUpdate(Fix64 deltaTime)
{
Fix64Vec3 force = new Fix64Vec3(
_strength * hortizontal,
Fix64.zero,
_strength * vertical);
_rigidbody.ApplyForce(force);
}
public void ParallelFixedUpdate(Fix64 deltaTime)
{
Fix64Vec3 delta = new Fix64Vec3(
_speed * deltaTime * hortizontal,
_speed * deltaTime * vertical,
Fix64.zero);
_pTransform.position += delta;
}
public void GetAABB(int x, int y, int z, out Fix64Vec3 lower, out Fix64Vec3 upper)
{
Fix64 lowerX = worldOrigin.x;
Fix64 lowerY = worldOrigin.y;
Fix64 lowerZ = worldOrigin.z;
lower = new Fix64Vec3(
lowerX + (Fix64)x * gridSize.x,
lowerY + (Fix64)y * gridSize.y,
lowerZ + (Fix64)z * gridSize.z);
upper = new Fix64Vec3(
lowerX + ((Fix64)x + Fix64.one) * gridSize.x,
lowerY + ((Fix64)y + Fix64.one) * gridSize.y,
lowerZ + ((Fix64)z + Fix64.one) * gridSize.z);
}
public void Move(Fix64 deltaTime)
{
//using (new SProfiler($"move"))
{
if (_waypoints.Count > 0)
{
Fix64Vec2 currentWaypoint = _waypoints[_currentWaypointTargetIndex];
Fix64Vec3 currentWaypoint3D = new Fix64Vec3(currentWaypoint.x, _pTransform.position.y, currentWaypoint.y);
Fix64Vec3 direction = currentWaypoint3D - _pTransform.position;
direction = new Fix64Vec3(direction.x, Fix64.zero, direction.z);
direction = direction.normalized;
Fix64 distanceToDestination = Fix64Vec3.Distance(currentWaypoint3D, _pTransform.position);
if (distanceToDestination > stopDistance)
{
if (physicsBasedMovement)
{
MoveByPhysics(distanceToDestination, direction, deltaTime);
}
else
{
Move(distanceToDestination, direction, deltaTime);
}
if (rotate)
{
RotateToTarget(direction, deltaTime);
}
}
else
{
_currentWaypointTargetIndex++;
if (_currentWaypointTargetIndex == _waypoints.Count)
{
//reached last waypoint
// DONE
_waypoints.Clear();
_currentWaypointTargetIndex = 0;
Stop();
}
}
}
}
}
public static void CheckVolume(PNavMesh pNavMesh, int xStart, int xEnd, int zStart, int zEnd, int yStart, int yEnd, int scale, Action <int, int, int, int, int, int> callback)
{
PShapeOverlapResult3D result = new PShapeOverlapResult3D();
Fix64Vec3 size = pNavMesh.gridSize * (Fix64)scale;
Fix64Vec3 toCenter = Fix64.half * size;
int scaledXEnd = xEnd / scale;
int scaledYEnd = yEnd / scale;
int scaledZEnd = zEnd / scale;
for (int x = xStart; x < scaledXEnd; x++)
{
for (int z = zStart; z < scaledZEnd; z++)
{
for (int y = yStart; y < scaledZEnd; y++)
{
int scaledX = x * scale;
int scaledY = y * scale;
int scaledZ = z * scale;
Fix64Vec3 l;
Fix64Vec3 u;
pNavMesh.GetAABB(scaledX, scaledY, scaledZ, out l, out u);
Fix64Vec3 center = l + toCenter;
Fix64Quat rot = Fix64Quat.identity;
Parallel3D.OverlapCube(
center, rot,
size.x, size.y, size.z,
-1,
result);
if (result.count > 0)
{
callback(scaledX, scaledX + scale, scaledZ, scaledZ + scale, scaledY, scaledY + scale);
}
}
}
}
}
public static void DrawAABB(Fix64Vec3 l, Fix64Vec3 u, Color color)
{
Color c = Handles.color;
Vector3 lower = (Vector3)l;
Vector3 upper = (Vector3)u;
Handles.color = color;
float XL = lower.x;
float YL = lower.y;
float ZL = lower.z;
float XU = upper.x;
float YU = upper.y;
float ZU = upper.z;
Vector3 p0 = new Vector3(XL, YU, ZL);
Vector3 p1 = new Vector3(XL, YL, ZL);
Vector3 p2 = new Vector3(XL, YU, ZU);
Vector3 p3 = new Vector3(XL, YL, ZU);
Vector3 p4 = new Vector3(XU, YU, ZL);
Vector3 p5 = new Vector3(XU, YL, ZL);
Vector3 p6 = new Vector3(XU, YU, ZU);
Vector3 p7 = new Vector3(XU, YL, ZU);
Handles.DrawLine(p0, p1);
Handles.DrawLine(p0, p2);
Handles.DrawLine(p0, p4);
Handles.DrawLine(p1, p3);
Handles.DrawLine(p1, p5);
Handles.DrawLine(p2, p3);
Handles.DrawLine(p2, p6);
Handles.DrawLine(p3, p7);
Handles.DrawLine(p4, p5);
Handles.DrawLine(p4, p6);
Handles.DrawLine(p5, p7);
Handles.DrawLine(p6, p7);
Handles.color = c;
}
public static void DrawAABB(PNavMesh pNavMesh)
{
if (pNavMesh.columns != null)
{
for (int x = 0; x < pNavMesh.columns.GetLength(0); x++)
{
//if (x * pNavMesh.gridSize.x < 95)
{
//continue;
}
for (int z = 0; z < pNavMesh.columns.GetLength(1); z++)
{
PNavColumn column = pNavMesh.columns[x, z];
for (int y = 0; y < column.nodes.Length; y++)
{
int objectCount = column.nodes[y].objectCount;
Fix64Vec3 l = column.nodes[y].lower;
Fix64Vec3 u = column.nodes[y].upper;
//if (l.x * pNavMesh.gridSize.x < 30)
{
//continue;
}
if (objectCount > 0)
{
SceneDebugDraw.DrawAABB(l, u, Color.green);
}
else
{
//DrawAABB(l, u, Color.green);
}
}
}
}
}
}
public static void DrawPolygon(PNavMesh pNavMesh)
{
foreach (PNavIsland island in pNavMesh.islands)
{
Fix64Vec3[] verts = new Fix64Vec3[1024];
int indiceRead = 0;
for (int p = 0; p < island.polygonCount; p++)
{
int ic = island.indiceCountsOfPolygons[p];
for (int indiceOfPolygon = 0; indiceOfPolygon < ic; indiceOfPolygon++)
{
int nodeIndex = island.indices[indiceRead];
PNavNode vertNode = island.nodes[nodeIndex];
verts[indiceOfPolygon] = vertNode.Center;
indiceRead++;
}
DrawPolygon(verts, ic, Color.cyan);
}
}
}
static void DrawPolygon(Fix64Vec3[] verts, int count, Color color)
{
Color c = Handles.color;
Handles.color = color;
Fix64Vec3 previous = Fix64Vec3.zero;
for (int i = 0; i < count; i++)
{
Fix64Vec3 current = verts[i];
if (i != 0)
{
Handles.DrawLine((Vector3)previous, (Vector3)current);
}
previous = current;
}
Handles.DrawLine((Vector3)previous, (Vector3)verts[0]);
Handles.color = c;
}
// Update is called once per frame
void Update()
{
transform.Rotate(Vector3.up * rotateSpeed * Time.deltaTime);
PRaycastHit3D raycastHit3D;
bool hit = false;
Fix64Vec3 start = (Fix64Vec3)transform.position;
Fix64Vec3 end = start + (Fix64)raycastLength * (Fix64Vec3)transform.right;
hit = Parallel3D.RayCast(start, end, layerMask, out raycastHit3D);
if (hit)
{
raycastPoint = raycastHit3D.point;
raycastNormal = raycastHit3D.normal;
}
else
{
raycastPoint = end;
}
}
public PNavMeshPath CalculatePath(Fix64Vec3 start, Fix64Vec3 end)
{
Fix64Vec2 startPosition = new Fix64Vec2(start.x, start.z);
Fix64Vec2 endPosition = new Fix64Vec2(end.x, end.z);
PNavPolygon startPolygon = null;
PNavPolygon endPolygon = null;
bool sameIsland = false;
NavMeshAStart astart = null;
PNavMeshPath result = new PNavMeshPath();
result.Destination = end;
result.Destination2D = endPosition;
result.Status = ParallelNavMeshPathStatus.Invalid;
result.navMesh = navMesh;
PNavIsland endIsland = null;
PNavIsland startIsland = null;
foreach (PNavIsland island in navMesh.islands)
{
bool foundStart = false;
bool foundEnd = false;
foreach (PNavPolygon polygon in island.graph.polygons)
{
bool isStart = polygon.TestPoint(startPosition);
bool isEnd = polygon.TestPoint(endPosition);
if (isStart && isEnd)
{
result.Status = ParallelNavMeshPathStatus.Valid;
result.startIndex = -1;
return(result);
}
if (isStart)
{
startPolygon = polygon;
startIsland = island;
foundStart = true;
}
if (isEnd)
{
endPolygon = polygon;
endIsland = island;
foundEnd = true;
}
}
if (foundStart && foundEnd)
{
sameIsland = true;
astart = astartDictionary[island];
result.island = island;
break;
}
if (sameIsland)
{
break;
}
}
if (startPolygon != null && endPolygon == null)
{
//we find the polygon that is closest to the end position in the start position island
sameIsland = true;
astart = astartDictionary[startIsland];
result.island = startIsland;
endPolygon = startIsland.FindNearestPolygong(endPosition);
}
else if (startPolygon == null && endPolygon != null)
{
//we find the polygon that is closest to the start position in the end position island
sameIsland = true;
astart = astartDictionary[endIsland];
result.island = endIsland;
startPolygon = endIsland.FindNearestPolygong(startPosition);
}
else if (startPolygon == null && endPolygon == null)
{
//we find the polygon that is closest to the start position
//we then find the polygon that is closest to the end position in the same island
Fix64 minStart = Fix64.FromDivision(1000, 1);
PNavPolygon minStartPolygon = null;
PNavIsland minStartIsland = null;
foreach (PNavIsland island in navMesh.islands)
{
foreach (PNavPolygon polygon in island.graph.polygons)
{
Fix64 dis = Fix64Vec2.Distance(polygon.centroid, startPosition);
if (dis < minStart)
{
minStart = dis;
minStartPolygon = polygon;
minStartIsland = island;
}
}
}
sameIsland = true;
astart = astartDictionary[minStartIsland];
result.island = minStartIsland;
startPolygon = minStartPolygon;
endPolygon = minStartIsland.FindNearestPolygong(endPosition);
}
if (sameIsland)
{
if (astart != null && startPolygon != null && endPolygon != null)
{
NavMeshAStarNode startNode = astart.FindNode(startPolygon.index);
NavMeshAStarNode endNode = astart.FindNode(endPolygon.index);
NavMeshAStarNode lastNode = null;
using (new SProfiler("Pathfinding"))
{
astart.PrePathFinding();
startNode.startPoint = startPosition;
endNode.isLastNode = true;
endNode.endPoint = endPosition;
lastNode = astart.FindPath(startNode, endNode);
}
result.polygonIndexes = new int[PNavMeshPath.MAX_CORNER_COUNT];
int startIndex = 127;
while (lastNode != null)
{
result.polygonIndexes[startIndex] = lastNode.UserObject.index;
lastNode = (NavMeshAStarNode)lastNode.Parent;
startIndex--;
}
result.startIndex = startIndex + 1;
result.Status = ParallelNavMeshPathStatus.Valid;
}
}
return(result);
}
public static void CubeCastInRange(PNavMesh pNavMesh, int xStart, int xEnd, int zStart, int zEnd, int yStart, int yEnd)
{
PShapeOverlapResult3D result = new PShapeOverlapResult3D();
if (xEnd > pNavMesh.xCount)
{
xEnd = pNavMesh.xCount;
}
if (zEnd > pNavMesh.zCount)
{
zEnd = pNavMesh.zCount;
}
if (yEnd > pNavMesh.yCount)
{
yEnd = pNavMesh.yCount;
}
Debug.Log("CubeCastInRange:" + " xStart=" + xStart + " xEnd=" + xEnd + " zStart=" + zStart + " zEnd=" + zEnd + " yStart=" + yStart + " yEnd=" + yEnd);
for (int x = xStart; x < xEnd; x++)
{
for (int z = zStart; z < zEnd; z++)
{
PNavColumn column = pNavMesh.columns[x, z];
int surfaceIndex = -1;
for (int y = yStart; y < yEnd; y++)
{
Fix64Vec3 l;
Fix64Vec3 u;
pNavMesh.GetAABB(x, y, z, out l, out u);
Fix64Vec3 center = Fix64.half * (l + u);
Fix64Quat rot = Fix64Quat.identity;
Parallel3D.OverlapCube(
center, rot,
pNavMesh.gridSize.x, pNavMesh.gridSize.y, pNavMesh.gridSize.z,
-1,
result
);
PNavNode node = new PNavNode();
node.lower = l;
node.upper = u;
node.objectCount = result.count;
node.islandIndex = -1;
node.point = new PNavPoint(x, z);
if (result.count > 0 && y > surfaceIndex)
{
surfaceIndex = y;
}
column.nodes[y] = node;
}
column.surfaceNodeIndexes[0] = surfaceIndex;
if (surfaceIndex >= 0)
{
column.type = ParallelNavColumnType.Walkable;
}
else
{
column.type = ParallelNavColumnType.Empty;
}
}
}
}
public bool NextCorner(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);
}
index = BoundaryCheckIndex(index);
if (index == -1)
{
Status = ParallelNavMeshPathStatus.Invalid;
return(false);
}
Fix64Vec2 testPosition = new Fix64Vec2(position.x, position.z);
Fix64Vec2 pa = Fix64Vec2.zero;
Fix64Vec2 pb = Fix64Vec2.zero;
bool pointABSet = false;
if (polygonIndexes == null)
{
//destination polygon and start polygon is the same
index = MAX_CORNER_COUNT;
}
while (index <= MAX_CORNER_COUNT)
{
if (index == MAX_CORNER_COUNT)
{
//reached final polygon
if (pointABSet)
{
//check if destination is in the span of pa and pb
Fix64Vec2 des = new Fix64Vec2(Destination.x, Destination.z);
Fix64Vec2 vd = des - testPosition;
Fix64Vec2 va = pa - testPosition;
Fix64Vec2 vb = pb - testPosition;
bool desInSpan = Fix64Math.InSpan(vd, va, vb);
if (!desInSpan)
{
Fix64Vec2 pointOnEdge = FindNearestPointOnEdge(testPosition, pa, pb, width);
index--;
corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y);
return(true);
}
}
corner = Destination;
break;
}
int nextIndex = index + 1;
int polygonIndex = polygonIndexes[index];
PNavPolygon currentPolygon = island.graph.polygons[polygonIndex];
int nextPolygonIndex = polygonIndexes[nextIndex];
PNavPolygon nextPolygon = island.graph.polygons[nextPolygonIndex];
PNavEdge edge = FindEdge(currentPolygon, nextPolygon, width);
if (edge != null)
{
if (!pointABSet)
{
pa = edge.pointA;
pb = edge.pointB;
pointABSet = true;
}
else
{
Fix64Vec2 va = pa - testPosition;
Fix64Vec2 vb = pb - testPosition;
Fix64 AXB = Fix64Vec2.Cross(va, vb);
Fix64 BXA = Fix64Vec2.Cross(vb, va);
Fix64Vec2 pa1 = edge.pointA;
Fix64Vec2 pb1 = edge.pointB;
Fix64Vec2 va1 = pa1 - testPosition;
Fix64Vec2 vb1 = pb1 - testPosition;
Fix64 A1XB1 = Fix64Vec2.Cross(va1, vb1);
bool sameDirection = A1XB1 * AXB > Fix64.zero;
bool va1InsideOfSpan = Fix64Math.InSpan(va1, va, vb);
bool vb1InsideOfSpan = Fix64Math.InSpan(vb1, va, vb);
if (va1InsideOfSpan)
{
if (sameDirection)
{
bool valid = IsValidEdgePoint(testPosition, pb, pa1, width);
if (valid)
{
pa = pa1;
}
else
{
va1InsideOfSpan = false;
}
}
else
{
bool valid = IsValidEdgePoint(testPosition, pa, pa1, width);
if (valid)
{
pb = pa1;
}
else
{
va1InsideOfSpan = false;
}
}
}
if (vb1InsideOfSpan)
{
if (sameDirection)
{
bool valid = IsValidEdgePoint(testPosition, pa, pb1, width);
if (valid)
{
pb = pb1;
}
else
{
vb1InsideOfSpan = false;
}
}
else
{
bool valid = IsValidEdgePoint(testPosition, pb, pb1, width);
if (valid)
{
pa = pb1;
}
else
{
vb1InsideOfSpan = false;
}
}
}
if (!va1InsideOfSpan && !vb1InsideOfSpan)
{
Fix64Vec2 pointOnEdge = FindNearestPointOnEdge(testPosition, pa, pb, width);
corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y);
index--;
return(true);
}
}
}
else
{
break;
}
index = BoundaryCheckIndex(nextIndex);
}
return(false);
}
public void UpdateCurrentPolygonIndex(Fix64Vec3 position)
{
}
public void SetDestination(Fix64Vec3 destination)
{
_destination = destination;
UpdatePath();
}
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);
}
