- Home /

# I'm trying to convert a 2d a* pathfinding algorithm to 3d but I've run into numerous problems[go to bold area to see main problem]

After a lot of debugging here are my problems that I've found thus far.

note: my script is based on https://www.youtube.com/watch?v=mZfyt03LDH4&t=748s

The neighbour nodes seem to be around the empty pathfinding object with my grid and pathfinding script instead of the object's i've assigned to be the player/enemy node

pathfinding's starting point is way off from the seeker and target ![alt text][1]

3.The math for finding the distance between the player's position and the enemy position is most likely wrong(Or could stem from problem 1)

## here are my 3 scripts for path finding(Look below scripts for areas I think are problems)

PathfindingScript

```
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
public class Pathfinding : MonoBehaviour
{
public Transform seeker, target;
Grid grid;
void Awake()
{
grid = GetComponent<Grid>();
}
void Update()
{
FindPath(seeker.position, target.position);
}
void FindPath(Vector3 StartPos, Vector3 TargetPos)
{
Node StartNode = grid.NodeFromWorldPoint(StartPos);
Node TargetNode = grid.NodeFromWorldPoint(TargetPos);
List<Node> openSet = new List<Node>();
HashSet<Node> closedSet = new HashSet<Node>();
openSet.Add(StartNode);
while(openSet.Count > 0)
{
Node currentNode = openSet[0];
for(int i = 1; i < openSet.Count; i++)
{
if(openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost)
{
currentNode = openSet[i];
}
}
openSet.Remove(currentNode);
closedSet.Add(currentNode);
if(currentNode == TargetNode)
{
//Debug.Log("this is getting called");
RetracePath(StartNode, TargetNode);
return;
}
foreach(Node neighbour in grid.GetNeighbours(currentNode))
{
if(!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if(newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, TargetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
}
void RetracePath(Node StartNode, Node EndNode)
{
List<Node> path = new List<Node>();
Node currentNode = EndNode;
while(currentNode != StartNode)
{
path.Add(currentNode);
currentNode = currentNode.parent;
}
path.Reverse();
grid.path = path;
}
int GetDistance(Node nodeA, Node nodeB)
{
int distX = Mathf.Abs(nodeA.GridX - nodeB.GridX);
int distY = Mathf.Abs(nodeA.GridY - nodeB.GridY);
int distZ = Mathf.Abs(nodeA.GridZ - nodeB.GridZ);
if(distX > distY && distX > distZ)
{
// Debug.Log("1");
return 14 * distY + 10 * (distX - distY);
}
else if(distY > distX && distY > distZ)
{
// Debug.Log("2");
return 14 * distX + 10 * (distY - distX);
}
else if(distZ > distX && distZ < distY)
{
// Debug.Log("3");
return 17 * distZ + 10 * (distZ - distX);
}
// Debug.Log(distX);
//Debug.Log(distY);
//Debug.Log(distZ);
return 17 * distZ + 10 * (distZ - distY);
}
}
```

Grid Script(creats the nodes that pathfinding uses)

