I am making a program where you can click on a map to see a "close-up view" of the area around it, such as on Google Maps.
When a user clicks on the map, it gets the X and Y coordinate of where they clicked.
Let's assume that I have an array of booleans of where these close-up view pictures are:
public static boolean[][] view_set=new boolean[Map.width][Map.height];
//The array of where pictures are. The map has a width of 3313, and a height of 3329.
The program searches through a folder, where images are named to where the X and Y coordinate of where it was taken on the map. The folder contains the following images (and more, but I'll only list five):
2377,1881.jpg, 2384,1980.jpg, 2389,1923.jpg, 2425,1860.jpg, 2475,1900.jpg
This means that:
view_set[2377][1881]=true;
view_set[2384][1980]=true;
view_set[2389][1923]=true;
view_set[2425][1860]=true;
view_set[2475][1900]=true;
If a user clicks at the X and Y of, for example, 2377,1882, then I need the program to figure out which image is closest (the answer in this case would be 2377,1881).
Any help would be appreciated,
Thanks.
Your boolean[][] is not a good datastructure for this problem, at least if it is not really dense (e.g. normally a point with close-up view is available in the surrounding 3×3 or maybe 5×5 square).
You want a 2-D-map with nearest-neighbor search. A useful data structure for this goal is the QuadTree. This is a tree of degree 4, used to represent spatial data. (I'm describing here the "Region QuadTree with point data".)
Basically, it divides a rectangle in four about equal size rectangles, and subdivides each of the rectangles further if there is more than one point in it.
So a node in your tree is one of these:
a empty leaf node (corresponding to a rectangle without points in it)
a leaf node containing exactly one point (corresponding to a rectangle with one point in it)
a inner node with four child nodes (corresponding to a rectangle with more than one point in it)
(In implementations, we can replace empty leaf nodes with a null-pointer in its parent.)
To find a point (or "the node a point would be in"), we start at the root node, look if our point is north/south/east/west of the dividing point, and go to the corresponding child node. We continue this until we arrive at some leaf node.
For adding a new point, we either wind up with an empty node - then we can put the new point here. If we end up at a node with already a point in it, create four child nodes (by splitting the rectangle) and add both points to the appropriate child node. (This might be the same, then repeat recursively.)
For the nearest-neighbor search, we will either wind up with an empty node - then we back up one level, and look at the other child nodes of this parent (comparing each distance). If we reach a child node with one point in it, we measure the distance of our search point to this point. If it is smaller than the distance to the edges or the node, we are done. Otherwise we will have to look at the points in the neighboring nodes, too, and compare the results here, taking the minimum. (We will have to look at at most four points, I think.)
For removal, after finding a point, we make its node empty. If the parent node now contains only one point, we replace it by a one-point leaf node.
The search and adding/removing are in O(depth) time complexity, where the maximum depth is limited by log((map length+width)/minimal distance of two points in your structure), and average depth is depending on the distribution of the points (e.g. the average distance to the next point), more or less.
Space needed is depending on number of points and average depth of the tree.
There are some variants of this data structure (for example splitting a node only when there are more than X points in it, or splitting not necessarily in the middle), to optimize the space usage and avoid too large depths of the tree.
Given the location the user clicked, you could search for the nearest image using a Dijkstra search.
Basically you start searching in increasingly larger rectangles around the clicked location for images. Of course you only have to search the boundaries of these rectangles, since you've already searched the body. This algorithm should stop as soon as an image is found.
Pseudo code:
int size = 0
Point result = default
while(result == default)
result = searchRectangleBoundary(size++, pointClicked)
function Point searchRectangleBoundary(int size, Point centre)
{
point p = {centre.X - size, centre.Y - size}
for i in 0 to and including size
{
if(view_set[p.X + i][p.Y]) return { p.X + i, p.Y}
if(view_set[p.X][p.Y + i]) return { p.X, p.Y + i}
if(view_set[p.X + i][p.Y + size]) return { p.X + i, p.Y + size}
if(view_set[p.X + size][p.Y + i]) return { p.X + size, p.Y + i}
}
return default
}
Do note that I've left out range checking for brevity.
There is a slight problem, but depending on the application, it might not be a problem. It doesn't use euclidian distances, but the manhattan metric. So it doesn't necessarily find the closest image, but an image at most the square root of 2 times as far.
Based on
your comment that states you have 350-500 points of interest,
your question that states you have a map width of 3313, and a height of 3329
my calculator which tells me that that represents ~11 million boolean values
...you're going about this the wrong way. #JBSnorro's answer is quite an elegant way of finding the needle (350 points) in the haystack (11 million points), but really, why create the haystack in the first place?
As per my comment on your question, why not just use a Pair<Integer,Integer> class to represent co-ordinates, store them in a set, and scan them? It's simpler, quicker, less memory consuming, and is way more scalable for larger maps (assuming the points of interest are sparse... which it seems is a sensible assumption given that they're points of interest).
..trust me, computing the Euclidean distance ~425 times beats wandering around an 11 million value boolean[][] looking for the 1 value in 25,950 that's of interest (esp. in a worst case analysis).
If you're really not thrilled with the idea of scanning ~425 values each time, then (i) you're more OCD than me (:P); (ii) you should check out nearest neighbour search algorithms.
I do not know if you are asking for this. If the user point is P1 {x1, y1} and you want to calculate its distance to P2 {x2,y2}, the distance is calculated using Pythagoras'Theorem
distance^2 = (x2-x1)^2 + (y2-y1)^2
If you only want to know the closest, you can avoid calculating the square root (the smaller the distance, the smaller the square too so it serves you the same).
Related
I have a rectangle Object with x, y, width and height. I have a list of these rectangles which are displayed on a screen. It is guaranteed that none of them overlap. Given a user's click position (x and y coordinates), I want to see which of these rectangles were clicked (since they do not overlap, there is a maximum of one rect that can be clicked).
I can obviously look through all of them and check for each one if the user clicked it but this is very slow because there are many on the screen. I can use some kind of comparison to keep the rectangles sorted when I insert a new one into the list. Is there some way to use something similar to binary search in order to decrease the time it takes to find which rect was clicked?
Note: the rectangles can be any size.
Thanks:)
Edit: To get an idea of what I am making visit koalastothemax.com
It highly depends upon your application and details we're not quite aware of yet for what the best solution would be. BUT, with as little as I know, I'd say you can make a 2D array that points to your rectangles. That 2D array would map directly to the pixels on the screen. So if you make the array 10x20, then the coordinate x divided by screen width times 10 (casted to int) will be the first index and y divided screen height times 20 would be your y index. With your x and y index, you can map directly to the rectangle that it points to. Some indexes might be empty and some might point to more than one rectangle if they're not perfectly laid out, but that seems the easiest way to me without knowing much about the application.
I have tackled a very similar problem in the past when developing a simulation. In my case the coordinates were doubles (so no integer indexing was possible) and there could be hundreds of millions of them that needed to be searched.
My solution was to create an Axis class to represent each axis as a sequence of ranges. The ranges were guaranteed to go from a minimum to a maximum and the class was smart enough to split itself into pieces when new ranges were added. Each range has a single generic object stored. The class used a binary search to find a range quickly.
So roughly the class looks like:
class Axis<T> {
public Axis(double min, double max, Supplier<T> creator);
public Stream<T> add(double from, double to);
public T get(double coord);
}
The add method needs to return a stream because the added range may cover several ranges.
To store rectanges:
Axis<Axis<Rectangle>> rectanges = new Axis<>(0.0, 100.0,
() -> new Axis<>(0.0, 100.0, Rectangle::new));
rectangles.add(x, x + w).forEach(r -> r.add(y, y + h).forEach(Rectangle::setPresent));
And to find a rectangle:
rectangles.get(x).get(y);
Note that there's always an object stored so you need a representation such as Rectangle.NULL for 'not present'. Or you could make it Optional<Rectangle> (though that indirection eats a lot of memory and processing for large numbers of rectangles).
I've just given the high level design here rather than any implementation details so let me know if you want more info on how to make it work. Getting the logic right on the range splits is not trivial. But I can guarantee that it's very fast even with very large numbers of rectangles.
The fastest way I can come up with is definitely not the most memory efficient. This works by exploiting the fact that an amortized hash table has constant lookup time. It will map every point that a rectangle has to that rectangle. This is only really effective if your are using integers. You might be able to get it to work with floats if you use a bit of rounding.
Make sure that the Point class has a hash code and equals function.
public class PointCheck
{
public Map<Point, Rect> pointMap;
public PointCheck()
{
pointMap = new HashMap<>();
}
/**
* Map all points that contain the rectangle
* to the rectangle.
*/
public void addRect(Rect rect)
{
for(int i = rect.x; i < rect.x + rect.width; ++i)
{
for(int j = rect.y; j < rect.y + rect.height; ++i)
{
pointMap.put(new Point(i, j), rect);
}
}
}
/**
* Returns the rectangle clicked, null
* if there is no rectangle.
*/
public Rect checkClick(Point click)
{
return pointMap.get(click);
}
}
Edit:
Just thought I should mention this: All of the rectangles held in the value of the hash map are references to the original rectangle, they are not clones.
This question covers a software algorithm, from On topic
I am working on an interview question from Amazon Software Question,
specifically "Given a set of points (x,y) and an integer "n", return n number of points which are close to the origin"
Here is the sample high level psuedocode answer to this question, from Sample Answer
Step 1: Design a class called point which has three fields - int x, int y, int distance
Step 2: For all the points given, find the distance between them and origin
Step 3: Store the values in a binary tree
Step 4: Heap sort
Step 5: print the first n values from the binary tree
I agree with steps 1 and 2 because it makes sense in terms of object-oriented design to have one software bundle of data, Point, encapsulate away the fields of x, y and distance.Ensapsulation
Can someone explain the design decisions from 3 to 5?
Here's how I would do steps of 3 to 5
Step 3: Store all the points in an array
Step 4: Sort the array with respect to distance(I use some build in sort here like Arrays.Sort
Step 5: With the array sorted in ascending order, I print off the first n values
Why the author of that response use a more complicated data structure, binary tree and not something simpler like an array that I used? I know what a binary tree is - hierarchical data structure of nodes with two pointers. In his algorithm, would you have to use a BST?
First, I would not say that having Point(x, y, distance) is good design or encapsulation. distance is not really part of a point, it can be computed from x and y. In term of design, I would certainly have a function, i.e. a static method from Point or an helper class Points.
double distance(Point a, Point b)
Then for the specific question, I actually agree with your solution, to put the data in an array, sort this array and then extract the N first.
What the example may be hinted at is that the heapsort actually often uses a binary tree structure inside the array to be sorted as explained here :
The heap is often placed in an array with the layout of a complete binary tree.
Of course, if the distance to the origin is not stored in the Point, for performance reason, it had to be put with the corresponding Point object in the array, or any information that will allow to get the Point object from the sorted distance (reference, index), e.g.
List<Pair<Long, Point>> distancesToOrigin = new ArrayList<>();
to be sorted with a Comparator<Pair<Long, Point>>
It is not necessary to use BST. However, it is a good practice to use BST when needing a structure that is self-sorted. I do not see the need to both use BST and heapsort it (somehow). You could use just BST and retrieve the first n points. You could also use an array, sort it and use the first n points.
If you want to sort an array of type Point, you could implement the interface Comparable (Point would imolement that interface) and overload the default method.
You never have to choose any data structures, but by determining the needs you have, you would also easily determine the optimum structure.
The approach described in this post is more complex than needed for such a question. As you noted, simple sorting by distance will suffice. However, to help explain your confusion about what your sample answer author was trying to get at, maybe consider the k nearest neighbors problem which can be solved with a k-d tree, a structure that applies space partitioning to the k-d dataset. For 2-dimensional space, that is indeed a binary tree. This tree is inherently sorted and doesn't need any "heap sorting."
It should be noted that building the k-d tree will take O(n log n), and is only worth the cost if you need to do repeated nearest neighbor searches on the structure. If you only need to perform one search to find k nearest neighbors from the origin, it can be done with a naive O(n) search.
How to build a k-d tree, straight from Wiki:
One adds a new point to a k-d tree in the same way as one adds an element to any other search tree. First, traverse the tree, starting from the root and moving to either the left or the right child depending on whether the point to be inserted is on the "left" or "right" side of the splitting plane. Once you get to the node under which the child should be located, add the new point as either the left or right child of the leaf node, again depending on which side of the node's splitting plane contains the new node.
Adding points in this manner can cause the tree to become unbalanced, leading to decreased tree performance. The rate of tree performance degradation is dependent upon the spatial distribution of tree points being added, and the number of points added in relation to the tree size. If a tree becomes too unbalanced, it may need to be re-balanced to restore the performance of queries that rely on the tree balancing, such as nearest neighbour searching.
Once have have built the tree, you can find k nearest neighbors to some point (the origin in your case) in O(k log n) time.
Straight from Wiki:
Searching for a nearest neighbour in a k-d tree proceeds as follows:
Starting with the root node, the algorithm moves down the tree recursively, in the same way that it would if the search point were being inserted (i.e. it goes left or right depending on whether the point is lesser than or greater than the current node in the split dimension).
Once the algorithm reaches a leaf node, it saves that node point as the "current best"
The algorithm unwinds the recursion of the tree, performing the following steps at each node:
If the current node is closer than the current best, then it becomes the current best.
The algorithm checks whether there could be any points on the other side of the splitting plane that are closer to the search point than the current best. In concept, this is done by intersecting the splitting hyperplane with a hypersphere around the search point that has a radius equal to the current nearest distance. Since the hyperplanes are all axis-aligned this is implemented as a simple comparison to see whether the difference between the splitting coordinate of the search point and current node is lesser than the distance (overall coordinates) from the search point to the current best.
If the hypersphere crosses the plane, there could be nearer points on the other side of the plane, so the algorithm must move down the other branch of the tree from the current node looking for closer points, following the same recursive process as the entire search.
If the hypersphere doesn't intersect the splitting plane, then the algorithm continues walking up the tree, and the entire branch on the other side of that node is eliminated.
When the algorithm finishes this process for the root node, then the search is complete.
This is a pretty tricky algorithm that I would hate to need to describe as an interview question! Fortunately the general case here is more complex than is needed, as you pointed out in your post. But I believe this approach may be close to what your (wrong) sample answer was trying to describe.
In Java SE 7, I'm trying to solve a problem where I have a series of Rectangles. Through some user interaction, I get a Point. What I need to do is find the (first) Rectangle which contains the Point (if any).
Currently, I'm doing this via the very naieve solution of just storing the Rectangles in an ArrayList, and searching for the containing Rectangle by iterating over the list and using contains(). The problem is that, because this needs to be interactive for the user, this technique starts to be too slow for even a relatively small number of Rectangles (say, 200).
My current code looks something like this:
// Given rects is an ArrayList<Rectangle>, and p is a Point:
for(Rectangle r : rects)
{
if(r.contains(p))
{
return r;
}
}
return null;
Is there a more clever way to solve this problem (namely, in O(log n) instead of O(n), and/or with fewer calls to contains() by eliminating obviously bad candidates early)?
Yes, there is. Build 2 interval trees which will tell you if there is a rectangle between x1 to x2 and between y1 and y2. Then, when you have the co-ordinates of the point, perform O(log n) searches in both the trees.
That'll tell you if there are possibly rectangles around the point of interest. You still need to check if there is a common rectangle given by the two trees.
So first of all I'm in a 100 level CS college class that uses Java. Our assignment is to make a tower defense game and I am having trouble with the pathing. I found from searching that A* seems to be the best for this. Though my pathing get's stuck when I put a U around the path. I'll show some beginner psuedo code since I haven't taken a data structures class yet and my code looks pretty messy(working on that).
Assume that I will not be using diagonals.
while(Castle not reached){
new OpenList
if(up, down, left, right == passable && isn't previous node){
//Adds in alternating order to create a more diagonal like path
Openlist.add(passable nodes)
}
BestPath.add(FindLeasDistancetoEnd(OpenList));
CheckCastleReached(BestPath[Last Index]);
{
private node FindLeastDistancetoEnd(node n){
return first node with Calculated smallest (X + Y to EndPoint)
}
I've stripped A* down(too much, my problem most likely). So I'm adding parents to my nodes and calculating the correct parent though I don't believe this will solve my problem. Here's a visual of my issue.
X = impassable(Towers)
O = OpenList
b = ClosedList(BestPath)
C = Castle(EndPoint)
S = Start
OOOOXX
SbbbBX C
OOOOXX
Now the capitol B is where my issue is. When the towers are placed in that configuration and my Nav Path is recalculated it gets stuck. Nothing is put into the OpenList since the previous node is ignored and the rest are impassable.
Writing it out now I suppose I could make B impassable and backtrack... Lol. Though I'm starting to do a lot of what my professor calls "hacking the code" where I keep adding patches to fix issues, because I don't want to erase my "baby" and start over. Although I am open to redoing it, looking at how messy and unorganized some of my code is bothers me, can't wait to take data structures.
Any advice would be appreciated.
Yes, data structures would help you a lot on this sort of problem. I'll try to explain how A* works and give some better Pseudocode afterwards.
A* is a Best-First search algorithm. This means that it's supposed to guess which options are best, and try to explore those first. This requires you to keep track of a list of options, typically called the "Front" (as in front-line). It doesn't keep track of a path found so far, like in your current algorithm. The algorithm works in two phases...
Phase 1
Basically, you start from the starting position S, and all the neighbouring positions (north, west, south and east) will be in the Front. The algorithm then finds the most promising of the options in the Front (let's call it P), and expands on that. The position P is removed from the Front, but all of its neighbours are added in stead. Well, not all of its neighbours; only the neighbours that are actual options to go. We can't go walking into a tower, and we wouldn't want to go back to a place we've seen before. From the new Front, the most promising option is chosen, and so on. When the most promising option is the goal C, the algorithm stops and enters phase 2.
Normally, the most promising option would be the one that is closest to the goal, as the crow flies (ignoring obstacles). So normally, it would always explore the one that is closest to the goal first. This causes the algorithm to walk towards the goal in a sort-of straight line. However, if that line is blocked by some obstacle, the positions of the obstacle should not be added to the Front. They are not viable options. So in the next round then, some other position in the Front would be selected as the best option, and the search continues from there. That is how it gets out of dead ends like the one in your example. Take a look at this illustration to get what I mean: https://upload.wikimedia.org/wikipedia/commons/5/5d/Astar_progress_animation.gif The Front is the hollow blue dots, and they mark dots where they've already been in a shade from red to green, and impassable places with thick blue dots.
In phase 2, we will need some extra information to help us find the shortest path back when we found the goal. For this, we store in every position the position we came from. If the algorithm works, the position we came from necessarily is closer to S than any other neighbour. Take a look at the pseudocode below if you don't get what I mean.
Phase 2
When the castle C is found, the next step is to find your way back to the start, gathering what was the best path. In phase 1, we stored the position we came from in every position that we explored. We know that this position must always be closer to S (not ignoring obstacles). The task in phase 2 is thus very simple: Follow the way back to the position we came from, every time, and keep track of these positions in a list. At the end, you'll have a list that forms the shortest path from C to S. Then you simply need to reverse this list and you have your answer.
I'll give some pseudocode to explain it. There are plenty of real code examples (in Java too) on the internet. This pseudocode assumes you use a 2D array to represent the grid. An alternative would be to have Node objects, which is simpler to understand in Pseudocode but harder to program and I suspect you'd use a 2D array anyway.
//Phase 1
origins = new array[gridLength][gridWidth]; //Keeps track of 'where we came from'.
front = new Set(); //Empty set. You could use an array for this.
front.add(all neighbours of S);
while(true) { //This keeps on looping forever, unless it hits the "break" statement below.
best = findBestOption(front);
front.remove(best);
for(neighbour in (best's neighbours)) {
if(neighbour is not a tower and origins[neighbour x][neighbour y] == null) { //Not a tower, and not a position that we explored before.
front.add(neighbour);
origins[neighbour x][neighbour y] = best;
}
}
if(best == S) {
break; //Stops the loop. Ends phase 1.
}
}
//Phase 2
bestPath = new List(); //You should probably use Java's ArrayList class for this if you're allowed to do that. Otherwise select an array size that you know is large enough.
currentPosition = C; //Start at the endpoint.
bestPath.add(C);
while(currentPosition != S) { //Until we're back at the start.
currentPosition = origins[currentPosition.x][currentPosition.y];
bestPath.add(currentPosition);
}
bestPath.reverse();
And for the findBestOption method in that pseudocode:
findBestOption(front) {
bestPosition = null;
distanceOfBestPosition = Float.MAX_VALUE; //Some very high number to start with.
for(position in front) {
distance = Math.sqrt(position.x * position.x - C.x * C.x + position.y * position.y - C.y * C.y); //Euclidean distance (Pythagoras Theorem). This does the diagonal thing for you.
if(distance < distanceOfBestPosition) {
distanceOfBestPosition = distance;
bestPosition = position;
}
}
}
I hope this helps. Feel free to ask on!
Implement the A* algorithm properly. See: http://en.wikipedia.org/wiki/A%2A_search_algorithm
On every iteration, you need to:
sort the open nodes into heuristic order,
pick the best;
-- check if you have reached the goal, and potentially terminate if so;
mark it as 'closed' now, since it will be fully explored from.
explore all neighbors from it (by adding to the open nodes map/ or list, if not already closed).
Based on the ASCII diagram you posted, it's not absolutely clear that the height of the board is more than 3 & that there actually is a path around -- but let's assume there is.
The proper A* algorithm doesn't "get stuck" -- when the open list is empty, no path exists & it terminates returning a no path null.
I suspect you may not be closing the open nodes (this should be done as you start processing them), or may not be processing all open nodes on every iteration.
Use a Map<GridPosition, AStarNode> will help performance in checking for all those neighboring positions, whether they are in the open or closed sets/lists.
Please take a moment to understand my situation. If it is not comprehendable, please tell me in a comment.
I have an ArrayList of Waypoints. These waypoints are not in any order. A waypoint has the following properties:
{int type, float z, float y, float x, float rotation}
This applies to a 3 dimensional world, but since my pathfinding should not care about height (and thus treat the world as a 2 dimensional one), the y value is ignored. Rotation is not of importance for this question.
In this 2 dimensional world, the x represents the x axis and the z represents the y axis.
If x increases, the object in the world moves east. If x decreases, the object in the world moves west.
If z increases, the object in the world moves north. If z decreases, the object in the world moves south.
Thus, these "new" waypoints can be simplified to: waypoint = {float x, float y}.
Now, these waypoints represent the X-axis (x) and Y-axis (z) locations of an object. Moreover, there is a current location: curLocation = {float x, float y} and a target location: tarLocation = {float x, float y}.
This is what I want to get:
All combinations of waypoints (aka: paths or routes) that will lead from curLocation to tarLocation under the following strict conditions:
The distance inbetween each waypoint may not be bigger than (float) maxInbetweenDistance. This includes the initial distance from curLocation to the first waypoint and the distance from the last waypoint to tarLocation. If no such combination of waypoints is possible, null should be returned.
When multiple waypoints are found within maxInbetweenDistance from a waypoint that lead towards the target waypoint, the closest waypoint should be chosen (even better would be if an alternative waypoint that is slightly further away would result in a new path with a longer distance that is also returned).
The order of returned waypoint combinations (paths) should be from shortest route (minimum distance) to longest route (maximum distance)
Finally, please consider these points:
This is the only thing I need to do AI/pathfinding wise, which is why I do not wish to use a full blown pathfinding or AI framework. I believe one function should be able to handle the above.
If returning all possible combinations of waypoints causes too much overhead, it'd also be fine if one can specify a maximum amount of combinations (but still ordered from closest to furthest). Eg. the 5 closest paths.
How would I achieve this? Any feedback is appreciated.
I think your solution is to start with Dijkstra's Algorithm to find the shortest path first. You can consider your waypoints to be a connected graph where nodes are connected if they are close enough in the xy plane then apply Dijkstra (there are many example code listings online).
Now you have the shortest path through your graph from start to finish, which will be composed of N edges of the graph.
You would next need to create N new graphs, each just like the first, but with one segment of your shortest route un-connected. Find the shortest routes from start to finish on these modified graphs. Now you have N+1 routes which you can sort by length.
Repeat this until you have found enough paths for your needs, or there are no unranked paths left.
I haven't found a name for this technique, but it is described as a modification to Dijkstra here.
If your waypoints possess connectivity, you should take a look at Dijkstra's shortest path algorithm. The first couple of google hits even lists an implementation in Java. (I can't tell if connectivity is known from the post, but it does contain the "graph-algorithm" tag, so I'll assume so). As the name suggests, this method give you a shortest path between the two nodes.
Your constraints are challenging, as is the need for all possible combinations of paths under those constraints. Again - assuming connectivity exists - your node adjacency matrix can enforce your maxInbetweenDistance rule. Likewise, you can use this matrix in obtaining the "next best" solutions. Once the optimal path is known, you can mark that path (or elements of it) as unavailable, then re-run Dijkstra's algorithm. By repeating this process, you can obtain a set of increasingly sub-optimal paths.
As a matter of convention: in most computational geometry problems, Z is the height, and the horizontal plane is formed by the XY axes.
Well the easiest to implement would probably be creating an ArrayList of paths, which would be in turn an ArrayList of waypoints, that contains ALL possible paths, then using a recursive function to return whether each path is Valid or not given the starting and finishing point values, and the max distance, and if a path is not valid remove it from the list. The next step would be going through each of the paths that is left and ordering them from shortest total distance to shortest. This would be the brute force method of getting what you want, so the least efficient one possible. When I get home tonight I will repost if some one already hasn't with a more efficient method for doing this in java.
Edit: if the brute force method is too much, the list of waypoints will have to be sorted some how, the best way is probably to sort them initially based on distance from the starting point.