RSD-TR-26-86 A Fast Procedure for Computing the Distance Between Complex Objects in Three Space by E.G. Gilbert D. W. Johnson and S.S. Keerthit The University of Michigan Ann Arbor, MI 48109 October 1986 CENTER FOR RESEARCH ON INTEGRATED MANUFACTURING Robot Systems Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109-1109 tThis research was partly supported by the Center for Research in Integrated Manufacturing at the University of Michigan

/ CcM

RSD-TR-26-86 ABSTRACT An efficient and reliable algorithm for computing the Euclidean distance between a pair of convex sets in R m is described. Extensive numerical experience with a broad family of polytopes in Rs shows that the computational cost is approximately linear in the total number of vertices specifying the two polytopes. The algorithm has special features which make its application in a variety of robotics problems attractive. These are discussed and an example of collision detection is given.

RSD-TR-26-86 TABLE OF CONTENTS 1. Introduction......................................................................... 2.- Object Representations and Distance Measures...................................... 2 3. Preliminaries.............................................. 4. The Theoretical Algorithm.................................................................... 5. The Distance Subalgorithm..................... 11 6. The Numerical Algorithm.................................................................... 15 7. Numerical Experiments........................................ 19 8. An Example of Collision Detection........................................ 21 9. Conclusion............................................................................................. 22 10. References.............................................................................................. 23 i