```
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
public class Grid : MonoBehaviour
{
public GameObject enemy;
public LayerMask unwalkableMask;
public Vector3 gridWorldSize;
public float nodeRadius;
Node[,,] grid;
public List<Node> temp;
float nodeDiameter;
int gridSizeX, gridSizeY, gridSizeZ;
void Start()
{
temp = new List<Node>();
nodeDiameter = nodeRadius * 2;
gridSizeX = Mathf.RoundToInt(gridWorldSize.x / nodeDiameter);
gridSizeY = Mathf.RoundToInt(gridWorldSize.y / nodeDiameter);
gridSizeZ = Mathf.RoundToInt(gridWorldSize.z / nodeDiameter);
CreateGrid();
}
void Update()
{
CreateGrid();
}
void CreateGrid()
{
grid = new Node[gridSizeX, gridSizeY, gridSizeZ];
Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2 - Vector3.forward * gridWorldSize.y / 2 - Vector3.up * gridWorldSize.z / 2;
for (int x = 0; x < gridSizeX; x++)
{
for (int y = 0; y < gridSizeY; y++)
{
for (int z = 0; z < gridSizeZ; z++)
{
Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.forward * (y * nodeDiameter + nodeRadius) + Vector3.up * (z * nodeDiameter + nodeRadius);
bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius - .1f, unwalkableMask));
grid[x, y, z] = new Node(walkable, worldPoint, x, y, z);
}
}
}
}
public List<Node> GetNeighbours(Node node)
{
List<Node> neighbours = new List<Node>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
for (int z = -1; z <= 1; z++)
{
if (x == 0 && y == 0 && z == 0)
{
continue;
}
int checkX = node.GridX + x;
int checkY = node.GridY + y;
int checkZ = node.GridZ + z;
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY && checkZ >= 0 && checkZ < gridSizeZ)
{
neighbours.Add(grid[checkX, checkY, checkZ]);
}
}
}
}
return neighbours;
}
public Node NodeFromWorldPoint(Vector3 worldPosition)
{
float percentX = (worldPosition.x + gridWorldSize.x / 2) / gridWorldSize.x;
float percentY = (worldPosition.z + gridWorldSize.z / 2) / gridWorldSize.z;
float percentZ = (worldPosition.y + gridWorldSize.y / 2) / gridWorldSize.y;
percentX = Mathf.Clamp01(percentX);
percentY = Mathf.Clamp01(percentY);
percentZ = Mathf.Clamp01(percentZ);
//Debug.Log(percentX + " " + percentY + " " + percentZ);
int x = Mathf.RoundToInt((gridSizeX - 1) * percentX);
int y = Mathf.RoundToInt((gridSizeY - 1) * percentY);
int z = Mathf.RoundToInt((gridSizeZ - 1) * percentZ);
// Debug.Log(x + " " + y + " " + z);
return grid[x, y, z];
}
public List<Node> path;
public Node currentNode;
void OnDrawGizmos()
{
Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, gridWorldSize.z, gridWorldSize.y));
//Node Startnode = NodeFromWorldPoint(enemy.transform.position);
// Debug.Log(Startnode.worldPosition);
// temp = GetNeighbours(Startnode);
if (grid != null)
{
foreach (Node n in grid)
{
//Gizmos.color = (n.walkable) ? Color.white : Color.red;
if (path != null)
{
if (path.Contains(n))
{
Gizmos.color = Color.black;
Gizmos.DrawCube(n.worldPosition, Vector3.one * (nodeDiameter - .1f));
}
}
/* foreach(Node neighbour in temp)
{
Gizmos.color = Color.black;
Gizmos.DrawCube(neighbour.worldPosition, Vector3.one * (nodeDiameter - .1f));
}*/
//}
}
}
}
}
```

Node(Basis for nodes)

```
using UnityEngine;
using System.Collections;
public class Node
{
public bool walkable;
public Vector3 worldPosition;
public int GridX;
public int GridY;
public int GridZ;
public int gCost;
public int hCost;
public Node parent;
public Node(bool _walkable, Vector3 _worldPos, int _GridX, int _GridY, int _GridZ)
{
walkable = _walkable;
worldPosition = _worldPos;
GridX = _GridX;
GridY = _GridY;
GridZ = _GridZ;
}
public int fCost
{
get
{
return gCost + hCost;
}
}
}
```

## Areas that I think are problems

for problem 1. , I keep getting worldposition to be the same as the empty gameobject with the pathfinding and grid script's position and not the player/enemy thats supposed to be pathfinding

my modified function(THIS IS THE PROBLEM AREA)

```
public Node NodeFromWorldPoint(Vector3 worldPosition)
{
float percentX = (worldPosition.x + gridWorldSize.x / 2) / gridWorldSize.x;
float percentY = (worldPosition.z + gridWorldSize.z / 2) / gridWorldSize.z;
float percentZ = (worldPosition.y + gridWorldSize.y / 2) / gridWorldSize.y;
percentX = Mathf.Clamp01(percentX);
percentY = Mathf.Clamp01(percentY);
percentZ = Mathf.Clamp01(percentZ);
//Debug.Log(percentX + " " + percentY + " " + percentZ);
int x = Mathf.RoundToInt((gridSizeX - 1) * percentX);
int y = Mathf.RoundToInt((gridSizeY - 1) * percentY);
int z = Mathf.RoundToInt((gridSizeZ - 1) * percentZ);
// Debug.Log(x + " " + y + " " + z);
return grid[x, y, z];
}
```

the unmodified function

```
public Node NodeFromWorldPoint(Vector3 worldPosition) {
float percentX = (worldPosition.x + gridWorldSize.x/2) / gridWorldSize.x;
float percentY = (worldPosition.z + gridWorldSize.y/2) / gridWorldSize.y;
percentX = Mathf.Clamp01(percentX);
percentY = Mathf.Clamp01(percentY);
int x = Mathf.RoundToInt((gridSizeX-1) * percentX);
int y = Mathf.RoundToInt((gridSizeY-1) * percentY);
return grid[x,y];
}
```

This is a little off topic but I think the first thing you have to ask yourself is why are you attempting to write pathfinding algorithms?

If this is a purely I just think it's a fun learning experience fine. But if your actually making this to actually use it, your kind of getting caught in the weeds here and just need to use a built in unity pathfinding or a free version on the asset store.

