Anyone have idea why this pathfinding algorithm isn't working?

[JaredBGreat]

If anyone can fix this pathfinding AI class, I don't mind anyone else using it (or "I hereby give permission to use" in legalese) so long as no one tries to stop me from using a fixed/working version (i.e., claims to own it).  The idea it to route from start to destination, check for obstacles, and if found route just past a corner and then own to destination, thus breaking the route into segments that wind around (non-connected) obstacles.  Unfortunately it doesn't work, it get suck early on for some (so entities freeze) while those that don't get stuck walk through objects.  Just a trade, code whether you can fix it or not, and thanks if you can give a suggestion that leads to my fixing it.

//Copyright (C) Jared Blackburn, Sep 27, 2013
package community.simulation.citizens.ai;

import community.map.*;
import community.simulation.citizens.Citizen;
import java.awt.geom.Line2D;
import java.util.ArrayList;
import static community.util.Util.*;

/**
 *
 * @author jared = JaredBGreat (Jared Blackburn)
 */
public class Router {
    static Citizen citizen;
    static int size, reached;
    static Location[] steps;
    static Location[] options;
    static ArrayList<MapLocale> obstacles;


////////////////////////////////////////////////////////////////////////////////
/*                                                                            */
/*                                  FIXME!!!                                  */
/*                                                                            */
////////////////////////////////////////////////////////////////////////////////


    public static Movement makeRoute(Location start, Location finish, Citizen traveler) {
        citizen = traveler;  
        steps = new Location[32];
        steps[0] = start;
        steps[1] = finish;
        options = new Location[2];
        obstacles = new ArrayList<MapLocale>(32);
        size = 2;
        reached = 1;
        
        while(reached < size) {
            
            System.out.println("Creating route for citizen #" + citizen.getId() +
                ", at setp " + reached + ", from X=" + steps[reached-1].getXint() +
                ", Z=" + steps[reached-1].getZint() + " to " + " X=" + steps[reached].getXint() + 
                ", Z=" + steps[reached].getZint() +", with " + size + " total steps " +
                " so far.");
            
            if(testRoute(steps[reached - 1], steps[reached])) {
                reached++;
            } else {
                size++;
                if(size >= steps.length) {
                    citizen.setStayTimer(600); // 30 seconds (remove when later)
                    return null;
                } // TODO: Return a Wandering
                for(int i = size - 1; i >= reached; i--) {
                    steps[i + 1] = steps[i];
                }
                circumvent(steps[reached - 1], findClosest(steps[reached - 1]));
                if((options[0] == null) && (options[1] == null)) {
                    citizen.setStayTimer(600); // 30 seconds (remove when later)
                    return null;
                } // TODO: Return a Wandering
                if (options[0] == null) {
                    steps[reached] = options[1];
                } else if(options[1] == null) {
                    steps[reached] = options[0];
                } else if((distance(steps[reached - 1], options[0]) + distance(options[0], steps[reached + 1]))
                    <= (distance(steps[reached - 1], options[1]) + distance(options[1], steps[reached + 1]))) {
                    steps[reached] = options[0];
                } else { // distance to optionsp[1] < distance to optionsp[0]
                    steps[reached] = options[1];
                }
            }
        }
    return new Route(citizen, steps, size);
    }


    private static boolean testRoute(Location begin, Location end) {
        obstacles.clear();
        Line2D.Float path = new Line2D.Float(begin.getXfloat(), begin.getZfloat(),
                                        end.getXfloat(), end.getZfloat());
           for(MapLocale obstacle: citizen.city.places) {
            if(path.intersects(obstacle.getFootprint()) 
                    && !obstacle.inside2d(citizen.getLocation())) obstacles.add(obstacle);
        }
        obstacles.remove(citizen.getHome());
        obstacles.remove(citizen.getLocale());
        obstacles.remove(citizen.getDestination());
        if(obstacles.isEmpty()) return true;
        else return false;
    }


    public static MapLocale findClosest(Location start) {
        int closest = 0;
        float shortest = CityMap.size;
        for(int i = 0; i < obstacles.size(); i++) {
            float newDist = distance(start, obstacles.get(i));
            if(newDist < shortest) {
                shortest = newDist;
                closest = i;
            }
        }
        return obstacles.get(closest);
    }


