private bool IsVisible(int a, int b, TriangulatorDataSet ds)
{
Vector2 aPos = ds.NodePosV2(a);
Vector2 bPos = ds.NodePosV2(b);
Vector2 dir = bPos - aPos;
float curSqrDist = SomeMath.SqrDistance(aPos, bPos);
foreach (var edge in ds.edges)
{
if (edge.Contains(a, b))
{
continue;
}
Vector2 intersection;
if (SomeMath.RayIntersectXZ(
aPos, dir, //from, direction
ds.NodePosV2(edge.a), ds.NodePosV2(edge.b), //a, b
out intersection) &&
SomeMath.SqrDistance(aPos, intersection) < curSqrDist)
{
//Debuger3.AddRay(new Vector3(intersection.x, 0, intersection.y), Vector3.up, Color.magenta);
//Debuger3.AddLine(ds.NodePosV3(a), new Vector3(intersection.x, 0, intersection.y), Color.magenta);
//Debuger3.AddLine(ds.NodePosV3(b), new Vector3(intersection.x, 0, intersection.y), Color.magenta);
return(false);
}
}
return(true);
}
//Vector2
public void GetClosestPointToCell(Vector2 targetPos, out Vector3 closestPoint, out bool isOutsideCell)
{
float closestSqrDistance = float.MaxValue;
closestPoint = Vector3.zero;
foreach (var edgeData in _contentDictionary.Keys)
{
if (SomeMath.PointInTriangle(edgeData.leftV2, edgeData.rightV2, centerVector2, targetPos))
{
closestPoint = new Vector3(targetPos.x, SomeMath.CalculateHeight(edgeData.leftV3, edgeData.rightV3, centerVector3, targetPos.x, targetPos.y), targetPos.y);
isOutsideCell = false;
return;
}
else
{
Vector3 curInte;
SomeMath.ClosestToSegmentTopProjection(edgeData.leftV3, edgeData.rightV3, targetPos, true, out curInte);
float curSqrDist = SomeMath.SqrDistance(targetPos.x, targetPos.y, curInte.x, curInte.z);
if (curSqrDist < closestSqrDistance)
{
closestSqrDistance = curSqrDist;
closestPoint = curInte;
}
}
}
isOutsideCell = true;
return;
}
public bool GetClosestToHull(float x, float y, float z, out Cell cell, out Vector3 closestToOutlinePos)
{
cell = null;
closestToOutlinePos = new Vector3();
if (empty || _mapCell == null)
{
return(false);
}
float sqrDist = float.MaxValue;
foreach (var pair in _contourLib)
{
CellContentData val = pair.Key;
Vector3 curNearest = pair.Key.NearestPoint(x, y, z);
float curSqrDist = SomeMath.SqrDistance(curNearest.x, curNearest.y, curNearest.z, x, y, z);
if (curSqrDist < sqrDist)
{
sqrDist = curSqrDist;
cell = pair.Value;
closestToOutlinePos = curNearest;
}
}
return(true);
}
private bool IsVisible(int a, int b)
{
float ax = _nodes[a].x;
float ay = _nodes[a].z;
float bx = _nodes[b].x;
float by = _nodes[b].z;
float abx = bx - ax;
float aby = by - ay;
float sqrDist = SomeMath.SqrDistance(ax, ay, bx, by);
isVisibleTemp.Clear();
DDARasterization.DrawLine(ax - chunkPos.x, ay - chunkPos.y, bx - chunkPos.x, by - chunkPos.y, edgeMapPiselSize, IsVisibleDelegate);
foreach (var edge in isVisibleTemp)
{
if (edge.Contains(a, b))
{
continue;
}
float ix, iy;
if (SomeMath.RayIntersect(ax, ay, abx, aby, _nodes[edge.a].x, _nodes[edge.a].z, _nodes[edge.b].x, _nodes[edge.b].z, out ix, out iy) && SomeMath.SqrDistance(ax, ay, ix, iy) < sqrDist)
{
return(false);
}
}
return(true);
//Vector2 aPos = _nodes[a].positionV2;
//Vector2 bPos = _nodes[b].positionV2;
//Vector2 dir = bPos - aPos;
//float curSqrDist = SomeMath.SqrDistance(aPos, bPos);
//foreach (var edge in edges) {
// if (edge.Contains(a, b))
// continue;
// Vector2 intersection;
// if (SomeMath.RayIntersectXZ(
// aPos, dir, //from, direction
// NodePosV2(edge.a), NodePosV2(edge.b), //a, b
// out intersection)
// && SomeMath.SqrDistance(aPos, intersection) < curSqrDist) {
// //Debuger3.AddRay(new Vector3(intersection.x, 0, intersection.y), Vector3.up, Color.magenta);
// //Debuger3.AddLine(ds.NodePosV3(a), new Vector3(intersection.x, 0, intersection.y), Color.magenta);
// //Debuger3.AddLine(ds.NodePosV3(b), new Vector3(intersection.x, 0, intersection.y), Color.magenta);
// return false;
// }
//}
//return true;
}
public void AddCover(NodeCoverTemp coverInfo)
{
Cover cover = new Cover(coverInfo.positionV3, coverInfo.connection.positionV3, coverInfo.connectionType, coverInfo.normal);
Vector3 agentNormalPoint = coverInfo.normal * properties.radius;
foreach (var coverPoint in coverInfo.points)
{
Vector3 pointPlusAgentOffset = coverPoint.positionV3 + agentNormalPoint;
HashSet <Cell> nearbyCells = new HashSet <Cell>();
foreach (var edge in coverPoint.edges)
{
CellContentData data = new CellContentData(edge);
foreach (var cell in _cells)
{
if (cell.Contains(data))
{
nearbyCells.Add(cell);
}
}
}
float closestSqrDistance = float.MaxValue;
Cell closestCell = null;
Vector3 closestPoint = Vector3.zero;
foreach (var cell in nearbyCells)
{
bool isOutside;
Vector3 currentPoint;
cell.GetClosestPointToCell(pointPlusAgentOffset, out currentPoint, out isOutside);
float curSqrDistance = SomeMath.SqrDistance(pointPlusAgentOffset, currentPoint);
if (curSqrDistance < closestSqrDistance)
{
closestCell = cell;
closestPoint = currentPoint;
closestSqrDistance = curSqrDistance;
}
}
if (closestCell == null)
{
//Debuger3.AddDot(coverPoint.positionV3, Color.red);
continue;
}
NodeCoverPoint coverNode = new NodeCoverPoint(coverPoint.positionV3, closestPoint, closestCell, cover);
cover.AddCoverPoint(coverNode);
}
covers.Add(cover);
}
public CirclePattern(int radius)
{
this.radius = radius;
size = radius + radius - 1;
int sqrRadius = (radius - 1) * (radius - 1);
pattern = new bool[size * size];
for (int x = 0; x < size; x++)
{
for (int y = 0; y < size; y++)
{
pattern[(y * size) + x] = SomeMath.SqrDistance(x, y, radius - 1, radius - 1) <= sqrRadius;
}
}
}
private static void SearchRecursive(PathFinderAgent agent, Bounds2D targetBounds, Vector3 targetPos, float targetSqrDist, int targetBranch)
{
kDTreeBranch branch = branches[targetBranch];
if (branch.bounds.Overlap(targetBounds))
{
if (branch.branchA != -1)
{
SearchRecursive(agent, targetBounds, targetPos, targetSqrDist, branch.branchA);
SearchRecursive(agent, targetBounds, targetPos, targetSqrDist, branch.branchB);
}
else
{
for (int i = branch.start; i < branch.end; i++)
{
var realAgent = PathFinder.agents[agents[i].index];
if (agent != realAgent)
{
float curSqrDist = SomeMath.SqrDistance(targetPos, realAgent.positionVector3);
if (curSqrDist < targetSqrDist)
{
if (agent.neighbourAgents.Count < agent.maxNeighbors)
{
agent.neighbourAgents.Add(realAgent);
agent.neighbourSqrDistances.Add(curSqrDist);
}
else
{
for (int n = 0; n < agent.maxNeighbors; n++)
{
if (agent.neighbourSqrDistances[n] > curSqrDist)
{
agent.neighbourAgents[n] = realAgent;
agent.neighbourSqrDistances[n] = curSqrDist;
break;
}
}
}
}
}
}
}
}
}
private void SearchRecursive(T target, Bounds2D targetBounds, Vector2 targetPos, float targetSqrDist, int targetBranch)
{
kDTreeBranch branch = branches[targetBranch];
if (branch.bounds.Overlap(targetBounds))
{
if (branch.branchA != -1)
{
SearchRecursive(target, targetBounds, targetPos, targetSqrDist, branch.branchA);
SearchRecursive(target, targetBounds, targetPos, targetSqrDist, branch.branchB);
}
else
{
for (int i = branch.start; i < branch.end; i++)
{
T realAgent = data[members[i].index];
if (target.Equals(realAgent) == false)
{
float curSqrDist = SomeMath.SqrDistance(targetPos, realAgent.position);
if (curSqrDist < targetSqrDist)
{
if (target.neighbourAgents.Count < target.maxNeighbours)
{
target.neighbourAgents.Add(realAgent);
target.neighbourSqrDistances.Add(curSqrDist);
}
else
{
for (int n = 0; n < target.maxNeighbours; n++)
{
if (target.neighbourSqrDistances[n] > curSqrDist)
{
target.neighbourAgents[n] = realAgent;
target.neighbourSqrDistances[n] = curSqrDist;
break;
}
}
}
}
}
}
}
}
}
public bool GetClosestCell(float x, float y, float z, out Cell cell, out bool outsideCell, out Vector3 closestPoint)
{
if (empty || _mapCell == null)
{
outsideCell = true;
cell = null;
closestPoint = new Vector3();
return(false);
}
Cell getCell;
Vector3 resultCell;
if (GetCell(x, y, z, out getCell, out resultCell))
{
Cell getHull;
Vector3 resultHull;
GetClosestToHull(x, y, z, out getHull, out resultHull);
outsideCell = SomeMath.SqrDistance(x, y, z, resultCell.x, resultCell.y, resultCell.z) > SomeMath.SqrDistance(x, y, z, resultHull.x, resultHull.y, resultHull.z);
if (outsideCell)
{
closestPoint = resultHull;
cell = getHull;
}
else
{
closestPoint = resultCell;
cell = getCell;
}
}
else
{
GetClosestToHull(x, y, z, out cell, out closestPoint);
outsideCell = true;
}
return(true);
}
private void ConnectBattleGrid(float connectDistance)
{
if (battleGrid == null)
{
return;
}
float connectionDistanceSqr = connectDistance * connectDistance;
for (int i = 0; i < 4; i++)
{
Directions directionFrom = (Directions)i;
Directions directionTo = Enums.Opposite(directionFrom);
Graph neighbourGraph;
if (TryGetNeighbour(directionFrom, out neighbourGraph) == false)
{
continue;
}
BattleGrid neighbourBattleGrid = neighbourGraph.battleGrid;
if (neighbourBattleGrid == null)
{
Debug.LogWarningFormat("somehow i have battle grid but my neighbour is not. probably just empty graph. my positions is {0}", chunk.position.ToString());
continue;
}
var ourBorder = battleGrid.GetBorderLinePoints(directionFrom);
var neighbourBorder = neighbourBattleGrid.GetBorderLinePoints(directionTo);
Axis projectionAxis = Axis.x;
if (directionFrom == Directions.xPlus || directionFrom == Directions.xMinus)
{
projectionAxis = Axis.x;
}
if (directionFrom == Directions.zPlus || directionFrom == Directions.zMinus)
{
projectionAxis = Axis.z;
}
BattleGridPoint point;
foreach (var ourBorderPoint in ourBorder)
{
point = null;
float curClosestSqrDist = float.MaxValue;
foreach (var neighbourBorderPoint in neighbourBorder)
{
if ((projectionAxis == Axis.x && ourBorderPoint.gridZ != neighbourBorderPoint.gridZ) ||
(projectionAxis == Axis.z && ourBorderPoint.gridX != neighbourBorderPoint.gridX))
{
continue;
}
float curSqrDist = SomeMath.SqrDistance(ourBorderPoint.positionV3, neighbourBorderPoint.positionV3);
if (curSqrDist < curClosestSqrDist && curSqrDist <= connectionDistanceSqr)
{
point = neighbourBorderPoint;
curClosestSqrDist = curSqrDist;
}
}
if (point != null)
{
ourBorderPoint.neighbours[(int)directionFrom] = point;
point.neighbours[(int)directionTo] = ourBorderPoint;
}
}
//for (int bi = 0; bi < ourBorder.Length; bi++) {
// foreach (var curPoint in ourBorder[bi]) {
// BattleGridPoint nbpn = null;
// float curClosestSqrDist = float.MaxValue;
// foreach (var neighbourPoint in neighbourBorder[bi]) {
// float curSqrDist = SomeMath.SqrDistance(curPoint.positionV3, neighbourPoint.positionV3);
// if(curSqrDist <= connectionDistanceSqr) {
// nbpn = neighbourPoint;
// curClosestSqrDist = curSqrDist;
// }
// }
// if(nbpn != null) {
// curPoint.neighbours[(int)directionFrom] = nbpn;
// nbpn.neighbours[(int)directionTo] = curPoint;
// }
// }
//}
}
}
private void CheckJumpConnections()
{
if (empty || portalBases.Count == 0)
{
return;
}
float rad = properties.radius;
float radSqr = rad * rad;
LayerMask mask = properties.includedLayers;
float jumpUpSqr = properties.JumpUp * properties.JumpUp;
float jumpDownSqr = properties.JumpDown * properties.JumpDown;
float maxCheckDistance = Math.Max(jumpUpSqr, jumpDownSqr);
float sampleStep = PathFinder.gridSize / properties.voxelsPerChunk;
int sampleSteps = Mathf.RoundToInt(properties.radius / sampleStep) + 2;//plus some extra
float bottomOffset = 0.2f;
float agentHeightAjusted = properties.height - rad;
float agentBottomAjusted = properties.radius + bottomOffset;
RaycastHit hitCapsule, hitRaycast;
if (agentHeightAjusted - agentBottomAjusted < 0) // somehow became spherical
{
agentHeightAjusted = agentBottomAjusted;
}
foreach (var portal in new List <JumpPortalBase>(portalBases))
{
Vector3 topAdd = new Vector3(0, agentBottomAjusted, 0);
Vector3 bottomAdd = new Vector3(0, agentHeightAjusted, 0);
Vector3 portalPosV3 = portal.positionV3;
Vector3 mountPointBottom = portalPosV3 + topAdd;
Vector3 mountPointTop = portalPosV3 + bottomAdd;
if (Physics.CheckCapsule(mountPointBottom, mountPointTop, rad, properties.includedLayers) ||
(Physics.CapsuleCast(mountPointBottom, mountPointTop, rad, portal.normal, out hitCapsule, rad * 3, mask) &&
SomeMath.SqrDistance(ToV2(hitCapsule.point), portal.positionV2) < (rad * 3) * (rad * 3)))
{
portalBases.Remove(portal);
continue;
}
for (int i = 0; i < sampleSteps; i++)
{
Vector3 normalOffset = portal.normal * (properties.radius + (i * sampleStep));
Vector3 axisPoint = mountPointBottom + normalOffset;
if (Physics.Raycast(axisPoint, Vector3.down, out hitRaycast, maxCheckDistance, mask) == false)
{
continue;
}
Vector3 raycastHitPoint = hitRaycast.point;
if (Physics.CapsuleCast(axisPoint, mountPointTop + normalOffset, rad, Vector3.down, out hitCapsule, Mathf.Infinity, mask) == false)
{
continue;
}
if (SomeMath.SqrDistance(raycastHitPoint, hitCapsule.point) > radSqr)
{
continue;
}
if (SomeMath.SqrDistance(portal.positionV3 + normalOffset, hitCapsule.point) < radSqr || Vector3.Angle(Vector3.up, hitCapsule.normal) > properties.maxSlope)
{
continue;
}
bool outside;
Cell closest;
Vector3 closestPos;
GetClosestCell(raycastHitPoint, out closest, out outside, out closestPos);
//Debuger3.AddLine(raycastHitPoint, closestPos, Color.red);
//Debuger3.AddRay(raycastHitPoint, Vector3.up, Color.magenta, 0.5f);
if (outside)
{
continue;
}
Cell cell;
bool outsideCell;
Vector3 closestPoint;
GetClosestCell(closestPos, out cell, out outsideCell, out closestPoint);
//Cell targetCell = GetClosestCellExpensive(raycastHitPoint, out closestPos);
if (SomeMath.SqrDistance(closestPos, hitCapsule.point) > radSqr)
{
//portalBases.Remove(portal);
goto NEXT_PORTAL;
}
float fallSqrDistance = SomeMath.SqrDistance(portal.positionV3 + normalOffset, raycastHitPoint);
if (fallSqrDistance < jumpUpSqr)
{
foreach (var pair in portal.cellMountPoints)
{
cell.SetContent(new CellContentPointedConnection(closestPos, raycastHitPoint, pair.Value, portal.positionV3, ConnectionJumpState.jumpUp, cell, pair.Key, false));
//cell.AddConnection(new CellJumpUpConnection(cell, pair.Key, closestPos, raycastHitPoint, portal.positionV3, pair.Value, false));
//cell.SetJumpUpConnection(pair.Key, closestPos, raycastHitPoint, portal.positionV3, pair.Value);
}
}
if (fallSqrDistance < jumpDownSqr)
{
foreach (var pair in portal.cellMountPoints)
{
pair.Key.SetContent(new CellContentPointedConnection(pair.Value, raycastHitPoint, closestPos, portal.positionV3, ConnectionJumpState.jumpDown, pair.Key, cell, false));
//pair.Key.AddConnection(new CellJumpDownConnection(pair.Key, cell, pair.Value, portal.positionV3, raycastHitPoint, closestPos, false));
//pair.Key.SetJumpDownConnection(cell, pair.Value, portal.positionV3, raycastHitPoint, closestPos);
}
}
goto NEXT_PORTAL;
}
//portalBases.Remove(portal);
NEXT_PORTAL : {
continue;
}
}
}
public void GetClosestCell(Vector3 pos, out Cell cell, out bool outsideCell, out Vector3 closestPoint)
{
cell = null;
closestPoint = Vector3.zero;
outsideCell = true;
//if true then no result
if (empty || _map == null)
{
return;
}
float s = PathFinder.gridSize / PathFinder.CELL_GRID_SIZE;
List <Cell> mapChunk = _map
[Mathf.Clamp((int)((pos.x - chunk.realX) / s), 0, PathFinder.CELL_GRID_SIZE - 1)]
[Mathf.Clamp((int)((pos.z - chunk.realZ) / s), 0, PathFinder.CELL_GRID_SIZE - 1)];
float sqrDist = float.MaxValue;
//Debug.Log(mapChunk.Count);
//search cell in chunk are inside chunk
if (mapChunk.Count > 0)
{
for (int i = 0; i < mapChunk.Count; i++)
{
Vector3 currentClosestToCell;
//see if this point are inside cell and get position if are
if (mapChunk[i].GetPointInsideCell(pos, out currentClosestToCell))
{
float curSqrDist = SomeMath.SqrDistance(pos, currentClosestToCell);
if (curSqrDist < sqrDist)
{
sqrDist = curSqrDist;
cell = mapChunk[i];
outsideCell = false;
closestPoint = currentClosestToCell;
}
}
}
}
//if there was result then here it is
if (cell != null)
{
return;
}
//if there is no result then search it by checking all contours that represent hole in graph
foreach (var pair in _contourLib)
{
Vector3 curNearest = SomeMath.NearestPointOnSegment(pair.Key.a, pair.Key.b, pos);
float curSqrDist = SomeMath.SqrDistance(curNearest, pos);
if (curSqrDist < sqrDist)
{
sqrDist = curSqrDist;
cell = pair.Value;
closestPoint = curNearest;
}
}
}
//takes edges and axis. check if edge exist, if exist add closest point to cell
public void AddPortal(IEnumerable <EdgeAbstract> edges, Vector3 axis)
{
Vector2 axisV2 = new Vector2(axis.x, axis.z);
Dictionary <Cell, Vector3> cellMountPoints = new Dictionary <Cell, Vector3>();
foreach (var abstractEdge in edges)
{
CellContentData data = new CellContentData(abstractEdge);
Vector3 intersection;
SomeMath.ClosestToSegmentTopProjection(data.a, data.b, axisV2, true, out intersection);
foreach (var cell in _cells)
{
if (cell.Contains(data))
{
if (cellMountPoints.ContainsKey(cell))
{
if (SomeMath.SqrDistance(cellMountPoints[cell], axis) > SomeMath.SqrDistance(intersection, axis))
{
cellMountPoints[cell] = intersection;
}
}
else
{
cellMountPoints.Add(cell, intersection);
}
}
}
}
Vector2 normalRaw;
switch (cellMountPoints.Count)
{
case 0:
return;
case 1:
normalRaw = ToV2((cellMountPoints.First().Value - axis)).normalized * -1;
break;
case 2:
normalRaw = (
ToV2(cellMountPoints.First().Value - axis).normalized +
ToV2(cellMountPoints.Last().Value - axis).normalized).normalized * -1;
break;
default:
normalRaw = Vector2.left;
Dictionary <Cell, float> cellAngles = new Dictionary <Cell, float>();
Cell first = cellMountPoints.First().Key;
cellAngles.Add(first, 0f);
Vector3 firstDirV3 = cellMountPoints.First().Value - axis;
Vector2 firstDirV2 = ToV2(firstDirV3);
foreach (var pair in cellMountPoints)
{
if (pair.Key == first)
{
continue;
}
Vector2 curDir = new Vector2(pair.Value.x - axis.x, pair.Value.z - axis.z);
cellAngles.Add(pair.Key, Vector2.Angle(firstDirV2, curDir) * Mathf.Sign(SomeMath.V2Cross(firstDirV2, curDir)));
}
normalRaw = (
ToV2(cellMountPoints[cellAngles.Aggregate((l, r) => l.Value > r.Value ? l : r).Key] - axis).normalized +
ToV2(cellMountPoints[cellAngles.Aggregate((l, r) => l.Value < r.Value ? l : r).Key] - axis).normalized).normalized * -1;
break;
}
portalBases.Add(new JumpPortalBase(cellMountPoints, axis, new Vector3(normalRaw.x, 0, normalRaw.y)));
}
private float GetSide(Vector3 pos)
{
return(SomeMath.SqrDistance(intX, intZ, pos.x, pos.z) * Mathf.Sign(SomeMath.LinePointSideMath(fromV2, toV2, pos.x, pos.z)));
}
private void Raycast(Vector3 origin, Vector3 direction, out RaycastHitNavMesh hit,
float length, int maxIterations, Passability expectedPassability, Area expectedArea, Cell cell)
{
HashSet <CellContentData> raycastExclude = new HashSet <CellContentData>();
List <RaycastSomeData> raycastTempData = new List <RaycastSomeData>();
float maxLengthSqr = length * length;
for (int iteration = 0; iteration < maxIterations; iteration++)
{
raycastTempData.Clear();//iteration data cleared
foreach (var pair in cell.dataContentPairs)
{
CellContentData curData = pair.Key;
if (!raycastExclude.Add(curData))//mean it's already contain this
{
continue;
}
Vector3 intersect;
if (SomeMath.RayIntersectXZ(origin, direction, curData.leftV3, curData.rightV3, out intersect))
{
if (pair.Value != null)
{
Cell otherCell = pair.Value.connection;
if (otherCell == cell | !otherCell.canBeUsed)
{
continue;
}
if (cell.passability != otherCell.passability || cell.area != otherCell.area)
{
hit = new RaycastHitNavMesh(intersect, SomeMath.SqrDistance(origin, intersect) < maxLengthSqr);//!!!
return;
}
raycastTempData.Add(new RaycastSomeData(intersect, otherCell));
}
else
{
raycastTempData.Add(new RaycastSomeData(intersect, null));
}
}
}
//check if there possible connection
for (int i = 0; i < raycastTempData.Count; i++)
{
if (raycastTempData[i].cell != null)
{
cell = raycastTempData[i].cell;
goto CONTINUE;
}
}
//now we definetly hit something and now find furthest
float furthestSqrDist = 0f;
Vector3 furthest = origin;
for (int i = 0; i < raycastTempData.Count; i++)
{
float curSqrDist = SomeMath.SqrDistance(raycastTempData[i].point, origin);
if (curSqrDist > furthestSqrDist)
{
furthestSqrDist = curSqrDist;
furthest = raycastTempData[i].point;
}
}
hit = new RaycastHitNavMesh(furthest, SomeMath.SqrDistance(origin, furthest) < maxLengthSqr);
return;
CONTINUE : { continue; }
}
hit = new RaycastHitNavMesh(origin, true, true);
return;
}