I've got a little problem with an A* algorithm that I need to constrict a little bit.

Basically : I use an A* to find the shortest path between 2 randomly placed room in 3D space, and then build a corridor between them. The problem I found is that sometimes it makes chimney like corridors that are not ideal, so I constrict the A* so that if the last movement was up or down, you go sideways.

Everything is fine, but in some corner cases, it fails to find a path (when there is obviously one).

Like here between the blue and red dot :

(i'm in unity btw, but i don't think it matters)

Here is the code of the actual A* (a bit long, and some redundency)

while(current != goal)
    {
        //add stair up / stair down
        foreach(Node<GridUnit> test in current.Neighbors)
        {
            if(!test.Data.empty && test != goal) continue;
            //bug at arrival;
            if(test == goal && penul !=null)
            {
                Vector3 currentDiff = current.Data.bounds.center - test.Data.bounds.center;
                if(!Mathf.Approximately(currentDiff.y,0))
                {
                    //wanna drop on the last
                    if(!coplanar(test.Data.bounds.center,current.Data.bounds.center,current.Data.parentUnit.bounds.center,to.Data.bounds.center))
                    {
                        continue;
                    }
                    else
                    {
                        if(Mathf.Approximately(to.Data.bounds.center.x, current.Data.parentUnit.bounds.center.x) &&
                           Mathf.Approximately(to.Data.bounds.center.z, current.Data.parentUnit.bounds.center.z))
                        {
                            continue;
                        }
                    }
                }
            }
            if(current.Data.parentUnit != null)
            {
                Vector3 previousDiff = current.Data.parentUnit.bounds.center - current.Data.bounds.center;
                Vector3 currentDiff = current.Data.bounds.center - test.Data.bounds.center;

                if(!Mathf.Approximately(previousDiff.y,0))
                {
                    if(!Mathf.Approximately(currentDiff.y,0))
                    {
                        //you wanna drop now :
                        continue;
                    }
                    if(current.Data.parentUnit.parentUnit != null)
                    {
                        if(!coplanar(test.Data.bounds.center,current.Data.bounds.center,current.Data.parentUnit.bounds.center,current.Data.parentUnit.parentUnit.bounds.center))
                        {
                            continue;
                        }else
                        {
                            if(Mathf.Approximately(test.Data.bounds.center.x, current.Data.parentUnit.parentUnit.bounds.center.x) &&
                               Mathf.Approximately(test.Data.bounds.center.z, current.Data.parentUnit.parentUnit.bounds.center.z))
                            {
                                continue;
                            }
                        }
                    }
                }

            }
            g = current.Data.g + HEURISTIC(current.Data,test.Data);
            h = HEURISTIC(test.Data,goal.Data);
            f = g + h;
            if(open.Contains(test) || closed.Contains(test))
            {
                if(test.Data.f > f)
                {
                    //found a shorter path going passing through that point
                    test.Data.f = f;
                    test.Data.g = g;
                    test.Data.h = h;
                    test.Data.parentUnit = current.Data;
                }
            }
            else
            {
                //jamais rencontré
                test.Data.f = f;
                test.Data.h = h;
                test.Data.g = g;
                test.Data.parentUnit = current.Data;
                open.Add(test);

            }
        }
        closed.Add (current);
        if(open.Count == 0)
        {
            Debug.Log("nothingfound");
            //nothing more to test no path found, stay to from;
            List<GridUnit> r = new List<GridUnit>();
            r.Add(from.Data);
            return r;
        }

        //sort open from small to biggest travel cost
        open.Sort(delegate(Node<GridUnit> x, Node<GridUnit> y) {
            return (int)(x.Data.f-y.Data.f);
        });
        //get the smallest travel cost node;
        Node<GridUnit> smallest = open[0];
        current = smallest;
        open.RemoveAt(0);
    }

    //build the path going backward;
    List<GridUnit> ret = new List<GridUnit>();

    if(penul != null)
    {
        ret.Insert(0,to.Data);
    }
    GridUnit cur = goal.Data;
    ret.Insert(0,cur);

    do{
        cur = cur.parentUnit;
        ret.Insert(0,cur);
    } while(cur != from.Data);


    return ret;

You see at the start of the foreach i constrict the A* like i said.

If you have any insight it would be cool.

Thanks