private void OptimizePath(List <AStarNode> path) { if (path.Count == 0) { return; } int currentPosition = 0; var edge = new NavigationEdge(); while (currentPosition < path.Count - 2) { edge.A = path[currentPosition].Position; edge.B = path[currentPosition + 2].Position; var result = constrainedEdgePolygons.CalculateMultipleIntersection(edge, 3); if (CanNotOptimize(result)) { currentPosition++; break; } else { path.RemoveAt(currentPosition + 1); } } }
public DeterministicVector2 Steer(Unit unit, List <Unit> neighbors, List <NavigationEdge> path, Map map) { if (path == null || path.Count == 0) { return(new DeterministicVector2()); } bool isLastMovementPossible = unit.LastSteering.GetLengthSquared() != 0; DeterministicVector2 naiveNewPosition = unit.Position; DeterministicVector2 lastSteering = unit.LastSteering; if (isLastMovementPossible) { lastSteering = lastSteering.Normalize(); naiveNewPosition += lastSteering; } DeterministicFloat shortestDistance = new DeterministicFloat(long.MaxValue, false); NavigationEdge nearestEdge = new NavigationEdge(); DeterministicVector2 closestPoint = new DeterministicVector2(); for (var i = 0; i < path.Count; i++) { var pathEdge = path[i]; var distanceResult = pathEdge.GetDistance(naiveNewPosition); if (distanceResult.Distance < shortestDistance) { shortestDistance = distanceResult.Distance; nearestEdge = pathEdge; closestPoint = distanceResult.ClosestPoint; if (i > 0) { unit.RecalcPathOnNextUpdate = true; } // naive positioning is on path, we don't need to steer if (isLastMovementPossible && shortestDistance <= PathSize) { return(lastSteering); } } } var dir = nearestEdge.B - nearestEdge.A; return(closestPoint + dir.Normalize() - unit.Position); }