2RSD-TR-268-8 1. Introduction In this paper we present an efficient algorithm for determining the Euclidean distance between two convex sets in three dimensional space. This problem is important in robotics and occurs also in other fields such as computer aided design and computer graphics. For convex polytopes and -their spherical extensions, the algorithm terminates finitely. Numerical experience with such problems is most encouraging. For a wide variety of examples the computational times are nearly linear in the total number of vertices, M = M1 + MO, required to specify the two polytopes. Moreover, the coefficient of linear growth is quite small. Because the algorithm is so efficient, we expect that it will become a useful tool in solving collision detection problems and path finding problems (see, e.g., [3], [6], [81, [10] and [4], [5], [12], [21], [28]). Our own applications of the algorithm have been to optimal path planning in the presence of obstacles [15], [17], [18]. Since there is an extensive literature concerning the polytope distance problem, we limit ourselves to a brief review of some representative papers. The problem is in the field of computational geometry [20]. Consequently, many algorithms are specifically designed to achieve bounds on the form of the asymptotic computational time. For two dimensional problems [27] gives an O(log2M) algorithm, and more recently, 0(logM) algorithms have been exhibited [9], [13]. The three dimensional problem has been considered in [11], but the O(M) result there seems to be in error; the actual time appears to be O(MlogM). See [24] for another O(MlogM) result. Because of their special emphasis on asymptotic performance, it is not clear that the algorithms in the preceding papers are efficient for practical problems where M is large, but not exceedingly large. Other schemes have also been described: [25] presents a program which uses a projection/combinatoric approach for polyhedra with facial representations, [5] and [7] are concerned with "directed" or "translational" distances (more about this later), [23] considers boxes [22] considers line segments. It is also possible to convert the distance problem to a quadratic programming problem and apply any of the well-developed computer programs which are applicable. Unlike the procedures of the previous paragraph, our algorithm has its origins in mathematical programming and treats directly the specification of the Computing the Distance i

RSD-TR-26-86 convex sets in terms of their support properties (for polytopes these properties are obtained easily from their vertices). The algorithm is in the same family as the algorithms described originally by Barr, Gilbert and Wolfe [1], [21, [29] and may be viewed as a descent procedure which works on the distance between elementary polytopes contained in the convex sets. We have devised a special procedure for evaluating the distance between the elementary polytopes. It contributes significantly to the overall efficiency of the algorithm. An important feature of the algorithm is its very general initialization features. When used in continuum collision detection problems, they allow significant reductions in the total computation time. The algorithm has good numerical properties and bounds on the computational errors are available. An early version of the algorithm due to D.W. Johnson was used in the optimal path planning computations described in [18]. A detailed treatment of underlying algorithmic questions in a broader setting is given in [16]. The plan of the paper is as follows. In Section 2 we formulate distance measures for complex, not necessarily convex, objects and suggest how our algorithm may be applied to their computation. We also review what happens when the position and orientation of the objects is specified by a set of configuration variables. Section 3 shows how the support properties of the Minkowski set difference between the two sets can be computed efficiently. This leads to the basic problem of finding the distance between the origin and a single convex set. Section 4 describes the theoretical algorithm for solving this problem; Section 5 presents the efficient procedure for elementary polytopes; Section 6 introduces modifications to account for the effects of numerical errors. Many numerical experiments have been carried out; these are reported in Section 7. In Section 8 the algorithm is applied to a collision detection problem due to Canny [8]. A conclusion summarizes the key contributions and indicates some extensions. 2. Object Representations and Distance Measures Given two objects A and B in three space, it is convenient to represent them by compact sets: KA, KS C R3. In particular, the points in KA and KB describe respectively the space occupied by the objects A and B. For -z = (z1, z', zS) E R3, let Iz. [ denote the Euclidean length i/(z)2 + (z2) + (zS)2. The distance between the objects A and B is defined by the closest points in KA and KB: 2 Computing the Distance

RSD-TR-26-88 d(KA, K )= min{ I x - y IxE IKA, Y E KB. (2.1) While computational considerations may suggest the use of other metrics in (2.1), the Euclidean distance I - y is the most natural. It conforms with the "physical" notion of distance and makes d invariant with respect to different choices for the origin and orientation of the coordinate system. Because KA and KIB are compact, the minimum in (2.1) exists and d is defined. However, it is only for simple objects such as spheres and line segments that formulas for d may be given. For some examples see [19]. If A and B are each composed of a collection of objects, the distance d(KA, KB ) may be computed in terms of the distances between the constituent objects. Specifically, suppose KI, i E I = (1,,N}, are compact sets in R 3, IA and IB are disjoint index sets in I, and KA = U K,, KB = U K (2.2) i E IA jEIa Then d(KA, K) = min( d:i i A, IB (2.3) where di =min -y l E K, y EKi = d(Ki,Ki). (2.4) See the example in Figure 1. When KA and KB are not convex, they can often be represented by (2.2) where the Ki, i E I, are convex. This allows our algorithm, which works on convex sets, to be applied to non-convex objects. If the distance between objects A and B is known, so is the distance between their spherical extensions [18]. The r-spherical extension of KCR3 is defined by K' I I-yl r, YKEK}, r 0. (2.5) It is easy to verify that Computing the Distance 3

RSD-TR-.26-88 d(K' Kr") = (d(KA, KBs)- rA -B) (2.6) where (a)+ = a, a > 0, and (a)+ = 0, a < 0. More generally, Kc= U K', KD = UKj (2.7) iEIA jEIB implies d(Kc, KD) = min{(di - r - ri)+: iE iA, j E I. (2.8) Spherical extensions are valuable for several reasons. They may be used to cover an object with a shell of safety: if z q Kr, it is clear that the distance between x and K exceeds r. More importantly, they lead to a rich family of geometric shapes, convex polytopes and their spherical extensions, for which our algorithm is effective. Object A in Figure 1 is a simple example of how the family can be exploited. It is the union of two spheres (extensions of points) and a circular cylinder with end caps (an extension of a line segment). A somewhat more complex example is a solid rectangular plate of thickness 2r with round edges; it is modelled by an r-spherical extension of a planar polytope with four vertices. Similarly, more general wire-frame objects can be given rounded representations. Often the position and orientation of the objects Ki in (2.2) are specified by a configuration vector q E R n. For instance, if A and B are interacting manipulators whose links and payloads are the Ki, the components of q are the joint variables for the two manipulators. To be more precise, Ki, i E I, is obtained by translating and rotating a closed point set C;: Ki (q) ={ T (q)w + pi (q): w E Ci (2.9) Here: pi(q)E R3 is the translation, Ti(q)E R3X3 is the (orthogonal) rotation matrix, and Ci describes Ki in its reference position. In practice, there are various ways of obtaining pi(q) and T;(q). For example, they may be extracted from the usual 4X4 homogeneous transformation matrix. If Ci is a polytope with vertices wii E R3, j=l,'',M;, the corresponding vertices of K (q) are given by z;i = T;(q)w;i + P;(q), a simple computation. It follows from the 4 Computing the Distance

RSD-TR-26-86 orthogonality of Tj(q) that the reference object for a spherical extension is independent of q; i.e., K'i (q) = Ti (q)w + pi (q): w E i' (2.10) In [15] the dependence of dii on q has been examined in detail. Suppose the elements of Tk (q) and pk (q), k=i,j are continuously differentiable in q. Then dii(q) is Lipschitz continuous and has a gradient (Frechet derivative) almost everywhere. It is easy to give examples (Ki and Ki may be convex) where at a specific q, dii (q) does not have a gradient. If d;q (q) > 0 and the nearest points z k E Kk (), k=i,j, are uniquely determined, dii(q) does have a gradient at q. In particular, Vq dii(q') = V, I Ti(q)w'i + Pi(q) - Ti(q)w - PiP(q) I, q=-, where w'k = TkT(q-)(zk - Pk (q-), k=i,j. Once z* and z*i have been found (by the distance algorithm) the evaluation of this expression is relatively easy to carry out. We conclude this section with a few remarks about a distance measure which gives further information when KA and KB intersect. For X, YCR3 let X: Y = ({x: y: x E X, y E Y} and let q denote the empty set. The condition KAf (KB + {z}) 3 b occurs if and only if z E KA - KB. Thus KA -KB is the set of translations z for object B which cause A and the translate of B to intersect. Thus, d(KA, KB)= min{ z; EK E KB - (2.11) may be interpreted as the smallest translational distance between A and B which allows A and B to touch. Similarly, d(K KB) inf{z I: z(KA - K)} (2.12) = min{ I: E I(KA - KB)' } where cl(KA - KB )' denotes the closure of the complement of KA - KB, is the lower bound on the translational distances which allow A and B to be separated. Using different notations, the measure d- has been proposed by Buckley [5] (for convex objects) and Cameron and Culley [7] as a measure of the depth of Computing the Distance 5

RSD-TR-26-86 intersection. Clearly, d > 0 implies d' = 0 and d' > 0 implies d = 0. Unfortunately, d' is much more difficult to deal with than d. In general, cl(Ki - Kj ) is not convex, even when Ki and Kj are. Thus, the computation of d'ii is much more difficult than the computation of dii. When KA and KB are given by (2.2) it is easy to see that d'(KA, KB ) dj, i E IA, j E IB. But, (2.3) does not apply to d-(KA, KB ). For an example, replace KA in Figure 1 by K6= {a}. Then dq5 = d4- 0, and d(KA, KB) > 0. This shows there is a further difficulty. Even if d' can be computed for convex objects, it is not possible to compute d' for the union of convex objects. There is one case of interest where d' can be obtained easily. Suppose the following assumptions hold: KA and KB are convex, d(KA, KB) > 0 and d(KA, K B)=0. Then it is possible to prove d'(KAs KJB)= rA + rB - d(KA, KB). Without the assumptions, it is only true that d'(KAA, KBB ) 2 rA + rB -d(KA, KB). 3. Prelimninaries In this section we introduce some notations and basic results which are required for the algorithm. Everything is stated in Rm, because the results are not restricted to m = 3. We use x-y for the inner product of x,yER m and z1 2 = z'z. Throughout the section X C Rm is compact and YC Rm is finite, i.e., Y= {(Y, Y ). The affine and convex hulls of X are given by affX= {X; zi: x EX,Xl+.. +X+t = — 1}, (3.1) coX;: xi E X, Xi > 0, Xl+...+Xm = 1. (3.2) It is easily confirmed that affX is the translate of a linear space. For example, affY =Y + {y) where Y is the linear span of (Y2 - Y1,, - Y }. Y is affinely independent if dim affY = dimY = v - 1. If Y is not affinely independent, it is always possible to pick an affinely independent set Y C Y such that affY -affY. The set coY is a convex polytope whose vertices are contained in Y. Suppose X belongs to the translate of a linear space X. The Caratheodory ~~~~~~~~8 ~~Computing the Distance

RSD-TR-26-86 theorem [261 states that there is no loss of generality if in (3.2) 1 is restricted so that 1 < dimX + 1. The nearest point in X to the origin, v(X), is determined by v(X) E X, v(X) = min: x E X} (3.3) Suppose X is convex. Then the near point v(X) is unique (otherwise it is easy to specify x E X with z x < I v(X) I) and has the representation v(X)= V Xi;, Xi E X, X; > 0, X1 +.- + X1 = l, (3.4) i -1 where 1 < m + 1 (use the Caratheodory theorem). If v(X) 3! 0, the even stronger result, < m, holds. (Since v(X) is a boundary point of X, v(X) E XnH where H is a support plane of X at v(X) [26]. Because dimH = m-l, the Caratheodory theorem implies I < m.) Often, the representation (3.4) is not unique. If the set (x, *, xi } is not affinely independent, it is possible (see a proof of the Caratheodory theorem) to obtain a representation of the form (3.4) using a proper subset of {xl, xi *, l}. Thus, in (3.4) there is always a choice for l and (xl,''', x } such that {xl,, xi } is affinely independent. The support function of X, hx: R n - R, is defined by hx () = max x'-: x E X}. (3.5) We use sx (r) to denote any solution of (3.5). Specifically, sx (q) satisfies hx(q) = sx(8I)', sX(8) E X. (3.6) Since it is easy to prove that Ax = Ih,,x and sx = sox hcy(7) and s,,y(r,) can be determined by a simple enumeration of inner products: hcoy(r) = hy(1) = max{y: i 1,,v } (3.7) scoy(1) = sy(,)= Yij, yj', = hy(q). Thus, (3.7) provides a simple procedure for evaluating Ax and sX when X is the polytope coY. Computing the Distance 7

RSD-TR-26-86 It is apparent from Section 2 that the problem of finding dij = d(K;, Kj ) is -equivalent to the problem of finding v(K) where K is the (Minkowski) set difference Ki - Ki. For convenience we set i = 1, j = 2. If Kl and K2 are the polytopes coZ1 and coZ2, where Zk = {Zkl:=1,',Mk }, then K = K1- K2 is the polytope coZ, where Z= (zli - z2 il,. *'*, M1, j=l,'', M2}. Since Z has M1M2 elements, K is much more complex than either K1 or K2. This complexity appears in [21] and work of others who have used the set difference. Our algorithm for computing dl2 is stated in terms of K and requires only the computation of hK and sK. These data are particularly simple to evaluate. In fact, it is directly verified that hK(,) = hK1(,) + hK2(-'), SK (8) = SK()- SK2(-q). (3.8) For the polytope case, (3.7)-(3.8) show that the computational effort associated with hK and 8K is proportional to Ml + M2, not M1M2 as might first be expected. When the algorithm stops, it produces the following data I < m + 1, X > 0, zi EK, i=, -., I, $(K)= Xi';, d 12=I(K)I (3. ilIn the algorithm, the zi are obtained either from initial data taken from K1 and K2 or, the evaluation of SK (t) for various r/. Hence, zi = zli - z2i, t i E K, Z2i E K2 and (3.0) yields d12= IZ-Ze 1Z, zEK1, zK, K2, (3.10) where z - E z i, z,= E za. (3.11) i-I = - i= —1 With all these facts in mind we can put K1 and K2 aside and concentrate on the computation of v(K) from hK and sK. 8 Computing the Distance

RSD-TR-26-88 4. The Theoretical Algorithm We now present the iterative procedure for determining the near point v(K) of any compact convex set KCR'. When K is a polytope, it is shown that this procedure terminates after a finite number of steps. The basic idea is due to Barr and Gilbert [1], [2]: generate a sequence of polytopes contained in K such that their near points converge to >(K). It is necessary to compute the near points of these polytopes, but this is a relatively easy calculation since these polytopes will have at most m+l vertices. To state the algorithm, we first establish criteria for descent and optimality and a bound on approximation error. These results appear elsewhere (e.g. in [141 and [29]), but proofs are given since they are brief and insightful. Theorem 4.1: Consider a compact convex set K CR m and an arbitrary point x E K. Then: (1) if I r + hK(-x) > 0, there exists a point z E co{x, sK(-x)}CK satisfying Izz < Ixl; (2) x = -(K) if and only if I I f + hK (-x)= 0; and (3) I x - iK) I~ < | X i| + hK(-x). Proof: Result (1) is obvious if sK(-x) < x so assume lSK(-T) X2 ~I 1 and define z = x + X(sK(-x)- ), x =-( x l + hK (-x))/ I - SK (-x) 2. Note that 0 < X < 1/2 since I X - SK(-X) 2 >_ 2(1 1 2+ hKz(-x)). Hence, z E co(X, SK(-X)} and zI 2= I 2 - X( z + hK(-Z)) < 1xl2. To show result (2), first let IxlI+hK(-z)=O. Since Ii=- -hK (-x)=min{zx: zEK}, it is clear that 112 < I 1z2 + I z - X= z1 2+ 2(l x 1 2 -z ) < Izl 2 for all z E K. Therefore, x = v(K). Now let x = v(K) and assume I 2 X + hK (-x)> 0. Since result (1) implies x $7 v(K), we must have XI z i + hK (-X) ~ O. However, I z 12 + hK(-2) ~ 0 since min{z-2: z E K) < x 1 2. Therefore, Ix 1 2 + hK(-x) = 0. Result (3) follows since result (2) implies I v(K) I' < z.v(K) for all z E K and consequently I x - (K)2~ I 1< | 1 |- 2x(K) < I x I + hK(-X). Distance Algorithm. Given a compact convex set KCR m and v {1, ~ ~ ~, m+l}, perform the following steps: (1) select V0 = {yl,', y }CK and let k = 0; (2) determine k =v(coVk); Computing the Distance 0

RSD-TR-26-86 (3) if I v 1 + hK(-vk)= O, Set v(K)= vk and stop; (4) let Vk+l= VkU{sK(-k)} where k C Vk has m elements or less and satisfies k E co V k, increment k and proceed to step 2. If the algorithm does not stop in step 3, then Ivk 3L 0 and I vk I 2+ hK (-vk) > O. Hence, the existence of Vk in step 4 is guaranteed (see the comment following (3.4)), and Vk+l will always have m + 1 or fewer elements. Furthermore, descent in the next iteration is guaranteed since result (1) of Theorem 4.1 implies I vk+l I =I $coVk+l) I I (Co({Vk, sK(-Vk )}) < [ vk. The choice of VO is quite arbitrary. Ideally, v(coVo) v(K). In the absence of any such insight, a variety of single point initializations, such as the one in Section 6, may be used. For K C R" compact and convex, the algorithm generates a sequence (k } which converges to v(K). The proof follows from the same arguments used in the convergence proof for the method of Barr and Gilbert (see [1], [14]). The convergence proof is simpler when K is a polytope because v(K) is obtained after a finite number of steps. Theorem 4.2: Let K be the convex polytope coZ, where ZC R m is finite. If SK(?)) (E Z for all q E R m, the Distance Algorithm generates v(K) in a finite number of steps. Proof: Clearly, K = co(ZUVO) and ZUVoCRm is finite. Assume v(K) is not generated in N steps where N is the number of nonempty subsets of ZU V0. In this case, Ivk + hK(-vk) > 0 and I vk+lI < I Vk I for all 0< k < N. Since near points are unique Vk $4 Vl for any 0 < 1 < k < N, and since Vki C ZU VO for all 0 < k < N every subset of ZU Vo must have entered the algorithm. However, V(K) E coV for some VC ZUJVo (see Section 3). Therefore, v(K) = v(coVk) for some k < N. The requirement that 8K(1) E Z is easy to obtain even when K is the set difference of two polytopes. This is clear from (3.7) - (3.8) and the associated discussion. If sK(Iq) E Z and VOCZ, it follows from the steps of the algorithm that Vk CZ for all k > O. Thus, when the algorithm terminates, v(K) has a representation of the form (3.9) where the zi E Z. This observation is useful for initializing the algorithm in continuum problems. See Section 8. 10. Computing the Distance

RSD-TR-26-86 5. The Distance Subalgorithm Each iteration of the Distance Algorithm requires the determination of v(coY), Y=- {y, * *, y,}CR"m, 1 < v < m+l. In this section we describe a procedure originated by Johnson [171 for doing this. It is particularly efficient when m is small (say, m < 4) and yields a representation of the form v(coY) =' yi, EX' = 1, X > O, i I C(l,. -, v), iEI5 iEI Ys -{y: i E Is } C Y is affinely independent, where s indicates a particular member of the family of all nonempty subsets of Y. That such a representation exists follows from Section 3. Since m is small, it is effective to take a combinatoric approach where the = E,_ [v!/(r!(v-r)!)l subsets of Y are successively tested until a representation of the form (5.1) is found. Geometrically, this test involves checking the open subsets of the polytope coY (e.g., a vertex, an open line segment or an open face) to see if they contain Y(coY). If m-3, there are at most a = 15 such subsets to examine. To develop the approach, we first consider a simpler problem: the determination of Y(affYs ), Ys C Y. It is straightforward to solve for v(affYs ). If Y, is a singleton the solution is trivial, so assume Y, has r > 1 elements and let 21, * * *, x represent an arbitrary ordering of these elements. In this case, v(affYs ) —' l X' xi where X 1 - Ei-2 X' and the WX,. ~,X' E R result from the unconstrained minimization of f(X2,., Xr )= 1 2+ ixi (xi-xl) 1 2. Since f is con*vex, the necessary and sufficient conditions for optimality are f(X2,...,' )/iX' = 0, i=2,'', r. Consequently, X ER' solves the linear system As X=b, A ER'X', b ER' (5.2) where Computing the Distance 11

RSD-TR-26-86 1... 1 (z2 - *)21.. (X2 - 1)Xr 10 As(x,,.).,x,) =. (5.3) (Zr, -,1).1 Zr -..)r. To determine X, define Ai(Ys), i E Is as the cofactor of element As Ii (x Z, * * XZr ) where j satisfies x = y;I. This is notationally correct since one may show, using elementary row and column operations on the matrix As (X2, Z, r), that these cofactors are invariant with respect to the selected order of the elements of Y1. If we define A( Y ) as the determinant of As, then a first row expansion yields A(Ys) Ai(Ys). (5.4) If A( Y8) > O then the solution to the linear system (5.2) is unique, and expressing it by Cramer's rule yields (affYs) ) [ (Ys )/A( Ys )]y (5 5) iEl, This representation holds when Y, is affinely independent. Theorem 5.1: A( Ys) > 0 if and only if Ys is affinely independent. Proof: If to each row i > 1 of As we add the product of the first row times (xl -; )xl1, then it is clear that A(Y1) is equal to the determinant of QsTQs where Q = [(Z2 - XI)'..(Z - xZ)] E R mX(r-l) Thus, A(Ys) is the grammian of the vectors (Z2- Z1),''', (xZ - Z1) and (Z2- Xl),'', (r, - xl) are linearly independent if and only if A( Ys) > 0. By the sentences below (3.2) the proof is complete. We may efficiently calculate the near point to any affinely independent subset of Y using (5.4) - (5.5) and a recursive formula for the cofactors. This formula is developed by appending a row and column to As using xr+l = yj (j E If', I3' being the complement of Is in {1,'', v}) as additional data, 12 Computing the Distance

RSDoTR-2886-86 and expanding the cofactor A, (YI U% }) of this larger matrix about the first r elements in the appended row. The result- is given by Ai(Y U(Yi }) = A;(Y )y'(yk - Yi ), k E I, j E Is, (EIi (5.6) a~({y.})= 1, i=l,., v,. This equation is valid for any k E Is (i.e., Ay does not vary with k), so we set k = min{i E Is } to be definitive. When v=4, only 36 multiplies and 10 innerproduct evaluations are required to evaluate all the cofactors of all the subsets of Y. Moreover, these data coupled with the evaluation of A(Ys ), Ys C Y are all that is needed to determine the subset of Y required for (5.1). Theorem 5.2: Consider a finite set Y-{yl,'',y }CRR and a nonempty subset 1s C Y. Then v(coY) may be written in the form of (5.1) using Ys if and only if (1) A(Ys )> 0, (2) I A(Ys)> 0 for each i E Is and (3) Ai( Ys UYj }) < 0 for each y E Is'. Furthermore, v(coY)= A i (Ys)/(YS)] i (5.7) whenever Ys satisfies these conditions. Proof: It is geometrically obvious, and can be proved from (5.2) that y = (affYs ) if and only if y *(Y - Yk)= 0, k E 1S. (5.8) Let y = v(affs ), and suppose (1) and (2) are satisfied. Then from Theorem 5.1 and equations (5.4)-(5.5), y has the form of the right hand side of equation (5.1). In addition, (1), (3), and (5.5)-(5.6) imply y (k - Y) <, j E Is', k E Is. Using this and (5.8) we obtain y ( - Yi) < 0, i E {1,.,v}. Hence, for any x E coY we have x=;a';, a yi.' a 1 a' &> i E 1,,v, y-' (ye- z) =;,i"'-.; y (y - y;) < O. Therefore, by result 2 of Theorem 4.1, icoutY) = y. Computing the Distance 13

RSD-TR-26-86 We now show the converse. Assume y -= coY) is given by (5.1). Theorem 4.1, result 2, implies y - (y -yi) < O i E (1,',v}. Since X' > 0, i E Is and iEtl, X'y (y _-yi)=0, it is clear (y - -y)=0, i E Is and v(affYs) = y. However, Theorem 5.1 yields A(Ys) > 0. Therefore, the coefficients in (5.5) and (5.1) are unique, Xi = Ai(Ys)/A(Y,), i E I, and, since X' > 0, i E Is we have i(Ys) > O, i E Is. Finally, subtracting (5.8) from y' (y - y1) < O,j E {1,...,v) results in Y (yk -Y ) < 0, j EIs', k E I Hence, using A(Ys ) > 0 and equations (5.5)-(5.6), we must have j (YUs{Yj }) < o i E Is j Distance Subalgorithm. Given a finite set Y = {y,, } C R m, perform the following steps: (1) select an ordering Ys, s=, ~ ~ ~,u of all subsets of Y and set s = 1; (2) if A(Ys) > 0 and A(Ys) > 0, j E Is and Ai(YsUJ{Yi}) < 0 j E Is then calculate $(coY) using (5.7) and stop; (3) if s < a, increment s and proceed to step (1); (4) stop and indicate failure. From Section 3 we know v(coY) can be written in the form of (5.1). Thus, according to Theorem 5.2, there exists a Ys C Y, 1 < s < a, satisfying the conditions in step (2). Since the algorithm can evaluate every Y. C Y, it must terminate in step (2) with the correct value of v(coY) regardless of the order selected in step (1). If there are numerical errors in the computation of the data in step (2), it may turn out on rare occasions that the conditions of step (2) are not satisfied for 1 < s < a. We need to account for this possibility in the next section. Thus we have added step (4). Suppose we obtain (5.1) with v = m+l and Ys = Y. Then coY is a simplex and v(coY)E interior coY. Hence, i(coY)= O and there is a sphere of maximum radius, centered on the origin, contained in coY. The radius of this sphere is d-({O), coY) and it is given by the distance to the mn dimensional face of coy which is closest to the origin. Hence; d-({O},coY) = mint I v(affs)~ Ys: C Y has m elements }. (5.9) 14 Computing the Distance

RSD-TR-26-86 From y = v(affIY ), (5.5) and (5.8), 1 I v(affYs ) = I) (A(ys)- Y Ai(Y )y; yk )2, k E Is. (5.10) iE, The data A(Ys ), A;(Ys ), Yi Yk are all needed in the determination of v(coY) and require no additional computational effort. Thus (5.10) is evaluated with only m multiplies and one divide. If m =3, this means (5.9) takes 12 multiplies and 4 divides. When the distance algorithm of Section 4 stops with vk = 0 and Vk coY, where Y has m+l points, it is clear that coY C K and d'({0}, coY) in (5.9) is a lower bound in d' ({0}, K). The lower bound may be important in applications (see, e.g., Section 8) and is computationally inexpensive. 6. The Numerical Algorithm Having fully established the theoretical algorithm for computing the distance between compact convex sets, we now present modifications of the algorithm to make it totally reliable in the presence of round-off errors. This is followed by some comments on the efficient implementation of the algorithm. Errors do not accumulate in our algorithm since at every iteration k, Vk = V(coYk) results from the explicit evaluation of formulas which are only dependent on the set Yk. This helps the ultimate accuracy of the results and simplifies the error analysis. Inner product evaluations are one source of error. When K = coZ1 -coZ2, Zi = {Zij: j=l,;'',Mi }, i=1,2, we reduce these errors by moving the origin of the system to a point located on the line segment joining the centroids of the sets Z1 and Z2. That is, we replace Z, and Z2 by Zi {ii - Pc: j=l,,Mi }, i=1,2 where 1_ 1 P 2=(12l),sA LZ: A, i=l,2. (61) Since coZ - coZ2 = K, all our previous notations apply to the transformed problem. Computing the Distance 15

RSD-TR-268-88 Other sources of error include the evaluation of the sums in (5.4), (5.6)-(5.7) and step 3 of the Distance Algorithm. To account for these errors and those from the inner products, it is reasonable to replace the convergence criterion in step (3) by k 2 + hK (-k ) D2(K) (6.2) where E > 0 is related to floating-point accuracy and D(K) = max z z E K}. (6.3) Since E D2 is very small, result (3) of the Theorem 4.1 shows that the effect on the accuracy of the final result should be small. If K = coZ1 -coZ2 as in the last paragraph, and the origin of the system is translated as indicated, then the upper bound on D(K), given by D(K) _ D(coZ, - {z1}) + D(coZb - ({}) + I - Z2, (6.4) may be appropriately used in (6.2). When Zl and Z2 are dependent on q (see the paragraph containing (2.9)), the first two terms in (6.4) are independent of q and may be computed from the wUj which specify C,, i=-1,2. Numerical errors may also cause the Distance Subalgorithm to fail, especially when Y = Vk is affinely dependent or nearly so. For example, if yi, j E Is', is close to affYs, A (YS U({y}) is close to zero. If the numerical value of Ai (Y U{Y }) is positive when the actual value is negative, the exit through step (4) may occur. If the Distance Subalgorithm does fail, we resort to a Backup Procedure which always runs to completion. Given Y= {y, -'',y, }, the Backup Procedure determines v(coY) by evaluating v(affYs) for all Ys C Y such that A(y4) > 0, Aj(Ys) > 0, j E Is. Clearly, such Ys- are all candidates for the representation (5.1). The Backup Procedure merely picks the best of the Y and sets v(coY) - v(affY ): 16 Computing the Distance

RSD-TR-2 -86 $coY-) =argument mint j y: y = (aff1s), Y C Y, (6.5) A(x) >0, > ~, j E where Iv(aff ) I is calculated using (5.10). In most cases (6.5) involves more effort than the Distance Subalgorithm, but it always succeeds since A(Ys) > 0, A j(Ys) > 0, j E I when Y. is a single element of Y. The above comments lead to the following algorithm. Numerical Algorithm. Given a compact convex set K C Rm and v E (1,,m+l)}, perform the following steps: (1) select V0 = {yx, - *,yv,) C K and letk = 0; (2) set Y= Vk and apply the Distance Subalgorithm; if it succeeds set alg = DS, otherwise use the Backup Procedure (6.5) and set alg = BP; set =k = v(coY) and Vk -= where Y. satisfies (5.1); (3) if (6.2) holds, set v(K) = vk and stop; A (4) if (k —0 or [ vk I < | vk-l |) and (V has m elements or less), then let Vk+l = Vk U{SK(-Vk )}, increment k and proceed to step (2); (5) if alg = BP, set v(K) = vk, indicate the error tolerance (6.2) is not satisfied and stop; (6) if alg = DS, recalculate vk = (COVk) using the Backup Procedure (6.5), set alg - BP and proceed to step (3). It is easy to see that the algorithm always terminates, even if c = 0. If E is small but reasonable (say 100 X machine error), the algorithm generally stops in step (3) and rarely passes through steps (5) and (6). Entrance to steps (5) and (6) implies the occurrence of a numerical result which is inconsistant with theory. The condition, I vk I > I 1k-l, k > 1, contradicts the expected descent. Furthermore, by the design of the Distance Subalgorithm and the Backup Pro. A' cedure, Vk has m+l elements only if k -= 0. But Vk = 0 contradicts the failure of (6.2) which is necessary for entrance to step (5). The algorithm exits in step (5) only after both the Distance Subalgorithm and the Backup Procedure have been tried. Step (6) guarantees that the Backup Procedure is always tried Computing the Distance 17

RSD-TR-28-86 before stopping in step (5). In practice; the Distance Subalgorithm almost always succeeds and produ a near point of high accuracy. This is in part due to the structure of the num, cal algorithm. Theoretically, both the Distance Algorithm and the Backup F A cedure produce affinely independent sets Vk, and SK(-Vk) should be affin independent of V k*. Thus, the Vk, k > 1, should be affinely independent. El if VO is affinely dependent, or Vk, k 1, is nearly so, the Distance Algorit usually functions well. We have confirmed this independently of the numeri algorithm by extensive experimentation with the Distance Algorithm. When K is the set difference of two polytopes it is not obvious how the tial set VO should be chosen. We have tested a variety of schemes. In absence of additional information about K such as that described in Section the single point initialization Vo = ({K(-71 + Z2)} has worked as well as a Here, 1 - FZ is the direction between centroids (see (6.1)) and serves as a rot estimate of v(K). Note that the initialization is easy to compute using the F cedures outlined in Section 3. Attention to details in the implementation of the overall algorithm adds c siderably to its efficiency. For example, the inner products of the elements V k appear in the Distance Subalgorithm (or Backup Procedure) for b Y= Vk and Y= Vk+l and can be saved for the Y = Vk+l computati Hence, if Vk has v elements, only (v + 1) new inner products need to be cal lated when v(coVk+l) is determined. Another aid to efficiency is the choice of ordering in step (1) of the Dista Subalgorithm. The sets most likely to produce the near point should be put the beginning of the list. Some of the subsets of Y-= Vk have already b tested in Y = Vkl, and they are put at the end of the list (essentially, they eliminated). We have also found it effective to put one face of coVk at the hi of the list. It is Y1 C Y = Vk = Vkl1U{SK(-Vk-1)} such that Vk = YiU and y maximizes y s8K(-vk-l) over all y E Vk -l. Our experiences indicates t coY1 contains L(coVk ) about 80% of the time. The complete description of ordering procedure is too lengthy for inclusion here. 18 Computing the Distal

RSD-TR-2 6-86 7. Numerical Experiments The algorithm described in the previous section has been programmed as a Fortran subroutine and applied to a large number of examples in three space. Figure 2 summarizes the main results. The examples were generated by selecting 20 pairs of polytopes from a family of 12 polytopes. The members of the family were centered on the origin and were of varying size (contained in spheres of radius I to 4). They included: a line segment (M1=2), an equilateral triangle (M2=3), a rectangular box (M3=8), a truncated cone with hexagonal ends (M4=12), truncated cylinders with octagonal and decagonal cross sections (M5=16 and M6=20), and a collection of irregular polytopes generated by placing an equal number of vertices randomly in two parallel planes (Mi =20, 40, 50, 60, 100, 100). The twenty pairs selected were: (i, j) = (i, 2), (i, 4), (i, 5), (i, 10) with i=1, 3, 6, 8 and (7,9), (7,12), (11,9), (11,12). For each of the 20 pairs three cases were considered: polytopes separated, just touching, or intersecting. In each of the cases there were 100 different examples, generated by random translations and rotations of the two polytopes. For the separated cases the expectation of the relative translation between the two polytopes was 10/3. The just touching and intersecting examples were generated by appropriate translations of the polytopes along the line) joining the near points for the separated examples. The total number of examples was 6000. The examples were run on a Harris 800 computer. The machine precision is 10-11 and the parameter e was set equal to 10-. In every example the program ran to completion and did not require the use of Steps (5), (6), or the Backup Procedure. The accuracy of the final results as measured by I z + hK (-z) was excellent; typical values were in the order of 10-~. The actual number of operations (multiplies NM, adds NA, divides ND, and comparisons NC) were counted for each example. These were converted to equivalent flops, EF, by the following formula: EF = (tM NM A + tA ND + tC Nc)/(tM + tA ), (7.1) where the t's denote the times required for the operations. For the Harris the times in microseconds are: tM - 3.8, tA - 2.1, tD = 6.7, to = 1.7. For a Computing the Distance 19

RSD-TR-26-86 different machine, EF would be different because the relative times required for the operations would be different. However, the variation of EF from machine to machine should not be very great. The EF's plotted in Figure 2 are the averages over the 100 examples in each case. The approximate times in seconds for the Harris computer can be obtained by multiplying EF by 6X10-6. See the CPU scale in Figure 2. The results can be summarized as follows. For problems of moderate size, M = M + Mj < 40, the intersection cases are the most difficult. They require approximately 24 EF/M. For larger problems the just touching cases are most difficult, with EFIM ranging between 24 and 27. There is some evidence that EF/M grows slightly with M, but the increase is definitely less than log M. When the data for the three cases are averaged together the performance is more uniform with EF/M ranging between 14 and 19 for all values of M. Additional examples have been considered. When the algorithm is run on polytopes which are very near to each other the computational times become close to those for the just touching cases; but on the average, never do they take more time than the just touching case. When the polytopes are widely separated the times drop significantly, with EF/M < 7. Pairs of line segments were tried using the same cases and numbers of runs described above. The results for EF were: separated, 36; just touching, 39, intersecting, 96. For line segments the intersecting case (both segments contained in a common line) is truly pathological and should probably be discarded. It is interesting to compare our algorithm with the efficient algorithm developed by Lumelsky [221 for the special case of line segments. When his algorithm is arranged to produce the same results as ours, EF ranges between 38 and 40 (using the Harris time weights). Thus, our algorithm appears to be competitive even though it is designed to handle the general polytope problem. In general, one might expect the computational effort to be dependent on the shape of the objects and, for fixed M, Mi and Mi. In a variety of experiments which have been performed to test such behavior, some variation has been noted. But, it is not very great; about 25% at most. The fact that the effort is proportional to Mi + Mi is most encouraging. In combinatoric procedures it is proportional to M, Mi. 20 Computing the Distance

RSD-TR-26-86 8. An Example of Collision Detection In this section we consider an object which is continuously translated and rotated through a field of obstacles. Specifically, its position and orientation are given on a configuration space path defined by a continuous function q(s). The initial position corresponds to s = 0 and the terminal position to s = 1. To locate approximately the points of collision on the path the distances between the object and each of the obstacles is evaluated for s = t/T, where t and T are integers and t = 0, ~ ~ ~, T. If T is large, the collision points are located closely by the values of t where the distance just goes to zero. The computational time can be decreased by using the general initialization feature of the distance algorithm. Suppose, for instance, d 12(q(s)) has been determined for s = t/l T and the corresponding near points are given by (3.11). From the comments in Sections 3 and 4, it is reasonable to assume the z1i andl the z2i are points taken respectively from the finite sets Zl(q(s)) and Z2(q(s)) which generate K, = coZ1 and K. = coZ2. If T is large the geometry changes only slightly in one time increment and it is likely that the elements from Zt(q((t+l)/T)) and Z(q((t+l)/T)) which have the same indices as those in (3.11) for s = t/T can be used in (3.11) when s = (t+l)/T. Thus, the algorithm is started at s = (t+l)/T with V0 = (z1; - zi, i=-1, ~,l} where Z1i E Z1(q((t+l)/T)) and zi E Z(q((t+l)/T)) have the same indices as the elements in (3.11) from the previous stage. Of course, the X' change to account for the motion of the sets and the algorithm must determine these changes. But it does not have to spend time finding the points in (3.11). Even if new points must be found by the algorithm, the starting set VO is likely to be more effective than the single point initialization described in Section 6. Figure 3 shows a particular example. It was provided to us by John Canny who used it to demonstrate his quaternion technique [8] for computing collision times. The initial and terminal positions of the moving object, KA = K6UK7, together with the fixed objects, K1,... *,K5, are indicated. The configuration variables specifying the motion are the cartesian position and the quaternion representation of rotation given in [8]. The configuration variables vary linearly in a from the initial position to the terminal position. Figure 4 shows the results of the computations. The distances between KA and each of the five obstacles are denoted by d1, ~. -,ds. For all values of s it Computing the Distance 21

RSD-TR-26-86 turns out that d6i ~ d7 i, 1,5, so that di = d, i, = e,5. The computational times are shown in Table 1 for both the special initialization described above and the single point initialization of Section 6. The improvement due to the special initialization is significant, and as expected, gets better as T increases. Since Canny's algorithm is a root finding procedure on s, it locates the collision points precisely but does not determine the separation distances. His computational time is 11.6 seconds on a Symbolics 3600 computer. When ds = 0 or d = 0, we have tabulated the lower bounds on do and d-' produced by the algorithm. The absolute value of the negative distances in Figure 4 correspond to these lower bounds. It is not known how closely they estimate deay and d',s, but they do determine that significant collision penetrations have occurred. We have run a simple test problem where d{j(q(s)) can be obtained analytically. The computed lower bounds range from good to poor, and are best when d3j{q(s)) is not too large. Fortunately, this is the situation of greatest interest. Table 1. CPU times (Harris 800) in seconds for the example of Figure 3. Number of Intervals Time with Single Time with Ratio of in Grid (T) Point Initialization Special Initialization Times 10.22.13 1.7 100 2.00.69 2.9 1000 19.81 6.32 3.1 9. Conclusion We have presented an algorithm for determining the Euclidean distance between compact sets in R m. The emphasis has been on polytopes in Rs, since this is the single most important case in applications. Input data for the algorithm are in the form of finite sets of points whose convex hulls define the polytopes. This data format is particularly convenient in robotics applications where the position and orientation of the polytopes may be functions of configuration variables such as joint angles. Extensive numerical experience shows that the algorithm is efficient and reliable with a computational cost which is 22 Computing the Distance

RSD-.TR-26-88 approximately linear in the total number of points specifying the polytopes. The algorithm has some other special advantages. It provides the nearest points in the two polytopes. These are of direct interest and can also be used to compute the gradient of the distance with respect to the configuration variables. In continuum problems the algorithm may be initialized in a special way so that the computational time is significantly reduced. We have demonstrated this advantage in the collision detection problem, but it occurs in other applications too, such as the mapping of collision free regions in configuration space, pith finding and path planning. It has been noted in Section 2 that it is difficult to compute the translational distance d' for intersecting objects. Our algorithm provides, with essentially no additional cost, a lower bound on d'. The use of this lower bound in applications such as those just mentioned remains to be explored. Finally, a few comments should be made about sets which aren't polytopes or spherical extensions of polytopes. Suppose the algorithm is applied to the vertex sets of nonconvex polytopes. Then it is easy to see that it produces the distance between the convex hulls of the nonconvex polytopes. This distance is a conservative measure of collision and may be useful. When the distance between an infinite polyhedral cylinder and a polytope is computed, the computations are actually simplified: the vertex points are projected on a plane normal to the axis of the cylinder and the algorithm is applied in the plane (Re). In the case of general convex sets, it is necessary to have a procedure for evaluating the support function of the sets. This is easy to arrange for ellipsoids and some other special objects. The convergence is not finite, but the algorithm can be made, through the choice of E, to stop with a solution of specified accuracy. Prior experience with a similar algorithm [1] indicates that convergence rates for general sets should be good. 10. References [11 R.O. Barr, "An efficient computational procedure for a generalized quadratic programming problem," SIAM Journal Control, vol. 7, pp. 415-429, 1969. [2] R.O. Barr, E.G. Gilbert, "Some efficient algorithms for a class of abstract optimization problems arising in optimal control," IEEE Trans. Automatic Computing the Distance 23

RSD-TR-2 -86 Control, vol. AC-14, pp. 640-652, 1969. [3] J.W. Boyse, "Interference detection among solids and surfaces," Communications of the ACM, vol. 22, pp. 3-9, 1979. [4] R.A. Brooks, T. Lozano-Perez, "A subdivision algorithm in configuration space for findpath with rotation," IEEE Trans. on Systems, Man and Cybernetics, vol. SMC-15, pp. 224-233, 1985. [5] C.E. Buckley, L.J. Leifer, "A proximity metric for continuum path planning," Proc. 9th International Joint Conf. Art. Intelligence, pp. 1096-1102, 1985. [6] S.A. Cameron, "A study of the clash detection problem in robotics," Proc. IEEE International Conf. Robotics and Automation, pp. 488-493, 1985. [7] S.A. Cameron, R.K. Culley, "Determining the minimum translational distance between two convex polyhedra," Proc. IEEE International Conf. Robotics and Automation, pp. 591-596, 1986. [8] J. Canny, "Collision detection for moving polyhedra," MIT Artificial Intelligence Lab. Report No. 806, 1984. [9] F. Chin and C.A. Wang, "Optimal algorithms for the intersection and minimum distance problems between planar polygons," IEEE Trans. Computing, vol. C-32, pp. 1203-1207, 1983. [10] R.K. Culley, K.G. Kempf, "A collision detection algorithm based on velocity and distance bounds," Proc. IEEE International Conf. Robotics and Automation, pp. 1064-1069, 1986. [11] D.P. Dobkin, D.G. Kirkpatrick, "A Linear algorithm for determining the separation of convex polyhedra," Journal Algorithms, vol. 6, pp. 381-392, 24 Computing the Distance

RSD-TR-2- 86 1985. [121 B.R. Donald, "On motion planning with six degrees of freedom: solving the intersection problems in configuration space," Proc. IEEE International Conf. Robotics and Automation, pp. 536-541, 1985. [13] H. Edelsbrunner, "On computing the extreme distances between two convex polygons," Report No 96, Tech. Univ. of Graz, Graz, Austria, 1982. [141 E.G. Gilbert, "An iterative procedure for computing the minimum of a quadratic form on a convex set," SIAM Journal Control, vol. 4, pp. 61-80, 1966. [15] E.G. Gilbert, D.W. Johnson, "Distance functions and their application to robot path planning in the presence of obstacles," IEEE Journal Robotics and Automation, vol. RA-1, pp. 21-30, 1985. [16] E.G. Gilbert, S.S. Keerthi, D.W. Johnson, "A family of algorithms for determining the distance between convex sets," Report in preparation, Center for Research in Integrated Manufacturing, Univ. of Michigan, 1986. [17] D.W. Johnson, The Optimization of Robot Motion in the Presence of Obstacles, University of Michigan Ph.D. Dissertation in preparation. [18] D.W. Johnson, E.G. Gilbert, "Minimum time robot path planning in the presence of obstacles," Proc IEEE Conf. Decision and Control, pp. 17481753, 1985. [19] 0. Khatib, "Real-time obstacle avoidance for manipulators and mobile robots," International Journal Robotics Research, vol. 5, pp. 90-98, 1986. [20] D.T. Lee, F.P. Preparata, "Computational geometry - a survey," IEEE Trans. Computing, vol. C-33, pp. 1072-1101, 1984. Computing the Distance 25

RSD-TR-26-86 [21] T. Lozano-Perez, "Spacial planning: a configuration space approach," IEEE Trans. Computing, vol. C-32, pp. 108-120, 1983. [22] V.J. Lumelsky, "On fast computation of distance between line segments," Info. Proc. Letters, vol. 21, pp. 55-61, 1985. [23] W. Meyer, "Distances between boxes: applications to collision detection and clipping," IEEE International Conf. Robotics and Automation, pp. 597-602, 1986. [24] M. Orlowski, "The computation of the distance between polyhedra in 3space," SIAM Conf. Geometric Modelling and Robotics, Albany, NY, July 1985. [25] W.E. Red, "Minimum distances for robot task simulation," Robotica, vol. 1, pp. 231-238, 1983. [26] R.T. Rockafellar, Convex Analysis, Princeton Univ. Press, Princeton, NJ, 1970. [27] J.T. Schwartz, "Finding the minimum distance between two convex polygons", Inform.Process.Lett., vol. 13, pp. 168-170, 1981. [28] J.T. Schwartz, M. Sharir, "On the piano movers problem I, the special case of a rigid polygonal body moving admidst polygonal barriers," Comm. Pure Appl. Math., vol. 36, pp. 345-398, 1983. [29] P. Wolfe, "Finding the nearest point in a polytope," Mathematical Programming Study, vol. 11, pp. 128-149, 1976. 26 Computing the Distance

RESD-TR-2-86 A =~~~~ 1\ Z 1 d = d4 4~K K3 K5 K2 Figure I An example of object representation Computing the Distance 27

RSD-TR-26-86 EF CPU(10-3 secs.) 6000 35.4 *v Collision o Just Touching * Near 5000 -29.5 4000 - 23.6 3000 - o 17.7 0 * 2000 - 11.8 o * oo 0 0 o0 0 1000 o * 5.90 50 o * * s oo 01 OW iI I I I 0.00 0 40 80 120 160 onn M Figure 2 Equivalent flops (EF) and CPU times vs. total number of vertices (M). Each data point is the average of 100 randomly generated examples. 28 Computing the Distance

RSD-TR-26-86 s =, K7 K, K2 Figure 3. The example of collision detection. ~~~z~~~~~~~~~ d2 141'0. 0 0.20. 0.40 0. 80 so. e80 S Figure 4. Results for the examples in Figure 3. The d; are the distances between KA and the Ki. Computing the Distance 29

CD -Z 2n z 01 co ON) Oo'" C_ o 00 n N) 0 01 CC) C Co0 Co CA) To-iat_