Pathfinding is a lot of rather complicated math and frankly, it's like writing a shader or writing code to draw objects etc. You can totally write code to draw objects, but Unity already draws objects for you, shaders for most types of materials already exist etc.

Your almost always better off not re-inventing the wheel, especially when that wheel is like a custom F1 vehicle wheel.

I just have seen several people on here who kind of get stuck in the weeds and don't seem to realize they probably got in this to make video games not math equations, if you like the math equations great, i'm not saying it's not a great legitimate thing to do, but if your passion is making a game, don't waste 100 hours writing code you could download online to do tedious functions. It's already been solved for you.

It's important you understand how pathfinding works, when to use it, how to kind of use it correctly, but debugging your own homebrewed path finding code is seldom a great use of time unless your intent is i'm passionate about just wanting to learn this for myself, you have a VERY unique game that requires super custom pathfinding for some reason, maybe you for example have newtonian physics or something and want your A.I.'s use of pathfinding to reflect that etc, but in general you should get to the business of making a game and not a path finder.

ah, I'm doing it the hard way because I want to learn these skills for my self so I can do even more complicated things in the future. I also have no money to spend on assets so i can't rely on those either. Also its fun! but, I'm a little stuck on the math and have been for awhile and I just need some help with that.

**Answer** by Bunny83
·
Jan 03, 2018 at 12:51 PM

Yes, your distance algorithm doesn't make much sense. Actually for the heuristic the estimated remaining distance doesn't need to be exact. The only important property is that the heuristic is monoton and the distance must not be overestimated. So the estimate should never be larger than the actual cost. That's why most A* algorithms simply use the euclidean distance to the target which is always the shortest distance and will never be larger than the actual solution / cost.

Though if you prefer using a heuristic that is closer to the actual cost you have to approach it different for 3d. In 2d you only have two ways you can move: laterally or diagonally. You first try to move diagonally (since it's more efficient) and move the rest laterally. In 3d you have 3 different ways you can move: laterally, diagonally in a plane, diagonally in a cube. You first want to move along a cube diagonal until the smallest distance is eleminated. Then the remaining distance is like the 2d version where you first move diagonally in the remaining plane and finally laterally. Though the required cases gets quite long if you implement it with an ordinary if else chain. It's easier to follow if you break it down into those 3 steps: cubic diagonal, plane diagonal, lateral movement:

```
int GetDistance(Node nodeA, Node nodeB)
{
int dX = Mathf.Abs(nodeA.GridX - nodeB.GridX);
int dY = Mathf.Abs(nodeA.GridY - nodeB.GridY);
int dZ = Mathf.Abs(nodeA.GridZ - nodeB.GridZ);
int cost = 0;
// cube diagonal
if (dX < dY && dX < dZ)
{
cost += dX * 17;
dY -= dX;
dX = dZ - dX;
}
else if (dY < dZ)
{
cost += dY * 17;
dX -= dY;
dY = dZ - dY;
}
else
{
cost += dZ * 17;
dX -= dZ;
dY -= dZ;
}
// remaining distance 2d in dX and dY
if (dX < dY)
{
cost += dX * 14;
dX = dY - dX;
}
else
{
cost += dY * 14;
dX -= dY;
}
// remaining lateral distance in dX
return cost + dX * 10;
}
```

Ahh, it looks like it works. Now the only problem I seem to have now is that the path stems from a point that isn't anywhere near where I designated the seeker and target to be

The red object is the seeker and the white object is the target

**Answer** by JusSumGuy
·
Dec 31, 2017 at 04:47 AM

I think the problem is that your 3d coordinates don't match up with the 2d coordinates. It's similar to when your using a ray cast to select units in a 3d space from your monitor which is 2d. There's a math formula to deal with this problem.

care to share where I can find this math formula? Also where don't they match because i have alot of cordinates

**Answer** by dragonking300
·
Jan 07, 2018 at 03:57 AM

OK, I learned the pathfinding was way off because it added the position of the empty gameobject I attached my pathfinding script to... However, bunny helped fix my math so I'm accepting his awnser

### Your answer

### Welcome to Unity Answers

The best place to ask and answer questions about development with Unity.

To help users navigate the site we have posted a site navigation guide.

If you are a new user to Unity Answers, check out our FAQ for more information.

Make sure to check out our Knowledge Base for commonly asked Unity questions.

If you are a moderator, see our Moderator Guidelines page.

We are making improvements to UA, see the list of changes.

### Follow this Question

### Related Questions

Collision detection script fails sometimes. 0 Answers

A* pathfinding for 2D top Down 0 Answers

A* pathfinding for dynamic obstacles and player made blockages? 0 Answers

RTS dynamic formation width and length 0 Answers

Make a flying enemy ? 0 Answers