    public static int circumvent(Location start, MapLocale obstacle) {
        int midOrientation = orienter(start, obstacle);
        Location tmp;
        switch (midOrientation) {
            case 0:
                tmp = obstacle.getCorner(3);
                if(orienter(start, tmp) == 0) {
                        tmp.change(1.0f, 0.0f, -1.0f);
                        options[0] = tmp;
                    } else {
                        tmp = obstacle.getCorner(2);
                        tmp.change(-1.0f, 0.0f, -1.0f);
                        options[0] = tmp;
                }
                tmp = obstacle.getCorner(1);
                if(orienter(start, tmp) == 0) {
                        tmp.change(-1.0f, 0.0f, 1.0f);
                        options[1] = tmp;
                    } else {
                        tmp = obstacle.getCorner(2);
                        tmp.change(-1.0f, 0.0f, -1.0f);
                        options[1] = tmp;
                }
                return midOrientation;
            case 1:
                tmp = obstacle.getCorner(0);
                if(orienter(start, tmp) == 1) {
                        tmp.change(1.0f, 0.0f, 1.0f);
                        options[0] = tmp;
                    } else {
                        tmp = obstacle.getCorner(3);
                        tmp.change(1.0f, 0.0f, -1.0f);
                        options[0] = tmp;
                }
                tmp = obstacle.getCorner(2);
                if(orienter(start, tmp) == 1) {
                        tmp.change(-1.0f, 0.0f, -1.0f);
                        options[1] = tmp;
                    } else {
                        tmp = obstacle.getCorner(3);
                        tmp.change(1.0f, 0.0f, -1.0f);
                        options[1] = tmp;
                }
                return midOrientation;
            case 2:
                tmp = obstacle.getCorner(1);
                if(orienter(start, tmp) == 2) {
                        tmp.change(-1.0f, 0.0f, 1.0f);
                        options[0] = tmp;
                    } else {
                        tmp = obstacle.getCorner(0);
                        tmp.change(1.0f, 0.0f, 1.0f);
                        options[0] = tmp;
                }
                tmp = obstacle.getCorner(3);
                if(orienter(start, tmp) == 2) {
                        tmp.change(1.0f, 0.0f, -1.0f);
                        options[1] = tmp;
                    } else {
                        tmp = obstacle.getCorner(0);
                        tmp.change(1.0f, 0.0f, 1.0f);
                        options[1] = tmp;
                }
                return midOrientation;
            case 3:
                tmp = obstacle.getCorner(2);
                if(orienter(start, tmp) == 3) {
                        tmp.change(-1.0f, 0.0f, -1.0f);
                        options[0] = tmp;
                    } else {
                        tmp = obstacle.getCorner(1);
                        tmp.change(-1.0f, 0.0f, 1.0f);
                        options[0] = tmp;
                }
                tmp = obstacle.getCorner(0);
                if(orienter(start, tmp) == 3) {
                        tmp.change(1.0f, 0.0f, 1.0f);
                        options[1] = tmp;
                    } else {
                        tmp = obstacle.getCorner(1);
                        tmp.change(-1.0f, 0.0f, 1.0f);
                        options[1] = tmp;
                }
                return midOrientation;
            default:
                community.util.HouseKeeping.reportError("Orienter gave invalid "
                        + " result to circumvent in router.", 1, true);
                return midOrientation;
        }
    }



    //////////////////////////////////////
    /*         GETTERS & SETTERS        */
    //////////////////////////////////////

     
    public Citizen getCitizen() {
        return citizen;
    }
    

    public ArrayList<MapLocale> getObstacles() {
        return obstacles;
    }
    

    public Location[] getOptions() {
        return options;
    }
    

    public int getReached() {
        return reached;
    }
    

    public int getSize() {
        return size;
    }    
    

    public Location[] getSteps() {
        return steps;
    }
}

This and an earlier attempt are here:
http://www.mediafire.com/folder/pc7auof601yen/Java