Trying to make A star algorithm implementation better

I am trying to write a simple bot that will find a path on the map from point A to point B and navigate along the established shortest path. I try to write the whole algorithm with my own, I know that there are such things as NavMeshSurface + NavMeshAgent, but I need to implement everything with my own.
For the implementation, I chose the A star pathfinding algorithm and implemented it. There was a problem with defining walls in the path, I solved this problem by creating a 2D mesh and checking the WallLayout. As a result, for now, the bot finds the shortest path to the point and moves there.
The problem is uneven surfaces. The whole algorithm and meshing works on a flat surface, if I add a surface with some kind of slope, for example, any upward rise, then I am not catching up on how to correctly plot the coordinates of the path.

Mesh script:

public class Grid : MonoBehaviour
{
    [SerializeField]
    private Transform StartPosition;
    [SerializeField]
    private LayerMask WallMask;
    [SerializeField]
    private Vector2 vGridWorldSize;
    [SerializeField]
    private float fNodeRadius;
    [SerializeField]
    private float fDistanceBetweenNodes;

    Node[,] NodeArray;
    public List<Node> FinalPath;

    float fNodeDiameter;
    int iGridSizeX, iGridSizeY;


    private void Start()
    {
        fNodeDiameter = fNodeRadius * 2;
        iGridSizeX = Mathf.RoundToInt(vGridWorldSize.x / fNodeDiameter);
        iGridSizeY = Mathf.RoundToInt(vGridWorldSize.y / fNodeDiameter);
        CreateGrid();
    }

    void CreateGrid()
    {
        NodeArray = new Node[iGridSizeX, iGridSizeY];
        Vector3 bottomLeft = transform.position - Vector3.right * vGridWorldSize.x / 2 - Vector3.forward * vGridWorldSize.y / 2;
        for (int x = 0; x < iGridSizeX; x++)
        {
            for (int y = 0; y < iGridSizeY; y++)
            {
                Vector3 worldPoint = bottomLeft + Vector3.right * (x * fNodeDiameter + fNodeRadius) + Vector3.forward * (y * fNodeDiameter + fNodeRadius);
                bool Wall = true;

                if (Physics.CheckSphere(worldPoint, fNodeRadius, WallMask))
                {
                    Wall = false;
                }
                NodeArray[x, y] = new Node(Wall, worldPoint, x, y);
            }
        }
    }



Since Physics.CheckSphere returns true if it hits something, shouldn’t you set Wall to true on line 42?

Also I don’t think a sphere is a good way to check for walls, if your characters are human-shaped (taller than they are wide). Physics.CheckCapsule would be better then. Otherwise you can get incorrect data on where the AI can walk. Suppose your character is 0.5m wide and 2m tall. If you check for a 2m wide sphere, the AI will think it can’t walk up some ramps right next to it.

The way that I did what you are trying to do was this:

  1. Get a 2D grid and lay it over the map (like a chess board)
  2. Raycast down from every grid point and get where the points hit the map
  3. Take the collision point and work out whether the bot could actually stand there
  4. Now look at all the neighbouring collisions and work out if the AI can walk out from a square to a neighbour collision (can I walk to the space next to me in the chess board?)
  5. Calculate the actual distances between points, because on a flat piece of ground it may be 1m between two cells. On a slope it may be >1m

And a general A* tip in case you didn’t know it: A lot of A* tutorials tell you to mark flags on the map or use “closed lists” to know where the pathfinding has looked. The problem is you have to reset the data after finding the path. What I do is stamp each node with a “version”. Each pathfinding search gets a new version. So you don’t have un-mark nodes, because you can use the version to see the node was marked by the previous pathfinding search instead of the current one.