//
// Find the closest point on an obstacles border to a point
//
public static Vector3 FindClosestPointOnObstacle(Obstacle o, Vector3 point)
{
Vector2 p = point.XZ();
Vector2 FL = o.cornerPos.FL.XZ();
Vector2 FR = o.cornerPos.FR.XZ();
Vector2 BL = o.cornerPos.BL.XZ();
Vector2 BR = o.cornerPos.BR.XZ();
//Each obstacle has four edges, so we need to test them all
Vector2 p1 = HelpStuff.GetClosestPointOnLineSegment(FL, FR, p);
Vector2 p2 = HelpStuff.GetClosestPointOnLineSegment(FR, BR, p);
Vector2 p3 = HelpStuff.GetClosestPointOnLineSegment(BR, BL, p);
Vector2 p4 = HelpStuff.GetClosestPointOnLineSegment(BL, FL, p);
//Can we speed up by using the normal of the line, and then the dot product to
//not test lines whose normal point in the opposite direction?
float d1 = (p1 - p).sqrMagnitude;
float d2 = (p2 - p).sqrMagnitude;
float d3 = (p3 - p).sqrMagnitude;
float d4 = (p4 - p).sqrMagnitude;
float closestDist = Mathf.Infinity;
Vector2 closestPos = Vector2.zero;
if (d1 < closestDist)
{
closestDist = d1;
closestPos = p1;
}
if (d2 < closestDist)
{
closestDist = d2;
closestPos = p2;
}
if (d3 < closestDist)
{
closestDist = d3;
closestPos = p3;
}
if (d4 < closestDist)
{
closestDist = d4;
closestPos = p4;
}
return(closestPos.XYZ());
}
/// <summary>
/// Calculate the car's new heading
/// </summary>
/// <param name="theta">The car's heading (= rotation) [rad]</param>
/// <param name="beta">Steering angle [rad]</param>
/// <returns>The car's new heading</returns>
public static float CalculateNewHeading(float theta, float beta)
{
//Change heading
theta = theta + beta;
//Clamp heading - is sometimes causing infinite loop so dont use the old version?
theta = HelpStuff.WrapAngleInRadians(theta);
//Clamp heading
//if (theta > 2f * Mathf.PI)
//{
// theta = theta - 2f * Mathf.PI;
//}
//if (theta < 0f)
//{
// theta = 2f * Mathf.PI + theta;
//}
//Debug.Log(theta + " " + theta2);
return(theta);
}
/// <summary>
/// Calculate the heading of a trailer attached to a drag vehicle
/// </summary>
/// <param name="thetaOld">The trailers old heading [rad]</param>
/// <param name="thetaOldDragVehicle">The drag vehicles old heading [rad]</param>
/// <param name="D">Drive distance [m]. Should be negative if we reverse</param>
/// <param name="d">Distance between trailer attachment point and trailer rear axle [m]</param>
/// <returns>The trailer's new heading [rad]</returns>
public static float CalculateNewTrailerHeading(float thetaOld, float thetaOldDragVehicle, float D, float d)
{
//According to some D is velocity of the drag vehicle but is not really working
//From "Planning algorithms" which is different from http://planning.cs.uiuc.edu/node661.html#77556
//where (thetaOldDragVehicle - thetaOld) is the opposite which gives a mirrored result
//float theta = thetaOld + (D / d) * Mathf.Sin(thetaOldDragVehicle - thetaOld);
float theta = thetaOld + ((D / d) * Mathf.Sin(thetaOldDragVehicle - thetaOld));
//Clamp heading - is sometimes causing infinite loop so dont use?
theta = HelpStuff.WrapAngleInRadians(theta);
//Clamp heading
//if (theta > 2f * Mathf.PI)
//{
// theta = theta - 2f * Mathf.PI;
//}
//if (theta < 0f)
//{
// theta = 2f * Mathf.PI + theta;
//}
return(theta);
}
//Find the neighboring nodes to a node by checking all nodes around it
private static HashSet <FlowFieldNode> FindNeighboringNodes(FlowFieldNode node, FlowFieldNode[,] nodeArray, int mapWidth, bool includeCorners)
{
HashSet <IntVector2> neighborCells = new HashSet <IntVector2>();
//Get the directions we can move in, which are up, left, right, down
IntVector2[] delta = HelpStuff.delta;
if (includeCorners)
{
delta = HelpStuff.deltaWithCorners;
}
//Will track if at least one neighbor is an obstacle, which may be useful to know later
bool isNeighborObstacle = false;
for (int i = 0; i < delta.Length; i++)
{
IntVector2 cellPos = new IntVector2(node.cellPos.x + delta[i].x, node.cellPos.z + delta[i].z);
//Is this cell position within the grid?
if (IsCellPosWithinGrid(cellPos, mapWidth))
{
//Is not a valid neighbor if its obstacle
if (!nodeArray[cellPos.x, cellPos.z].isWalkable)
{
isNeighborObstacle = true;
}
else
{
neighborCells.Add(cellPos);
}
}
}
//If we are checking 8 neighbors we have to be careful to not jump diagonally if one cell next to the diagonal is obstacle
//This is a costly operation (0.3 seconds if we do it for all cells) so only do it if at least one neighbor is obstacle
if (includeCorners && isNeighborObstacle)
{
HashSet <IntVector2> corners = new HashSet <IntVector2>(HelpStuff.deltaJustCorners);
//Loop through all 8 neighbors
for (int i = 0; i < delta.Length; i++)
{
//Is this neighbor a corner?
if (corners.Contains(delta[i]))
{
IntVector2 cellPos = new IntVector2(node.cellPos.x + delta[i].x, node.cellPos.z + delta[i].z);
//Have we added the corner to the list of neighbors
if (!neighborCells.Contains(cellPos))
{
continue;
}
//Check if neighbors to the corner are obstacles, if so we cant move to this corner
IntVector2 n1 = delta[HelpStuff.ClampListIndex(i + 1, delta.Length)];
IntVector2 n2 = delta[HelpStuff.ClampListIndex(i - 1, delta.Length)];
IntVector2 cellPos_n1 = new IntVector2(node.cellPos.x + n1.x, node.cellPos.z + n1.z);
IntVector2 cellPos_n2 = new IntVector2(node.cellPos.x + n2.x, node.cellPos.z + n2.z);
if (!nodeArray[cellPos_n1.x, cellPos_n1.z].isWalkable || !nodeArray[cellPos_n2.x, cellPos_n2.z].isWalkable)
{
//This is not a valid neighbor so remove it from neighbors
neighborCells.Remove(cellPos);
}
}
}
}
//From cell to node
HashSet <FlowFieldNode> neighborNodes = new HashSet <FlowFieldNode>();
foreach (IntVector2 cell in neighborCells)
{
neighborNodes.Add(nodeArray[cell.x, cell.z]);
}
return(neighborNodes);
}
//
// Generate a path with Hybrid A*
//
public static List <Node> GeneratePath(Car startCar, Car endCar, Map map, List <Node> allExpandedNodes, Car startTrailer)
{
//Reset timers
timer_selectLowestCostNode = 0;
timer_addNodeToHeap = 0;
timer_findChildren = 0;
timer_isCollidingWithObstacle = 0;
timer_ReedsSheppNode = 0;
timer_ReedsSheppHeuristics = 0;
timer_TrailerCollision = 0;
//Other data we want to track
//How many nodes did we prune?
int prunedNodes = 0;
//To track max number of nodes in the heap
int maxNodesInHeap = 0;
//Init the data structure we need
int mapWidth = map.MapWidth;
//Is faster to cache this than using map.cellData
Cell[,] cellData = map.cellData;
//Open nodes - the parameter is how many items can fit in the heap
//If we lower the heap size it will still find a path, which is more drunk
Heap <Node> openNodes = new Heap <Node>(200000);
//int in the dictionaries below is the rounded heading used to enter a cell
HashSet <int>[,] closedCells = new HashSet <int> [mapWidth, mapWidth];
//The node in the cell with the lowest g-cost at a certain angle
Dictionary <int, Node>[,] lowestCostNodes = new Dictionary <int, Node> [mapWidth, mapWidth];
//Trailer
//int in the dictionaries below is the rounded heading used to enter a cell
HashSet <int>[,] closedCellsTrailer = new HashSet <int> [mapWidth, mapWidth];
HashSet <int>[,] lowestCostNodesTrailer = new HashSet <int> [mapWidth, mapWidth];
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
closedCells[x, z] = new HashSet <int>();
lowestCostNodes[x, z] = new Dictionary <int, Node>();
//Trailer
closedCellsTrailer[x, z] = new HashSet <int>();
lowestCostNodesTrailer[x, z] = new HashSet <int>();
}
}
//Create the first node
//Why rear wheel? Because we need that position when simulating the "skeleton" car
//and then it's easier if everything is done from the rear wheel positions
IntVector2 startCellPos = map.ConvertWorldToCell(startCar.rearWheelPos);
Node node = new Node(
previousNode: null,
rearWheelPos: startCar.rearWheelPos,
heading: startCar.HeadingInRadians,
isReversing: false);
if (startTrailer != null)
{
node.TrailerHeadingInRadians = startTrailer.HeadingInRadians;
}
node.AddCosts(gCost: 0f, hCost: cellData[startCellPos.x, startCellPos.z].heuristics);
openNodes.Add(node);
//The end of the path, which we will return
Node finalNode = null;
//bools so we can break out of the main loop
//Set when search is complete
bool found = false;
//Set if we can't find a node to expand
bool resign = false;
//To break out of the loop if it takes too long time
int iterations = 0;
IntVector2 goalCellPos = map.ConvertWorldToCell(endCar.rearWheelPos);
while (!found && !resign)
{
if (iterations > 400000)
{
Debug.Log("Stuck in infinite loop");
break;
}
iterations += 1;
//If we don't have any nodes to expand
if (openNodes.Count == 0)
{
resign = true;
Debug.Log("Failed to find a path");
}
//We have nodes to expand
else
{
if (openNodes.Count > maxNodesInHeap)
{
maxNodesInHeap = openNodes.Count;
}
//Get the node with the lowest f cost
int timeBefore = Environment.TickCount;
Node nextNode = openNodes.RemoveFirst();
timer_selectLowestCostNode += Environment.TickCount - timeBefore;
//Close this cell
IntVector2 cellPos = map.ConvertWorldToCell(nextNode.rearWheelPos);
int roundedHeading = HelpStuff.RoundValue(nextNode.HeadingInDegrees, headingResolution);
HashSet <int> closedHeadingsInThisCell = closedCells[cellPos.x, cellPos.z];
bool haveAlreadyClosedCell = false;
//Close the cell, but we can still drive into this cell from another angle
if (!closedHeadingsInThisCell.Contains(roundedHeading))
{
closedHeadingsInThisCell.Add(roundedHeading);
}
else if (startTrailer == null)
{
haveAlreadyClosedCell = true;
}
if (startTrailer != null)
{
int roundedHeadingTrailer = HelpStuff.RoundValue(nextNode.TrailerHeadingInDegrees, headingResolutionTrailer);
HashSet <int> closedTrailerHeadingsInThisCell = closedCellsTrailer[cellPos.x, cellPos.z];
if (!closedTrailerHeadingsInThisCell.Contains(roundedHeadingTrailer))
{
closedTrailerHeadingsInThisCell.Add(roundedHeadingTrailer);
}
else
{
haveAlreadyClosedCell = true;
}
}
//We have already expanded a better node with the same heading so dont expand this node
if (haveAlreadyClosedCell)
{
iterations -= 1;
continue;
}
//For debugging
allExpandedNodes.Add(nextNode);
//Check if the vehicle has reached the target
float distanceSqrToGoal = (nextNode.rearWheelPos - endCar.rearWheelPos).sqrMagnitude;
//But we also need to make sure the vehiclke has correct heading
float headingDifference = Mathf.Abs(endCar.HeadingInDegrees - nextNode.HeadingInDegrees);
//If we end up in the same cell or is within a certain distance from the goal
if ((distanceSqrToGoal < posAccuracy * posAccuracy || (cellPos.x == goalCellPos.x && cellPos.z == goalCellPos.z)) &&
headingDifference < headingAccuracy)
{
found = true;
Debug.Log("Found a path");
finalNode = nextNode;
//Make sure the end node has the same position as the target
finalNode.rearWheelPos.x = endCar.rearWheelPos.x;
finalNode.rearWheelPos.z = endCar.rearWheelPos.z;
}
//If we havent found the goal, then expand this node
else
{
//Get all child nodes
timeBefore = Environment.TickCount;
List <Node> children = GetChildrenToNode(nextNode, map, cellData, startCar.carData, endCar, startTrailer);
timer_findChildren += Environment.TickCount - timeBefore;
//Should we add any of the child nodes to the open nodes?
foreach (Node child in children)
{
IntVector2 childCell = map.ConvertWorldToCell(child.rearWheelPos);
int roundedChildHeading = HelpStuff.RoundValue(child.HeadingInDegrees, headingResolution);
//Has this cell been closed with this heading?
//If so, it means we already have a path at this cell with this heading,
//and the existing node is cheaper because we have already expanded it
if (closedCells[childCell.x, childCell.z].Contains(roundedChildHeading) && startTrailer == null)
{
prunedNodes += 1;
continue;
}
//If we have a trailer
else if (closedCells[childCell.x, childCell.z].Contains(roundedChildHeading) && startTrailer != null)
{
int roundedTrailerHeading = HelpStuff.RoundValue(child.TrailerHeadingInDegrees, headingResolutionTrailer);
if (closedCellsTrailer[childCell.x, childCell.z].Contains(roundedTrailerHeading))
{
prunedNodes += 1;
continue;
}
}
//Have we already expanded a node with lower cost in this cell at this heading?
float costSoFar = child.gCost;
//The dictionary with lowest cost nodes in this cell
Dictionary <int, Node> nodesWithLowestCost = lowestCostNodes[childCell.x, childCell.z];
//Have we expanded with this angle to the cell before?
if (nodesWithLowestCost.ContainsKey(roundedChildHeading) && startTrailer == null)
{
//If the open node has a large gCost then we need to update that node with data
//from the child node
if (costSoFar < nodesWithLowestCost[roundedChildHeading].gCost)
{
//If this child node is better then we should update the node in the open list
//which is faster than deleting the old node and adding this child node
Node existingNode = nodesWithLowestCost[roundedChildHeading];
child.StealDataFromThisNode(existingNode);
//Modify the heap-position of the node already in the open nodes
openNodes.UpdateItem(existingNode);
}
//If the open node has a smaller gCost, then we dont need this child node, so do nothing
prunedNodes += 1;
continue;
}
//We have a trailer
else if (nodesWithLowestCost.ContainsKey(roundedChildHeading) && startTrailer != null)
{
//Have we expanded to this node before with this trailer heading
int roundedTrailerHeading = HelpStuff.RoundValue(child.TrailerHeadingInDegrees, headingResolutionTrailer);
if (lowestCostNodesTrailer[childCell.x, childCell.z].Contains(roundedTrailerHeading))
{
//If the open node has a large gCost then we need to update that node with data
//from the child node
if (costSoFar < nodesWithLowestCost[roundedChildHeading].gCost)
{
//If this child node is better then we should update the node in the open list
//which is faster than deleting the old node and adding this child node
Node existingNode = nodesWithLowestCost[roundedChildHeading];
child.StealDataFromThisNode(existingNode);
//Modify the heap-position of the node already in the open nodes
openNodes.UpdateItem(existingNode);
}
//If the open node has a smaller gCost, then we dont need this child node, so do nothing
prunedNodes += 1;
continue;
}
}
else
{
//Add the node to the cell with this angle
nodesWithLowestCost[roundedChildHeading] = child;
if (startTrailer != null)
{
int roundedTrailerHeading = HelpStuff.RoundValue(child.TrailerHeadingInDegrees, headingResolutionTrailer);
lowestCostNodesTrailer[childCell.x, childCell.z].Add(roundedTrailerHeading);
}
}
//Dont add the node if its colliding with an obstacle or is outside of map
timeBefore = Environment.TickCount;
if (ObstaclesDetection.HasCarInvalidPosition(child.rearWheelPos, child.heading, startCar.carData, map))
{
prunedNodes += 1;
continue;
}
timer_isCollidingWithObstacle += Environment.TickCount - timeBefore;
//Trailer obstacle calculations
int startTrailerTimer = Environment.TickCount;
if (startTrailer != null)
{
//Now we need to check if this new position is valid by checking for collision with obstacles and the drag vehicle
//To do that we need the rear wheel pos of the trailer with this heading
//We know where the trailer is attached to the drag vehicle
Vector3 trailerAttachmentPoint = startCar.carData.GetTrailerAttachmentPoint(child.rearWheelPos, child.HeadingInRadians);
//Now we need the trailer's rear-wheel pos based on the new heading
Vector3 trailerRearWheelPos = startTrailer.carData.GetTrailerRearWheelPos(trailerAttachmentPoint, child.TrailerHeadingInRadians);
//Obstacle detection
//With the environment
if (ObstaclesDetection.HasCarInvalidPosition(trailerRearWheelPos, child.TrailerHeadingInRadians, startTrailer.carData, map))
{
prunedNodes += 1;
//Debug.Log("Semi trailer environment collision");
continue;
}
//With the drag vehicle
if (ObstaclesDetection.IsTrailerCollidingWithDragVehicle(
child.rearWheelPos, child.HeadingInRadians, startCar.carData,
trailerRearWheelPos, child.TrailerHeadingInRadians, startTrailer.carData))
{
prunedNodes += 1;
//Debug.Log("Semi trailer collision");
continue;
}
}
timer_TrailerCollision += Environment.TickCount - startTrailerTimer;
timeBefore = Environment.TickCount;
openNodes.Add(child);
timer_addNodeToHeap += Environment.TickCount - timeBefore;
}
}
}
}
//Generate the final path when Hybrid A* has found the goal
List <Node> finalPath = GenerateFinalPath(finalNode);
//Display how long time everything took
string display = DisplayController.GetDisplayTimeText(timer_selectLowestCostNode, "Select lowest cost node");
display += DisplayController.GetDisplayTimeText(timer_addNodeToHeap, "Add new node to heap");
display += DisplayController.GetDisplayTimeText(timer_findChildren, "Find children");
display += DisplayController.GetDisplayTimeText(timer_isCollidingWithObstacle, "Is node colliding");
display += DisplayController.GetDisplayTimeText(timer_ReedsSheppNode, "Reeds-Shepp Node");
display += DisplayController.GetDisplayTimeText(timer_ReedsSheppHeuristics, "Reeds-Shepp Heuristics");
display += DisplayController.GetDisplayTimeText(timer_TrailerCollision, "Trailer collision");
display += DisplayController.GetDisplayText("Max nodes in heap", maxNodesInHeap, ". ");
display += DisplayController.GetDisplayText("Expanded nodes", allExpandedNodes.Count, ". ");
display += DisplayController.GetDisplayText("Pruned nodes", prunedNodes, null);
Debug.Log(display);
//Display car positions along the final path
DisplayController.DisplayVehicleAlongPath(finalPath, startCar.carData, startTrailer);
return(finalPath);
}
//To smooth the path we need to know which nodes are fixed and shouldnt be moved when smoothing to keep the shape of the path
private static List <bool> FindFixedNodes(List <Node> path, bool isCircular)
{
List <bool> isNodeFixed = new List <bool>();
//Step 1
//Find the points where we change direction from forward -> reverse or the opposite and fix them
for (int i = 0; i < path.Count; i++)
{
//Add the first and the last wp as fixed if the path is not circular
if ((i == 0 || i == path.Count - 1) && !isCircular)
{
isNodeFixed.Add(true);
}
//Add the wp where we are going from forward -> reverse or the opposite
else if (path[i].isReversing != path[HelpStuff.ClampListIndex(i + 1, path.Count)].isReversing)
{
isNodeFixed.Add(true);
}
//This node is not fixed
else
{
isNodeFixed.Add(false);
}
}
//Step 2
//Fix the waypoints before a change in direction of the car will not smoothly move to that waypoint
//This is not needed when if we have moved the position of the path to the front wheels
//Then this gives a worse result
//Need a temp list or all waypoints will be set to fixed when we loop though the list
//List<bool> isNodeFixedTemp = new List<bool>(isNodeFixed);
//for (int i = 1; i < isNodeFixed.Count - 1; i++)
//{
// //If the previous node is fixed or the upcoming node is fixed, then this node should be fixed
// if (isNodeFixed[i - 1] || isNodeFixed[i + 1])
// {
// isNodeFixedTemp[i] = true;
// }
//}
////Transfer the new true values from the temp list to the list we are actually using
//for (int i = 0; i < isNodeFixedTemp.Count; i++)
//{
// if (isNodeFixedTemp[i])
// {
// isNodeFixed[i] = true;
// }
//}
//But the second to last node should not be fixed, because the last node is often not at the correct position
//because Hybrid A* is not always finding the exact solution
//if (isNodeFixed.Count > 1)
//{
// isNodeFixed[isNodeFixed.Count - 2] = false;
//}
return(isNodeFixed);
}
//Smooth a path with constrained batch gradient descent (x = x - gamma * grad(x))
//https://www.youtube.com/watch?v=a3whnj4H4DQ
public static void ConstrainedGradientDescent(
List <Vector3> path,
List <bool> isNodeFixed,
Map map,
bool isCircular,
float gamma,
bool isDebugOn)
{
//The list with smooth coordinates
List <Vector3> smoothPath = new List <Vector3>();
//Add the old positions
for (int i = 0; i < path.Count; i++)
{
smoothPath.Add(path[i]);
}
//Debug.Log(smoothPath.Count);
//Stop smoothing when all points have moved a distance less than this distance
//If 0.00001 we need more than 1000 iterations
float tolerance = 0.001f;
//How far has all points together moved this iteration?
//We are comparing the distance sqr instead of magnitude which is faster
float totalChangeSqr = tolerance * tolerance;
//So we dont end up in an infinite loop, is generally < 100
int iterations = 0;
//If we find a nan value its super slow to print that we did each iteration, so we do it once in the end
bool hasFoundNan = false;
while (totalChangeSqr >= tolerance * tolerance)
{
if (iterations > 1000)
{
break;
}
iterations += 1;
totalChangeSqr = 0f;
//We are using surrounding values, so we need an array where we add values found during the iteration
List <Vector3> newValuesThisIteration = new List <Vector3>(smoothPath.Count);
for (int i = 0; i < path.Count; i++)
{
//Dont move nodes that are fixed
if (isNodeFixed[i])
{
newValuesThisIteration.Add(smoothPath[i]);
continue;
}
//Clamp when we reach end and beginning of list
//The first and last node should be set to fixed if the path is not set to circular
int i_plus_one = HelpStuff.ClampListIndex(i + 1, path.Count);
int i_minus_one = HelpStuff.ClampListIndex(i - 1, path.Count);
int i_plus_two = HelpStuff.ClampListIndex(i + 2, path.Count);
int i_minus_two = HelpStuff.ClampListIndex(i - 2, path.Count);
if (!isCircular)
{
if (i_plus_two == 0)
{
i_plus_two = path.Count - 1;
}
if (i_minus_two == path.Count - 1)
{
i_minus_two = 0;
}
}
//Smooth!
Vector3 newSmoothPos = smoothPath[i];
//1. Minimize the distance between this position and the surrounding positions
newSmoothPos += gamma * (smoothPath[i_plus_one] + smoothPath[i_minus_one] - 2f * smoothPath[i]);
//2. Right side
newSmoothPos += gamma * 0.5f * (2f * smoothPath[i_minus_one] - smoothPath[i_minus_two] - smoothPath[i]);
//3. Left side
newSmoothPos += gamma * 0.5f * (2f * smoothPath[i_plus_one] - smoothPath[i_plus_two] - smoothPath[i]);
//Sometimes the algorithm is unstable and shoots the vertices far away
//Maybe better to check for Nan in each part, so if the pos is NaN after optimizing to obstacle, dont add it???
if (float.IsNaN(newSmoothPos.x) || float.IsNaN(newSmoothPos.x) || float.IsNaN(newSmoothPos.x))
{
newSmoothPos = smoothPath[i];
hasFoundNan = true;
}
//Check if the new pos is within the map and if it is a value
if (map != null && !map.IsPosWithinGrid(newSmoothPos))
{
newSmoothPos = smoothPath[i];
}
//How far did we move the position this update?
totalChangeSqr += (newSmoothPos - smoothPath[i]).sqrMagnitude;
newValuesThisIteration.Add(newSmoothPos);
}
//Add the new values we created this iteration
for (int i = 0; i < smoothPath.Count; i++)
{
smoothPath[i] = newValuesThisIteration[i];
}
}
if (isDebugOn)
{
string debugText = DisplayController.GetDisplayText("Smooth path iterations", iterations, null);
Debug.Log(debugText);
}
if (hasFoundNan)
{
Debug.Log("Found Nan value");
}
//Add the new smooth positions to the original waypoints
for (int i = 0; i < path.Count; i++)
{
path[i] = smoothPath[i];
}
}
//Smooth a path with batch gradient descent (x = x - gamma * grad(x))
//https://www.youtube.com/watch?v=umAeJ7LMCfU
public static void GradientDescent(
List <Vector3> path,
List <bool> isNodeFixed,
List <Obstacle> obstacles,
Map map,
bool isCircular,
float alpha, float beta, float gamma, float delta,
bool isDebugOn)
{
//Using map.VoronoiField is slow in each iteration, so should cache
VoronoiFieldCell[,] voronoiField = null;
if (map != null && map.VoronoiField != null)
{
voronoiField = map.VoronoiField;
}
//The list with smooth coordinates
List <Vector3> smoothPath = new List <Vector3>();
//Add the old positions
for (int i = 0; i < path.Count; i++)
{
smoothPath.Add(path[i]);
}
//Stop smoothing when all points have moved a distance less than this distance
//If 0.00001 we need more than 1000 iterations
float tolerance = 0.001f;
//How far has all points together moved this iteration?
//We are comparing the distance sqr instead of magnitude which is faster
float totalChangeSqr = tolerance * tolerance;
//So we dont end up in an infinite loop, is generally < 100
int iterations = 0;
//If we find a nan value its super slow to print that we did each iteration, so we do it once in the end
bool hasFoundNan = false;
while (totalChangeSqr >= tolerance * tolerance)
{
if (iterations > 1000)
{
break;
}
iterations += 1;
totalChangeSqr = 0f;
//We are using surrounding values, so we need an array where we add values found during the iteration
List <Vector3> newValuesThisIteration = new List <Vector3>(smoothPath.Count);
for (int i = 0; i < path.Count; i++)
{
//Dont move nodes that are fixed
if (isNodeFixed[i])
{
newValuesThisIteration.Add(smoothPath[i]);
continue;
}
//Clamp when we reach end and beginning of list
//The first and last node should be set to fixed if the path is not set to circular
int i_plus_one = HelpStuff.ClampListIndex(i + 1, path.Count);
int i_minus_one = HelpStuff.ClampListIndex(i - 1, path.Count);
//Smooth!
//1. Minimize the distance between the smooth path and the original path
Vector3 newSmoothPos = smoothPath[i] + alpha * (path[i] - smoothPath[i]);
//2. Minimize the distance between this position and the surrounding positions
newSmoothPos += beta * (smoothPath[i_plus_one] + smoothPath[i_minus_one] - 2f * smoothPath[i]);
//3. Maximize the distance to the closest obstacle
//Is sometimes unstable because we use the closest obstacle, so is bouncing back and forth
//until the loop stops because of infinite check
//if (obstacles != null)
//{
// Vector3 closestObstaclePos = FindClosestObstaclePos(smoothPath[i], obstacles);
// Vector3 dirToObstacle = closestObstaclePos - smoothPath[i];
// //Ignore obstacles far away
// float maxDist = 10f;
// if (dirToObstacle.sqrMagnitude < maxDist * maxDist)
// {
// float distanceToObstacle = dirToObstacle.magnitude;
// //To make obstacles closer more important
// float scaler = 1f - (distanceToObstacle / maxDist);
// newSmoothPos -= gamma * dirToObstacle.normalized * scaler;
// }
//}
//We can also use the voronoi field to find the closest obstacle
if (voronoiField != null && gamma > 0f)
{
Vector3 nodePos = smoothPath[i];
//Get the data for this cell
IntVector2 cellPos = map.ConvertWorldToCell(nodePos);
//The data for this cell
VoronoiFieldCell v = voronoiField[cellPos.x, cellPos.z];
Vector3 closestObstaclePos = v.ClosestObstaclePos(nodePos, voronoiField);
if (closestObstaclePos.x != -1f)
{
Vector3 dirToObstacle = closestObstaclePos - nodePos;
//Ignore obstacles far away
float maxDist = 10f;
if (dirToObstacle.sqrMagnitude < maxDist * maxDist)
{
float distanceToObstacle = dirToObstacle.magnitude;
//To make obstacles closer more important
float scaler = 1f - (distanceToObstacle / maxDist);
newSmoothPos -= gamma * dirToObstacle.normalized * scaler;
}
}
}
//4. Use the Voronoi field to push vertices away from obstacles
//We need to find the derivative of the voronoi field function with respect to:
//- distance to obstacle edge - should be maximized
//- distance to voronoi edge - should be minimized
//...to optimize the distance to both edges
//This is kinda slow and useless since we are already pushing away the path from obstacles above?
if (voronoiField != null && delta > 0f)
{
Vector3 nodePos = smoothPath[i];
//Get the data for this cell
IntVector2 cellPos = map.ConvertWorldToCell(nodePos);
//The data for this cell
VoronoiFieldCell v = voronoiField[cellPos.x, cellPos.z];
//Same each iteration
float d_max = v.d_obs_max;
float a = v.alpha;
Vector3 closestObstaclePos = v.ClosestObstaclePos(nodePos, voronoiField);
Vector3 closestEdgePos = v.ClosestEdgePos(nodePos, voronoiField);
//If we are inside an obstacle when debugging, we wont have a closest
if (closestObstaclePos.x != -1f && closestEdgePos.x != -1f)
{
//The directions
Vector3 dirToObstacle = closestObstaclePos - nodePos;
Vector3 dirToEdge = closestEdgePos - nodePos;
//The distances
float d_obs = dirToObstacle.magnitude;
float d_edg = dirToEdge.magnitude;
//The distance to voronoi edge
float upper_edge = a * d_obs * (d_obs - d_max) * (d_obs - d_max);
float lower_edge = d_max * d_max * (d_obs + a) * (d_edg + d_obs) * (d_edg + d_obs);
newSmoothPos -= delta * (upper_edge / lower_edge) * (-dirToEdge / d_edg);
//The distance to voronoi obstacle
float upper_obs = a * d_edg * (d_obs - d_max) * ((d_edg + 2f * d_max + a) * d_obs + (d_max + 2f * a) + a * d_max);
float lower_obs = d_max * d_max * (d_obs + a) * (d_obs + a) * (d_obs + d_edg) * (d_obs + d_edg);
newSmoothPos += delta * (upper_obs / lower_obs) * (dirToObstacle / d_obs);
}
}
//Sometimes the algorithm is unstable and shoots the vertices far away
//Maybe better to check for Nan in each part, so if the pos is NaN after optimizing to obstacle, dont add it???
if (float.IsNaN(newSmoothPos.x) || float.IsNaN(newSmoothPos.x) || float.IsNaN(newSmoothPos.x))
{
newSmoothPos = smoothPath[i];
hasFoundNan = true;
}
//Check if the new pos is within the map and if it is a value
if (map != null && !map.IsPosWithinGrid(newSmoothPos))
{
newSmoothPos = smoothPath[i];
}
//How far did we move the position this update?
totalChangeSqr += (newSmoothPos - smoothPath[i]).sqrMagnitude;
newValuesThisIteration.Add(newSmoothPos);
}
//Add the new values we created this iteration
for (int i = 0; i < smoothPath.Count; i++)
{
smoothPath[i] = newValuesThisIteration[i];
}
}
if (isDebugOn)
{
string debugText = DisplayController.GetDisplayText("Smooth path iterations", iterations, null);
Debug.Log(debugText);
}
if (hasFoundNan)
{
Debug.Log("Found Nan value");
}
//Add the new smooth positions to the original waypoints
for (int i = 0; i < path.Count; i++)
{
path[i] = smoothPath[i];
}
}
