finding feasible paths for a two-point body

JOURNAL OF ALGORITHMS 10,109-119 (1989)

Finding Feasible Paths for a Two-Point Body*

ELLEN B. FEINBERG

A T&T Bell Laboratories, Holmdel, New Jersey 07733

AND

CHRISTOS H. PAPADIMITRIOU

University of California at San Diego, LA JoNa, California 92093

Received July 17,1987; accepted April 8,1988

We present an 0(n*) algorithm for determining whether two points can move from an initial to a final pair of positions amidst polygonal obstacles such that they always remain a fixed distance apart. We also consider a variation of this problem in which the distance between the points can vary, but is never greater than some upper bound. 0 1989 AC~&~IGC PWS, IIIC.

1. INTRODUCTION

Suppose that two points are constrained to move in two dimensions amidst polygonal obstacles from an initial pair of positions to a final pair, subject to the restriction that they always remain a fixed distance apart. (Without loss of generality, we may assume this distance is equal to one.) We call this the Two-Point Problem. We also consider a variation in which the distance between the points can vary, but it is neuer more than one. This will be referred to as the Variable Two-Point Problem. The Two-Point Problem can be thought of as two people carrying a ladder among low-rise obstacles (ponds perhaps), while the variable problem is that of planning paths among obstacles for two people who must be under constant radio contact, but the radio has some fixed range. These problems are variations of the coordinated motion planning problem [9, 121. However, instead of moving two independent bodies, we have added a constraint which limits the distance of the bodies.

*Research was partially supported by the Hughes Artificial Intelligence Center.

109 0196-6774/89 $3.00

Copyright 0 1989 by Academic Press. Inc. All rights of reproduction in my form reserved.

110 FEINBERG AND PAPADIMITRIOU

Goodman, Path, and Yap [2] studied a closely related set of problems. Their ladder moving problem is to determine whether two painters carrying a ladder can stay on a specified path, i.e., “stay off the grass.” They also consider the following problem: if an irregularly shaped body can be moved through two points at a distance d apart, then can it also be moved through two points at a distance greater than d apart. The latter problem is called the Ring- Width Problem. It was first suggested by Yap in [13]. An algorithm is presented to solve this problem in 0( n2 log n) time when the body is a polygon with n vertices [2].

The problems considered here are variants of the Ladder Problem [4-6, 8, lo] (which in turn is a special case of the Piano Movers’ Problem [8], the fundamental problem in motion planning). There are two approaches to motion planning which have yielded successful algorithms that solve the ladder problem, namely decomposition and retraction. In both of these methods, the space of free positions FP of the ladder is defined as the set of positions in which the ladder does not come into contact with any obstacles. In the decomposition approach, the basic idea is to divide FP into relatively simple cells. Then the adjacency relations between these cells are defined and represented in a graph G in which each node corresponds to a cell and there is an edge between two nodes if the cells are adjacent. Motion planning is then reduced to searching in a finite graph. In the retraction approach, a lower-dimensional subspace FP’ c FP is defined, such that there is a motion-preserving mapping (or retraction) from FP to FP’. This procedure is repeated (retract FP’ to FP” c FP’, etc.), until eventually we have a l-dimensional network and thus motion planning has been again reduced to graph searching.

Historically, the first solution to the ladder problem used the decomposition technique. Schwartz and Sharir [8] present an O(n’) time algorithm which can be extended to solve the Piano Movers’ Problem for general connected polygonal bodies. (Here n stands for the complexity of the scene, that is, the number of lines and vertices in the obstacles.) Retraction was used by D’Dunlaing, Sharir, and Yap [S, 61 to reduce this complexity to O(n2 log n log* n). More recently, Leven and Sharir [4] obtained an altema- tive decomposition algorithm which can be implemented to run in O(n2 log n) and Sifrony and Sharir [lo] developed a technique which also runs in O(n2 log n) time but is more efficient if the obstacles are not too cluttered, The latter two algorithms are the fastest ones known for the ladder problem. In fact, O’Rourke [7] constructed an example which requires Q(n2) moves, establishing a lower bound for the worst-case complexity of any algorithm which specifies the moves. Both decomposition approaches can be applied to the Two-Point Problem. For this special case, the Schwartz and Sharir [8] approach yields an O(n3 log n) algorithm and the Leven and Sharir [4] approach yields an O(n2 log n) algorithm [ill.

FINDING FEASIBLE PATHS 111

However, because both approaches deal with rigid bodies, they do not easily extend to the Variable Two-Point Problem.

In this paper we present an Q(n*) algorithm for the Two-Point Problem (and show how to extend it to the Variable Two-Point Problem). The new algorithm takes advantage of the special structure of this problem and is essentially a different decomposition technique, based on triangulations of the area outside the obstacles.

2. AN EFFICIENT ALGORITHM FOR THE TWO-POINT PROBLEM

Some preliminary definitions: Given two points x and y in the plane, d(x, y) denotes the Euclidean distance between them. If Y is a nonempty set of points, then let the distance d(x, Y) be defined as inf{ d(x, y): y E Y }, and if X is another nonempty set, then d( X, Y) = inf{ d(x, Y): x E X}. It will only be necessary to consider sets .X and Y which are closed, bounded, convex polygons. Thus the distances d(x, Y) and d( X, Y) are well defined and can easily be computed.

We consider obstacles that are polygons consisting of line segments which we call walls and vertices which we call wall corners. The complexity of the scene, n, is simply the total number of walls (or comers) of the obstacles. We let W, the wall region, be the union of the open obstacles. The complement Y of W is the (closed) free region. We exclude degenerate situations in which the interior of an obstacle is empty or in which a wall comer is incident to more than two walls. Furthermore, we assume that the obstacles are in general position, e.g., no three wall comers are colinear and no two parallel walls are such that the perpendicular distance between them is equal to the length of the ladder. The two points under consideration will be denoted by P and Q. A free position of the two points is a pair (p, q) E V2, where p stands for the position of P and q the position of Q. We are given two free positions ( pl, qJ and ( p2, q2), and we wish to find a continuous motion between these two positions, or establish that no such motion exists. A continuous motion is a continuous function f: [0, l] + ‘v* such that (4 f(o) = ( pl, qd, (b) f(l) = ( p2, q2), and (4 for any 0 I t I 1, f(t) = (p, q) where p, q E V and d( p, q) = 1.

The following is a key lemma which shows the existence of certain basic continuous motions, from which the overall motion f, if it exists, will be composed.

-- LEMMA 2.1. Let line segments q and E2 be such that d(L,, Ld I 1, and

they may only intersect at a common endpoint. Suppose the motion of PQ is restricted such that P must remain on G and Q must remain on &. If the initial position of PQ is ( pl, a,) where p1 E G, q1 E rz and d( pl, ql) = 1,


then there exists a continuous motion of the points, always remaining at unit distance, so that an endpoint of either G or & is final& reached.

Prooj: Trivially, if G and & are parallel the result is true. Thus we may assume they are not parallel and therefore the lines L, and L, containing G and G, respectively, intersect. Call this intersection point 0 and let the variable p (resp. q) represent the distance of P (resp. Q) from 0. Also, let (Y be the angle p,Oq,. Since 5 and G may only intersect at a common endpoint, at least one of L, and & is completely to one side of 0. Without loss of generality we may assume q lies entirely to one side of 0. Then q may be represented by the interval ( pmin, p,,), where pmin 2 0.

All positions with P on segment q and Q on line L, may be represented by the following equations:

and

q=pcoslX-ii=&&,

Clearly both of these are continuous functions of p and the initial position ( pl, ql) lies on one of these curves. Thus as endpoint P moves from p1 to ptin along G, either P reaches pmin (an endpoint of q) or Q reaches an endpoint of G. 0

As in [4, 81, our goal is to partition V into regions. However, in our case we need not be concerned that the interior of the segment m does not intersect any obstacles. We partition V into a collection of O(n) disjoint triangles { R; }. (We may assume that V is bounded by taking the smallest rectangle that includes all obstacles and extending it by one in all direc- tions. Then the triangulation can be done by the technique of Foumier and Montuno [l] in O(n log n) time.) If (p, q) is a free position of PQ ((p, q) E FP) such that p E R, and q E R, (where R, and R, are triangles), then we say that (p, q) E FP( R, x R2). This subset of R, X R, may contain several connected components, studied in the following lemma:

LEMMA 2.2. The closure of each connected component of FP(R x R’) contains at least one free position ( p*, q*) such that either p* is a vertex of R or q* is a vertex of R’.

Proof: We will show that from any free position ( p, q) E FP( R x R’) of PQ, there exists a continuous motion in which P remains in R and Q remains in R’, such that PQ reaches a vertex of either R or R’. We begin by choosing a vertex v of R arbitrarily. Keeping the orientation of PQ


fixed, we move P along the segment F. One of three things will happen:

(1) P reaches u,

(2) Q reaches a vertex of R’, or

(3) Q reaches an interior point of a boundary edge E’ of R’.

If either of the first two cases occur, we are done. If case (3) occurs, we change the direction of motion of PQ such that Q moves along edge E’ and we continue to hold the orientation of PQ constant. Again, one of three things happen:

(1) Q reaches an endpoint of E’,

(2) P reaches a vertex of R, or

(3) P reaches an interior point of a boundary edge E of R.

We are done if case (1) or case (2) occurs. Otherwise, we restrict the motion of PQ such that P remains on E and Q remains on E’. Applying Lemma 2.1, PQ reaches an endpoint of either E or E’. 0

Consider now a single triangular region R and a point x such that d(x, R) < 1. The intersection of R and the circle of radius one centered at x consists of up to three arcs. See Fig. 1. We can label each arc by the edges which limit it. For example, in Fig. 1 the arcs are labeled [E,, El], [E2, E,], and [E,, E,]. Clearly, (x, y) and (x, y’) are in the same connected component of FP(x x R) if and only if y and y’ are on the same arc of this circle.

In the previous lemma, we showed that the closure of each connected component of FP( R X R’) contains a point (p*, q*) such that p* or q* is a vertex of R or R’, respectively. Suppose the vertex is p*. Then let the connected component be fubeled by ( p*, [E,, E,]), where [E,, E,] is the arc in R’ containing q* of the circle of radius one centered at p*, i.e., q* E [E,, E,]. Symmetrically, if the vertex is q*, then the connected component will be labeled by ([E,, EJ, q*), where [E,, E2] is the arc in R containing p* of the unit circle centered at q*. Note that this does not uniquely label a connected component since there may be more than one

FIG. 1. Intersection of triangular region and circle of radius one about x.


vertex in each. Furthermore, if p* is a vertex of both R and R” then the same label (p*, [E,, ,?&I) represents a connected component of both FP( R x R’) and FP( R” X R’). Symmetrically, the same is true for a label

([El, 417 4*1* LEMMA 2.3. Let (pl, ql) E FP( R X R,) and ( p2, q2) E FP( R X R2).

Suppose R, and R, share a common boundary edge E’. Then ( pt, qJ and ( p2, q2) are in the same connected component of FP( R X (R, U R,)) if and only if either the connected component of ( pl, ql) E FP( R X R,) is labeled by (p*, [E,, E’]) and the connected component of ( p2, q2) E FP( R X R2) is Iabeled by (p*, [E’, E2J) or both connected components are labeled by ([F,, Fz], q*), where FI and F2 are boundary edges of R.

Proof If ( pl, ql) and ( pz, q2) are in the same connected component of FP( R X (R, U R,)) then there exists a position (p’, q’) E FP(R x (R, U R2)) such that q’ E E’. Suppose PQ is in position (p’, q’). Using the method in the proof of Lemma 2.2, we move PQ, keeping its orientation constant, such that Q remains on E’. One of the following will occur:

(1) Q reaches an endpoint of E’,

(2) P reaches a vertex of R, or

(3) P reaches an interior point of a boundary edge F of R.

If case (3) occurs, P is on an edge F and Q is on an edge E’. Applying Lemma 2.1, either P will reach an endpoint of F or Q will reach an endpoint of E’. Thus in any of the three cases, PQ has reached a position (p*, q*) such that p* or q* is a vertex.

First, suppose p* is a vertex. Then Q is on edge E’, the shared boundary of R, and R,, and E’ defines an endpoint of an arc of the circle centered at p* with respect to both regions R, and R,. If instead q* is a vertex then p* is an element of an arc [F,, F,] and both connected components are labeled by ([ FI, Fz], q*). This proves the “only if” part of the lemma.

For the converse, first suppose the connected component of (pl, qJ E FP( R X R,) is labeled by (p*, [E,, E’]) and the connected component of ( p2, q2) E FP( R X R,) is labeled by (p*, [E’, EJ). Then there exists q* on E’ such that (p*, q*) E closure(FP( R X R,)) and (p*, q*) E closure(FP(R X R,)). Clearly, (pl, ql) and ( p2, q2) are in the same connected component of closure( FP( R X (R, U R2))). If both connected components are labeled by ([I;,, F2], q*) then the result is immediate. •I

As in [4, 81, these results enable us to reduce. the original continuous problem to reachability in a finite undirected graph called a connectivity graph CG. The nodes in CG correspond to the labels of the connected components of FP(R X R’) for each pair of regions R, R’. As mentioned before a label is not unique and thus there may be more than one node


FIG. 2. Conditions for the existence of an edge.

representing the same connected component of FP(R x R’). To account for this, every pair of nodes representing the same connected component (see Lemma 2.2) are connected by an edge in CG. Next, Lemma 2.3 gives the necessary and sufficient conditions for the existence of an edge between nodes representing FP( R X R,) and FP( R X R2). These conditions are illustrated in Fig. 2. In Fig. 2a, a vertex p* in R has an arc [E,, E’] in R, and an arc [E’, E,] in R,. Since these arcs share a common endpoint, the corresponding nodes in CG are joined by an arc. In Fig. 2b, an endpoint q* of the common boundary of R, and R, has an arc [E, E’] in R. ([E, E’], q*) labels connected components in both FP(R x R,) and FP(R x R2). Thus the corresponding nodes are connected in CG.

THEOREM 2.4. Let ( pl, ql) E FP(R, x R[) and ( p2, q2) E FP(R, x R;). Suppose ( pl, ql) is in a connected component of FP(R, X Ri) labeled by ( p:, [E,, I$‘]) (or ([E,, E;], 4:)) and ( p2, q2) is in u connected component of FP(R, X R$) labeled by (p,*,[E,, E,‘]) (or ([E2, &I, q$)). Then PQ can move continuously through FP from ( pl, ql) to ( p2, q2) if and only if the corresponding nodes are in the same connected component of CG.

Proof. Suppose there exists a motion x(t) = [p(t), q(t)] in. FP from ( pl, ql) to ( p2, q2). If for some value of t both p(t) and q(t) are on region boundaries then x(t) can easily be perturbed so that P and Q do not change regions simultaneously. Also, by an argument similar to that in [8], we can assume that x(t) crosses regions transversally and only finitely often. Thus the motion x(t) passes through a sequence of region pairs such that for any two consecutive ones (R,, Ri) and (R,, R;), either R, = R, or R[ = R;, and the remaining regions must share a common boundary edge. Suppose x(t) E R, x R; for all t E (tI, tz) then all of these points are in the same connected component of FP( R, X R;). If at some t’, x(t) crosses from R, (resp. R;) to R, (resp. R$) then the corresponding connected components of FP(R, x R;) and FP( R, x R;) (resp. FP(R, X R;)) are


adjacent. Therefore, x(t) determines a sequence of connected components and the corresponding nodes are connected by an edge in CG.

Conversely, suppose the two nodes are in the same connected component of CG. By definition of CG, each node corresponds to a connected component of FP( R x R') for some regions R, R'. Since there is a path in CG between the two nodes, each of these connected components must belong to the same overall connected component of FP. Thus there exists a pathin FP between(p,,q,)E FP(R, x R;) and(p,,q,)E FP(R, X R;).

0

Our algorithm for deciding whether a continuous motion exists, proceeds as follows:

(1) Reprocessing. Compute the connectivity graph CC: (a) Read the input, and triangulate the free space V. (b) For each pair of regions R,, R, such that d( RI, R2) I 1, identify the pairs (p, [ Ei, E{])

and ([E,, E;], q) where p is a vertex of R,,[ El, E,‘] is an arc in R, with respect to p. q is a vertex of Ri, and [ E2, Ei] is an arc in RI with respect to q. These are the nodes

of CC. (c) For each pair of nodes representing the same connected component of FP( R, X R,),

add an edge to CC. (d) For each pair of nodes (p,[E,, Ez]), (p, [E2, EJ), where [El, E,] is an arc of some

region R, with respect to p and [E,, E3] is an arc of an adjacent region R, with respect to p, add an edge connecting these nodes to CC. Symmetrically, add edges joining corresponding pairs of nodes ([E,, EJ. q) and ([E,, E,], q). Also, if two nodes have identical labels, ( p, [ El, EJ) (or ([E,, EJ, q)), then add an edge connecting these nodes.

(2) Locate initial and final nodes. Locate the pairs of regions containing the initial and final positions for the endpoints P and Q of PQ. Determine a vertex and arc defining the appropriate connected components of these region pairs. This identifies the initial and final nodes.

(3) Path search. Search for a path in CC that connects the initial node to the final node.

If a path in CG has been found then a corresponding motion through FP can be determined. Recall that each node of CG corresponds to a label of a connected component of FP(R X R') for a pair of regions R, R'. A label denoted by ( p*, [E,, EJ) implies there exists a position (p*, q*) such that

p* is a vertex of R and q* E [E,, EJ. (SimiIarly, a label ([E,, EJ, q*)

implies there exists a position (p*, q*) such that q* is a vertex of R' and p* E [El, E,].) Thus each edge of the path in CG can be described as a motion in FP between positions ( p?, ql*) and ( pz, q$) such that either pr or qi* is a vertex (for i = 1,2) as discussed above. There are three types of motions: First, there are motions remaining within a connected component of FP(R X R'). Second, there are motions from FP( R x R,) to FP(R X R,) (resp., FP(R, X R) to FP(R, X R)) in which P (resp., Q) stays fixed at a vertex p* (resp., q*) and Q (resp., P) moves along an arc of the circle about p* (resp., q*). Finally, there are motions from FP( R x R,)


to FP(R x R2) (resp., FP(R, x R) to FP( R, X R)), both represented by the same label ([E,, E,], 4*) (resp., (p*, [E,, &I)), in which no actual movement is required.

LEMMA 2.5. FP(R X R’) has O(1) connected components and each connected component is identified by O(1) nodes.

Proof. By Lemma 2.2, each connected component can be identified by at least one vertex and arc. As mentioned before, each vertex can have up to three distinct arcs. Since there are six vertices in a pair of triangles there are only O(1) connected components. (Note this upper bound is eighteen but in practice the number will be far smaller.) 0

LEMMA 2.6. The number of nodes and edges in CG is O(n2).

Proof. The triangulation decomposes I/ into O(n) regions. There are O(n2) pairs of regions and by Lemma 2.5 each pair of regions defines O(1) nodes. Thus there are O(n2) nodes in CG. There are O(1) edges between the nodes representing the same connected component of FP(R x R’). Finally, there are O(1) edges connecting nodes representing a connected component of FP( R X R,) and nodes representing a connected component of FP( R X R2). Overall, there are O(n2) edges in CG. 0

THEOREM 2.7. The complexity of the algorithm is O(n2).

Proof The triangulation of V can be done in 0( n log n) [l]. There are O(n2) pairs of regions and computing the distance between any two regions takes O(1) time. So step (b) requires O(n2) time. There are O(1) nodes representing the same connected component of FP( R X R’). These nodes can be recognized by applying one of the standard decomposition tech- niques [4, 81 to the space R x R’. (This takes O(1) time since the number of walls is constant.) Thus in step (c) we add O(n2) edges and each edge requires constant time. Adding each of the edges in step (d) can be done in O(1) time. (With the appropriate data structures [3] the adjacent regions can easily be determined.) Since each region has at most three adjacent regions, this step requires O(n2) time. Phase (2) requires a point location subroutine which takes O(n) time. Finally, from Lemma 2.6 we know that CG has 0( n 2, edges and searching the graph will require 0( n 2, time. Thus the overall complexity is O(n2). 0

3. EXTENSIONS AND CONCLUSIONS

We have presented an O(n2) algorithm for the Two-Point Problem. In fact, the algorithm is an optimal solution to this problem. To see this, consider the situation in Fig. 3. The only free space in which the points are


A2 B2

A6 i3 A7 87

*El B8

FIG. 3. Example requiring Q(n2) moves.

allowed to move consists of the path including a and b and the path A,B,B,A2A,B,. . . B,,,. The path from a to b consists of e(n) linear segments and there are e(n) line segments m. Each of the line segments && is parallel to & and d( a, Ai) = 1, d(b, Bi) = 1 for all i. Suppose the initial position of the two points is (a, A,) and the final position is (b, B,,,) then a feasible path requires that P traverse the path from a to b (possibly including the parts of the path extending out from a and b) @(n) times while Q traverses each of the segments of the path from A, to B,,,. Since the path from a to b consists of e(n) segments and each is traversed O(n) times, this instance of the Two-Point Problem requires 0(n2) moves.

Precisely the same approach yields an O(n2) algorithm for the Variable Two-Point Problem. There are obvious (and easier to prove) analogues of Lemmata 2.1, 2.2, and 2.3. Nodes of CG are now simply pairs of triangles [R,, R, 1, which are at a distance of one or less from each other and an edge exists between two nodes [R, R,] and [R, R2] (or [R,, R] and [R,, R]) if R, and R, share a boundary and the distance between R and the shared boundary is less than or equal to one. It follows that connectivity on CG captures precisely reachability in the Variable Two-Point Problem, and can be decided in O(n*) time. The observation of the previous paragraph still applies here. Thus this approach also yields an optimal solution for the Variable Two-Point Problem.

ACKNOWLEDGMENT

‘This research was partially supported by a grant from the Hughes Artificial Intelligence Center, part of Hughes Aircraft.


REFERENCES

1. A. FOURN~ER AND D. Y. MONTUNO, Triangulating simple polygons and equivalent problems, ACM Trans. Graphics 3 (1984), 153-174.

2. J. E. GOODMAN, J. PANOS, AND C. YAP, “Mountain Climbing, Ladder Moving, and the Ring-Width of a Polygon,” Technical Report 308, New York University, Courant Institute of Mathematical Sciences, July 1987.

3. L. GUIBAS AND J. STOLFI, Primitives for the manipulation of general subdivisions and the computation of Voronoi diagrams, ACM Trans. Graphics 4 (1985), 74-123.

4. D. LEVEN AND M. SHARIR, An efficient and simple motion planning algorithm for a

8.

9.

10.

11.

12.

13.

ladder moving in two-dimensional space amidst polygonal barriers, J. Algorithms 8 (1987), 192-215. C.O’DUNLAING, M. SHARIR, AND C. K. YAP, Generalized Voronoi diagrams for moving a ladder. I. Topological analysis, Comm. Pure Appl. Math. 39 (1986), 423-483. C. G’DUNLAING, M. SHARIR, AND C. K. YAP, Generalized Voronoi diagrams for moving a ladder. II. Efficient construction of the diagram, Algorithmica 2 (1987) 27-59. J. O’ROURKE, “A Lower Bound on Moving a Ladder,” Technical Report 85/20, Department of Computer Science, Johns Hopkins University, November 1985. (Also discussed in Y. KE AND J. O’ROURKE, Moving a ladder in three dimensions: Upper and lower bounds, “Proceedings, Third Symposium on Computational Geometry, 1987,” pp. 136-145.) J. T. SCHWARTZ AND M. SHARIR, On the Piano Movers’ Problem. I. The special case of a rigid polygonal body moving amidst polygonal barriers, Comm. Pure Appl. Math. 36

(1983), 345-398. J. T. SCHWARTZ AND M. SHARIR, On the Piano Movers’ Problem. III. Coordinating the motion of several independent bodies: The special case of circular bodies moving amidst polygonal barriers, Internal. J. Robotics 2 (1983), 46-75. S. SIFRONY AND M. SHARIR, A new efficient motion-planning algorithm for a rod in a two-dimensional polygonal space, Algorithmica 2 (1987), 367-402. E. B. SILVERBERG, “Motion Planning and Shortest Paths in the Plane,” Ph.D. thesis, Department of Operations Research, Stanford University, 1986. C. K. YAP, “Coordinating the Motion of Several Discs,” Technical Report 105, New York University, Courant Institute of Mathematical Sciences, February 1984. C. K. YAP, How to move a chair through a door, IEEE Trans. Automatic Control, in

finding feasible paths for a two-point body

Documents