Computing the Minimum Directed Distances Between Convex Polyhedra

Transcription

1 JOURNAL OF INFORMAION COMUING SCIENCE DISANCES AND ENGINEERING BEWEEN CONVEX 15, OLYHEDRA (1999) 353 Computng the Mnmum Drected Dstances Between Convex olyhedra Department of Electrcal Engneerng Natonal awan Insttute of echnology ape, awan 106, R.O.C. Gven two dsjonted objects, the mnmum dstance (MD) s the short Eucldean dstance between them. When the two objects ntersect, the MD between them s zero. he mnmum drected Eucldean dstance (MDED) between two objects s the shortest relatve translated Eucldean dstance that results n the objects comng just nto contact. he MDED s also defned for ntersectng objects, and t returns a measure of penetraton. Gven two dsjonted objects, we also defne the mnmum drected L dstance (MDLD) between them to be the shortest sze ether object needs to grow proportonally that results n the objects comng nto contact. he MDLD s equvalent to the MDED for two ntersectng objects. he computaton of MDLD and MDED can be recast as a Mnkowsk sum of two objects and fnshed n one routne. he algorthms developed here can be used for collson detecton, computaton of the dstance between two polyhedra n three-dmensonal space, and robotcs path-plannng problems. Keywords: mnmum dstance, mnmum drected dstance, Mnkowsk sum, collson detecton, path plannng 1. INRODUCION Determnng the mnmum dstance between two convex polyhedra s an mportant problem n robotcs, mage processng, CAD systems, computatonal geometry, and other areas of nformaton processng whch deal wth geometrcal data. All work that s developed for automated path plannng requres at ts lowest level the ablty to detect whether or not collson has occurred. he ablty to compute dstance and nterference effcently wll result n a substantal reducton n the overall tme requred for most path-plannng algorthms. In general, a path plannng algorthm needs to ascertan for any poston n the workspace not only f a collson has occurred, but also how close t s to occurrng f t has not. For nstance, a generate-and-test type path-plannng algorthm requres tests for whether confguraton and trajectory are collson-free, and methods for searchng ntermedate subgoals [2]. Several technques have been reported for calculaton of the mnmum dstance between convex polyhedra. In partcular, Glbert, et al. [5] defned an object by means of the convex hull of ts vertces and provded a quck procedure to compute the dstance between 3D objects. her approach performs an teratve sequence of dstance mnmzatons to obtan elementary subsets of the orgnal shape untl a subset contanng the global mnmum s attaned. Bobrow [1] proposed an approach whch casts the problem as a constraned Receved January 4, 1997; accepted September 5, Communcated by Youn-Long Ln. 353

2 354 nonlnear mnmzaton. Hs algorthm uses a drect approach to mnmze the nonlnear dstance functon, whch generates a sequence of search drectons along the surfaces of the objects to obtan the global mnmum. he computaton tme of ths algorthm s roughly lnear wth the number of faces. Ln and Canny [6] proposed an ncremental dstance calculaton startng wth a canddate par of features, one from each polyhedron, that checks the closest ponts that le n these features. he algorthm then steps to the next closest par of features untl the closest ponts are found. he mnmum dstance between three-dmensonal segments was developed n [8]. he computatonal effcency of two-dmensonal algorthms for polygons was gven n [4,10]. he dstance between boxes was consdered n [9]. he above mnmum dstance algorthms are asymptotcally fast, but they only return a zero value for ntersectng objects. Dstance s used as a measure of how far a robot part s from colldng wth an obstacle. When the two objects ntersect, the dstance between them s zero. hs gves no nformaton about the ntensty of the ntersecton. he general objectve of a penetraton measure s to quantty the depth of ntersecton for object modelng. A mnmum drected Eucldean (or translated) dstance was proposed n [2,3] to defne the ntensty of penetraton and uses the negatve of the mnmum Eucldean dstance by whch the two overlappng objects must be relatvely translated so as to have no nteror pont n common. he mnmum drected Eucldean dstance s equvalent to the dstance between the two objects f the objects are dsjonted. Buckley [2] proposed a method to compute the mnmum drected Eucldean dstance between two-dmensonal convex polygons and used ths measure n a penalty approach n collson-avodance robot moton plannng. he result shows that the nonntersecton constrant provdes useful nformaton n the case of body ntersecton, and that path plannng can be done accordng to the flexble trajectory paradgm. Cameron & Culley [3] appled the Mnkowsk sum technque to compute the mnmum translated dstance between two convex polyhedra n a three-dmensonal space. However, the complexty and completeness of ther algorthm was not analyzed. As one step toward reducng the number of computatons needed for path plannng, the am of ths research s to present effcent algorthms to compute the mnmum drected dstance functons between two convex polyhedra. he polyhedral representaton of 3D objects s wdely used n robotcs and computer graphcs research. We defne a new mnmum drected L dstance (MDLD) between convex polyhedra and derve ts relatonshp wth the mnmum drected Eucldean dstance (MDED) proposed n [2,3]. In ts smple form, the measure of the L dstance between a pont and a convex polyhedron s the maxmum of the dstances of the pont from the half-space whch passes through the faces of the polyhedron. Our computaton of the MDLD and MDED functons and the Mnkowsk sum of two convex polyhedra are based on the boundary model of the polyhedron. he followng s an outlne of the paper and ts contents. In Secton 2, the mnmum drected L dstance s defned, and ts propertes as well as ts connecton wth the mnmum drected Eucldean dstance are also derved. Effcent algorthms for computng both MDLD and MDED are proposed n Secton 3. Several examples and applcatons are llustrated n Secton 4. Fnally, Secton 5 provdes conclusons wth regard to the proposed approach.

3 COMUING DISANCES BEWEEN CONVEX OLYHEDRA ROBLEM FORMULAION AND RELIMINARIES We wll assume that two objects and A are convex polyhedra n R r space for further dscusson. When r = 2, objects and A are two-dmensonal convex polygons, and when r = 3, objects and A are three-dmensonal convex polyhedra. Some termnology wth regard to polyhedral objects used n ths paper are ntroduced here. A convex polyhedron n three-dmensonal space s characterzed by ts faces, edges, and vertces. A face of a threedmensonal polyhedron s a 3D convex polygon. A plane n 3D ndcates a set whch satsfes the plane equaton, n x+d = 0. A lne means a straght lne of nfnte length passng through two ponts a and b, and lne segment ab ndcates a segment of a lne connectng two end ponts a and b. An edge s a lne segment connectng two vertces. For twodmensonal polygons, the terms face and edge are usually nterchangeable. he closest par of features between two objects s defned as the par of features whch contan the par of closest ponts between objects. he par of closest ponts means the mnmum Eucldean dstance ponts between two objects. Let p (p Œ R r ) be the translatonal vector of the reference pont of polyhedron. A convex polyhedron wth m faces can be represented by the set where = {x Œ R r ÍN (x p) + d 0}, N = [n 1,..., n m ] Œ R r m, and d = [d 1,..., d m ] Œ R m. Matrx N s an r by m matrx whose th column n s the unt outward normal vector of the th face of polyhedron. Vector d s an m dmensonal vector, and d s the magntude of the vector from reference pont p perpendcular to the th face, measured n the negatve n drecton. Let the vertces of be represented by U = [u 1,..., u k ] Œ R r k, where k s the number of vertces, and u 1,..., u k are the vertex translatonal vectors relatve to the reference pont of. A grown convex polyhedron of convex polyhedron s defned as = {x Œ Rr ÍN (x p) + d s, s Œ R m and s 0}. and a proportonally grown polyhedron e (e Œ R, e 0) of convex polyhedron s defned by e = {x Œ R r ÍN (x p) + d [1,..., 1] e}. Smlarly, a convex polyhedron A wth n faces and l vertces s represented by the set where A = {x Œ R r ÍA (x q) + b 0},

4 356 A = [a 1,..., a n ] Œ R r n, b = [b 1,..., b n ] Œ R n. Moreover, q (q Œ R r ) s the translatonal vector of the reference pont of A, and ts vertces are represented by V = [v 1,..., v l ] Œ R r l, where v 1,..., v l are the vertex translatonal vectors of A relatve to ts reference pont. he followng defntons and notatons are used n the paper and are defned below. Defnton 1: he maxmum value of vector y, max(y), s gven by max(y) = max{y 1,..., y n }, y Œ R n. Defnton 2: he nteger functon arg{max(y)} s the ndex (or argument) such that y = max(y). Defnton 3: he mnmum value of vector y, mn(y), s gven by mn(y) = mn{y 1,..., y n }, y Œ R n. Defnton 4: he nteger functon arg{mn(y)} s the ndex (or argument) such that y = mn(y). -: polygon whose vertex vectors are the negatons of the vertex vectors of when the reference pont of s at the orgn. A : Mnkowsk sum of and -A, and t s the set of translatons of the reference pont q of Q that brng t nto nterference wth and has ts new reference pont at pont p,.e. A = {q : «A π }. d(x, ): mnmum dstance (MD) between pont object x and object, and dx (, )= mn x y 2 y d (x, ): mnmum drected L dstance (MDLD) between a pont object x and an object, and d (x, ) = max(n (x - p) + d) = -mn(-n (x - p) - d). A contour of d (x, ) = e gves all the boundary faces of e for e greater than zero. When x œ, MDLD d (x, ) (= max(n (x p) + d ) > 0) s the shortest sze that needs to grow proportonally, resultng n pont x beng on the boundary of e, and that resembles an L - norm dstance. When x Œ, d (x, ) (= -max(-n (x - p) - d ) 0) s the negatve of the shortest dstance that x needs to translate, resultng n pont x just touchng the boundary of. Note that d (x, ) Á x=q = max(n (q - p) + d ) = max(-n (p - q) + d ) = d (x, c )Á x=p, where c = {x Œ R r Í-N (x - q) + d 0} s the Mnkowsk sum of pont object q and object. d 2 (x, ) : mnmum drected Eucldean dstance (MDED) between pont x and object, and d (, d ) ( x, ) x 2 x = d ( x, ) x. {

5 COMUING DISANCES BEWEEN CONVEX OLYHEDRA 357 Examples of MD, MDLD and MDED between pont object x and convex polygon n two-dmensonal space are shown n Fg. 1. MDLD d (x, ) returns the orthogonal dstance from pont x to the half-space determned by arg{max(n (x - p) + d )}, and d 2 (x, ) returns the mnmum dstance from pont x to the correspondng closest face. Assume that the orthogonal projecton of pont x to the closest plane s denoted by x *. If pont x * s on the nearest face, then d 2 (x, ) = d (x, ); otherwse, d 2 (x, ) > d (x, ). hus, n general, we have the relatonshp d 2 (x, ) d (x, ). In other words, d (x, ) determnes only the dstance between the closest par of features of a pont and a face. However, d 2 (x, ) determnes the dstance between the closest par of features n all cases, ncludng a par of a pont and an edge, and a par of a pont and a vertex. MDED = MD x MDED = MDLD < 0 MDLD MD = 0 x (a) x (b) x Fg. 1. Geometrcal llustraton of MD, MDLD, and MDED between a pont object x and a convex polygon n two-dmensonal space; (a) pont x s outsde of, and (b) pont x s nsde of. We can now defne the mnmum drected dstances (MDD) between two convex polyhedra A and. In concept, the dstance problem for two objects A and can be reduced to the problem of fndng the dstance from a pont object and the Mnskows sum A. d(a, ): mnmum dstance (MD) between convex polyhedra A and, and da (, ) = mn x y = d( x, A ) =. 2 x q x A, y d (A, ): he mnmum drected L dstance (MDLD) between convex polyhedra A and, d (A, ), s gven by d (A, ) = d (x, A )Á x = q. Note that d (A, ) = d (x, A )Á x = q = d (x, A )Á x = p = d (, A), (1) where A s the Mnkowsk sum of A and -. hus, d (A, ) s a symmetrc dstance functon. Fg. 2 shows an example of Eq. (1) n 2D. When A and are dsjonted, d (A, ) s the shortest sze ether object needs to grow proportonally that results n the objects beng n contact and, hence, returns a measure of dstance between A and. In the two-dmensonal case, d (A, ) can be geometrcally nterpreted as

6 358 q d ( A, ) A A p (a) d (a) d ( A,) = d ( q, A ) (A, ) = d (q, A ) A q A d (, A) p (b) d (b) d (, A) = d ( p, A p ) (, A) = d (p, A p ) Fg. 2. Illustraton of d (A, ) = d (x, A )Á x = q = d (x, A )Á x = p = d (, A), n 2D; (a) d (A, ) = d (q, A ), and (b) d (, A) = d (p, A ). d (A, ) = max{e A, e }, for A «=, (2) where e A s the smallest value of e such that A e and ntersect, and e s the smallest value of e such that A and e ntersect. If the objects overlap, d (A, ) s the negatve of the shortest dstance needed to translate one object wth respect to the other untl there s no ntersecton. Fg. 3 shows the geometrcal nterpretaton of MDLD n 2D. d 2 (A, ): mnmum drected Eucldean dstance (MDED) between convex polyhedra A and, and da (, ) fa d2( A, ) = =. d ( A, ) f A Examples of MDED are also shown n Fg. 3. One computatonal dffculty n the above formulaton s that the Mnkowsk sum generally has a very complcated shape n hgher dmensons. Nevertheless, not every face of the set A needs be consdered to fnd the mnmum dstance. hs s the focus of the followng secton.

7 COMUING DISANCES BEWEEN CONVEX OLYHEDRA 359 A ε A ε A ε MD ε (a) (a) and and A A and are dsjonted A MD = 0 MDED=MDLD<0 (b) and A are overlappng (b) and A are overlappng Fg. 3. Geometrcal nterpretaton of MDLD, MD, and MDED between two convex polygons A and n 2D; (a) dsjonted case, and (b) overlappng case. 3. MDD ALGORIHMS In ths secton, we shall develop effcent algorthms for computng the mnmum drected dstance functons for two convex objects. We shall frst develop the boundarymodel approach to represent the Mnkowsk sum A, whch s most sutable for computng the proposed mnmum drected dstances between convex polyhedra. If A and are convex polyhedra, then set A s also a convex polyhedron, wth ponts on ts boundary representng the confguraton wth whch objects A and come nto contact. he faces of A correspond to modes of contact between the objects such that they can slde along each other, and these modes can be classfed as (1) face-vertex (FV) contact, (2) vertex-face (VF) contact, and (3) edge-edge (EE) contact. hs set of three types of nteracton cover all possble types of contact between polyhedra. Other forms of contact are specal cases. For nstance, an edge-face contact requres at least two contacts from among the above three contact types. For FV contact cases, a supportng face of A s obtaned when a face of sldes aganst a vertex of A. Let

8 360 n ( x p) + 0 (3) d be the half-space correspondng to a face of ; then, the vertex of A wth a maxmum value of ( n vj ) for j = 1,..,l, can slde aganst (see Fg. 4(a)), where l s the number of vertces of A. hus, the set A should satsfy n ( x p) + d e, = 0, 1,..., m 1, (4) where e = max{ nv, j = 1,..., l} = mn( V n ), j and m s the number of faces of. For each th face of, let j = arg(mn(v n )); the th face of and jth vertex of A are called a face-vertex (FV) contact par, and the correspondng mnmum dstance d between them s δ = n ( q p) + d + mn( V n ) = n ( q p) + d e. (5) he contact par whch has a maxmum value of d n Eq. (5) among all possble face-vertex contact pars s called a face-vertex closed par. For VF contact cases, a supportng face of A s obtaned when a face of A sldes aganst a vertex of (see Fg. 4(b)). Let n 1 ( x-p ) + d 1= e 1 n 1 ( x-p ) + d 1= 0 n 1 n 1 p n 1 h 1 q A h h 3 2 e 1 e 1 (a) FV contact -n 1 h2 = = max{ -n 1 h 1 -n 1 h2 -n 1 h 3 } u 1 u 2 p u 3 f 2 a 2 -b 2 a 2 ( x-q ) + = 0 A q b 2 -a 2 ( x-p ) + b 2 = f 2 f 2 -a 2 a 2 = - u 3 = max{ - u 1 - u 2 - u 3 } a 2 (b) VF contact Fg. 4. Illustraton of the supportng faces of Mnkowsk sum A generated by (a) face-vertex contact, and (b) vertex-face contact. a 2 a 2

9 COMUING DISANCES BEWEEN CONVEX OLYHEDRA 361 a ( x q) + 0 b (6) correspond to a face of A; then, the vertex of wth a maxmum value of ( a uj ) for j = 1,.., k, can slde aganst A, where k s the number of vertces of. hus, A should also satsfy where a ( x p) f b, = 0, 1,..., n 1, (7) f = max{ a u, j = 1,..., k} = mn( U a ), j and n s the number of faces of A. For each th face of A, let j = arg(mn(u a )); the th face of A and jth vertex of are called a vertex-face (VF) contact par, and the correspondng mnmum dstance d between then s δ = a ( p q) + b + mn ( U a ) = a ( p q) + b f. (8) he contact par whch has a maxmum value of d n Eq. (8) among all possble vertex-face contact pars s called a face-vertex closed par. he three-dmensonal case s exactly analogous for the FV and VF contacts n the two-dmensonal case. he EE contact mode occurs only n the three-dmensonal case. In EE contact cases, not every par of edges wll generate a supportng face of A. Let u a u b be an edge of, and let v g v l be an edge of A; then, edges u a u b and v g v l can come nto contact f there exsts a normal vector n (see Fg. 5) n = uu uu α β γ λ uu α β uu (9) γ λ 2 n n = e A x e p A q e A e 2 e p n. ( x-p ) = e 1 + e 2 e 1 p Fg. 5. Illustraton of the supportng faces of the Mnkowsk sum A generated by edge-edge contact n 3D.

10 362 such that and e 1 = n u a = n u b = max{n u j, j = 1,..., k} 0 e 2 = -n v g = -n v l = max{-n v, = 1,..., l} 0. In addton, the supportng face generated by the EE contact s the half-space represented by n (x - p) e 1 + e 2. (10) hese two edges become an edge-edge contact par, and the mnmum dstance d between the two lnes u a u b and v g v l s d = n (q - p) - e 1 - e 2. (11) he edge-edge par whch has a maxmum value of d among all possble edge-edge contact pars s called an edge-edge closed par (named edge of and edge j of A). For two-dmensonal convex polygonal objects, we only need to consder the FV contact for all faces of and the VF contact for all faces of A; thus, an analytcal form of the set A can be obtaned. For cases of FV contact, we obtan the grown object : = {xœr r ÍN (x - p) + d - e 0}, where m e = max{ nv, j = 1,..., l} = max( Vn), e R. j For the VF contact case, we obtan the grown object A c : where A c = {xœr r Í-A (x - p) + b - f 0}, n f = max{ au, j = 1,..., k} = max( Ua), f R. j he set A s, thus, the ntersecton of and A c and s the set r A = R + x N A ( x p) d e b f 0. (12) In addton, the set A s, then, the set represented by A p r = R + x N A ( d e x q ) b f 0. (13)

11 COMUING DISANCES BEWEEN CONVEX OLYHEDRA 363 Fg. 6 shows an example of the two-dmensonal Mnkowsk sum A obtaned by the boundary-model approach. For the three-dmensonal case, the expresson n (12) s only a good approxmaton for A. a 2 h 1 h2 h 3 q A a 1 n 2 u 1 u 2 p n 1 u 3 a 3 n 3 (a) (a) Object Object and and AA -a 3 u1 -n 2 h 3 -a1 u2 n 1 -A q p n - h n 1 h2 -a2 u3 A c (b) (b) Mnkowsk Mnkowsk sum sum A A Fg. 6. An example of the two-dmensonal Mnkowsk sum A ; (a) the objects A and, and (b) A obtaned by the boundary-model approach. For two dsjonted convex polygons n 2D, there are three possble types of closest pars of features: (1) a par of vertces, (2) a vertex and an edge, and (3) a par of edges. Case (3) occurs when the closest par of features s a par of parallel edges, and the mnmum dstance between them s easy to obtan. Case (2) (a vertex and an edge) s mportant

12 364 because t covers case (1) (a par of vertces). Algorthm MDD2, whch computes the MDLD, MD, and MDED functons for two-dmensonal convex polygons, works n the followng way. Frst, t computes the MDLD functons between pont q and objects for and A c, and searches for the closest par of features from the vertex-face closed par and the face-vertex closed par. If objects A and are overlappng, there s no need to contnue. Otherwse, t fnds the closest par of features from the face-vertex closed par and vertex-face closed par, and then checks whether the closest par of features s a par of vertces or not. Detals of algorthm MDD2 are stated below. Algorthm MDD2: Mnmum drected dstances for two convex polygons A and. Input: convex polygons A and Output: MD d(a, ), MDLD d (A, ), and MDED d 2 (A, ) Intal: x = q ; reduce A to a pont Step 1: Compute d (q, ) ; face-vertex closed par Step 2: Compute d (q, A c ) ; vertex-face closed par Step 3: d (A, ) = max{d (q, ), d (q, A c )} If (d (A, ) 0 ), then ; overlappng case d 2 (A, ) = d (A, ) d(a, ) = 0 otherwse, f (d (q, ) > d (q, A c )), then ; dsjonted case = arg{max(n (q - p) + d - e)} j = arg{mn(v n )} d(a, ) = d(vertex j of A, edge of ) ; face-vertex closed par d 2 (A, ) = d(a, ) or f (d (q, ) < d (q, A c )), then = arg{max(-a (q - p) + b - f)} j = arg{mn(u a )} d(a, ) = d(vertex j of, edge of A) ; vertex-face closed par d 2 (A, ) = d(a, ) or f (d (q, ) = d (q, A c ), then ; dsjonted case = arg{max(n (q - p) + d - e)} j = arg{max(-a (q - p) + b - f)} d(a, ) = d(edge of, edge j of A) ; parallel edges d 2 (A, ) = d(a, ) Assume that an operaton means an nner-product of two vectors n R r space. he frst step n Algorthm MDD2 requres (m+ml) operatons, the second step requres (n+nk) operatons, and the last step requres four operatons to solve the smple problem of comput-

13 COMUING DISANCES BEWEEN CONVEX OLYHEDRA 365 ng the mnmum dstance between a pont and a lne segment. hus, the computatonal complexty of MDD2 s O(ml+nk). he complexty can be further reduced to O(m+n) as wll be dscussed next. he search for m FV contact-pars n Step 1 of MDD2 can be done n lnear tme for convex polygons and A as follows. Assume that the th face of and the jth vertex of A s a FV contact-par; then, t s requred that or nv j nv t, t = 0, 1,..., n 1, n ( vt vj) 0, t = 0, 1,..., n 1, where n s the number of vertces of polygon A. Because of the convexty of polygon, the above nequalty constrants can be reduced to the followng two constrants: and n ( v j vj) 1 0 n ( v j+ vj) 1 0 he procedure of algorthm FVC for searchng m FV contact-pars s gven n detal n the followng. Algorthm FVC: Input: wo convex polygons and A, where and A contan m and n faces, respectvely. Output: m FV contact-pars. Step 1: {Fnd the FV contact-par of n 0.} 1.1 regster FV = for t = 0 to n-1 do 1.3 FV = max{fv, n 0 v t }; 1.4 f FV = n 0 v t then s = t; 1.5 end for Step 2: {Fndng the FV contact-par of n, where = 1, 2,..., m 1.} 2.1 t = s 2.2 for = 1 to m 1 do 2.3 whle n (v t-1 - v t ) < 0 or n (v t+1 - v t ) < 0 do 2.4 t = (t + 1) mod n; 2.5 end whle 2.6 end for he Algorthm FVC for searchng m FV contact-pars conssts of two major steps. In the frst step, we fnd the contact-par of n 0 of by searchng each vertex of A. hus, ths step takes O(n) tme to fnd max{ n 0 v t, t = 0, 1,..., n 1} for the face n 0 and ts correspondng contact-par v s, where s = arg(max{ n 0 v t, t = 0, 1,..., n 1}). In the second step, we utlze the property descrbed above to fnd the contact-par of the faces n 1, n 2,..., n n 1 wth ther correspondng contact-pars smply by sequentally searchng the vertces v s, v s+1,..., and v s n ccw order. hs step has O(m) tme to fnd the contact-pars of faces n 1, n 2,..., n n 1 of. herefore,

14 366 the algorthm FVC for computng the FV contact-pars has O(n + m) tme complexty. For the VF contact case, the n VF contact-pars can be computed n a smlar way. hus, the procedure used to compute the FV and VF contact-pars of two polygons and A has O(n + m) tme complexty. herefore, the computatonal complexty of MDD2 s reduced to O(n + m). For two nonoverlappng convex polyhedra n three-dmensonal space, there are sx possble types of closest pars of features: (1) a par of vertces, (2) a vertex and an edge, (3) a vertex and a face, (4) a par of edges, (5) an edge and a face, and (6) a par of faces. Cases (5) and (6) occur when the closest par of features s two parallel faces and/or edges. Cases (3) and (4) are most mportant for computng the MDLD functon. Cases (1) and (2) are degenerated cases of Cases (3) and (4) and only need to be consdered n computng the MD and MDED functons. he MDD3 algorthm whch computes MDLD, MD, and MDED for two three-dmensonal convex polyhedra A and works n a way smlar to that of algorthm MDD2 except that t consders addtonal edge-edge pars, and t s lsted below. Algorthm MDD3: Mnmum drected dstances for two convex polyhedra A and. Input: convex polyhedra A and Output: MD d(a, ), MDLD d (A, ), and MDED d 2 (A, ) Intal: x = q ; reduce A to a pont Step 1: Compute d (q, ) ; face-vertex closed par Step 2: Compute d (q, A c ) ; vertex-face closed par Step 3: Compute d max among edge-edge pars ; edge-edge closed par Step 4: d (A, ) = max{d (q, ), d (q, A c ), d max } If ( d (A, ) 0 ), then ; overlappng case d 2 (A, ) = d (A, ) d(a, ) = 0 otherwse, f (d (q, ) > d (q, A c ), d max ), then ; dsjonted case = arg{ max( N (q p) + d e)} j = arg{ mn(v n ) } d(a, ) = d(vertex j of A, face of ) ; face-vertex closed par d 2 (A, ) = d(a, ) or f (d (q, A c ) > d (q, ), d max ), then = arg{ max ( A (q p) + b f)} j = arg{ mn(u a ) } d(a, ) = d(vertex j of, face of A) ; vertex-face closed par d 2 (A, ) = d(a, ) or f (d (q, A c ) = d (q, ) > d max ), then = arg{ max (N (q p) + d e)} j = arg{ max( A (q p) + b f)} d(a, ) = d(face of, face j of A) ; parallel faces d 2 (A, ) = d(a, ) or f (d max > d (q, ), d (q, A c )), then d(a, ) = d(edge of, edge j of A) ; edge-edge closed par d 2 (A, ) = d(a, ) or f ( d max = d (q, ) > d (q, A c )), then = arg{ max (N (q p) + d e)}

15 COMUING DISANCES BEWEEN CONVEX OLYHEDRA 367 d(a, ) = d(edge j of A, face of ) d 2 (A, ) = d(a, ) or f (d max = d (q, A c ) > d (q, )), then j = arg{ max ( A (q p) + b f)} d(a, ) = d(face j of A, edge of ) d 2 (A, ) = d(a, ) ; parallel face and edge ; parallel edge and face he above algorthm frst computes the MDLD functon between pont q and set and searches for the vertex-face closed par. Second, t computes the MDLD functon between pont q and set A c and searches for the face-vertex closed par. It then fnds the maxmum value d max of the mnmum dstances n Eq. (11) among possble c edge-edge pars whch are generated durng the frst two steps, as shown n Fg. 7. he edges can become a contact par f each les n the boundary of ts face-vertex closed par and vertex-face closed par found durng Steps 1 and 2. If objects A and are overlappng, then t stops. Otherwse, t fnds the closest par of features among the closed pars found n Steps 1, 2, and 3, and then checks whether the closest par of features s a par of vertces or a par contanng a vertex and an edge. vertex-face closed par face-vertex closed par A Fg. 7. ossble canddate edge-edge pars for computng MDLD. he canddate edge-edge par les on the boundary of ts face-vertex closed par and vertex-face closed par. For the MDD3 algorthm, the frst step requres (m+ml) operatons, the second step requres (n+nk) operatons, and the thrd step requres around c(l+k) operatons. he last step requres many fewer operatons than does Step 3 to compute the mnmum dstance between a pont and a 3D convex polygon or between two lne segments [8]. he problem of fndng the mnmum dstance between two segments can be transformed nto the problem of fndng the mnmum dstance between a pont and a parallelogram n 3D. Computng the mnmum dstance between a pont and a convex polygon n 3D s a smple task and can be solved n at most max (l,k) operatons. hus, the computatonal complexty of MDD3 s O

16 368 (ml+nk+c(l+k)). he complexty can be further reduced to O(m+n+c(l+k)) when both polyhedra A and have fxed orentatons durng moton, and growth obstacles and A c are only computed once. Although the FV constact pars n Algorthm MDD3 cannot be done n lnear tme as we have shown n Algorthm MDD2, the computaton of MDLD usng the boundary model of the Mnkowsk sum only requres consderaton of cases wth FV, VF, and EE contacts. Compared to exhaustve searchng, the number of operatons requred to compute MD and MDED s reduced to about one-thrd. Moreover, the penetraton measure for two ntersectng objects can also be obtaned usng the proposed approach. 4. EXAMLES AND ALICAIONS he examples used n testng algorthm MDD3 nclude platonc solds lsted n able 1. he data structure of a convex polyhedron has a feld for ts faces, edges, vertces, the poston of the reference pont, and ts orentaton. Each face s represented by ts outward normal vector and ts dstance from the orgn. Its data structure also ncludes a lst of vertces whch le on ts boundary. Each edge s descrbed by two connectng vertces and ts two neghborng faces. Each vertex s characterzed by ts x, y, and z coordnates wth respect to ts reference pont. Examples were run on an IBM C whch had a 33Mhz 486 CU. Fg. 8 shows two examples of the mnmum drected Eucldean dstance d 2 (A, ) obtaned usng the MDD3 algorthm. able 1. latonc solds used n 3D examples. etrahedron Cube Octahedron Icosahedron Number of vertces Number of faces (a) cube and cosahedron (b) cube and cube Fg. 8. Example results of algorthm MDD3 for the mnmum dstance between two platonc solds; (a) cube and cosahedron, and (b) cube and cube.

17 COMUING DISANCES BEWEEN CONVEX OLYHEDRA 369 he resultng mnmum dstance has been verfed by usng the tradtonal optmzaton (reduced gradent) routne. he computatonal tme needed for the MDD3 algorthm for objects lsted n able 1 s summarzed n able 2. If sets and A c were computed n advance, the subroutne could generally be run n several mllseconds (about 3 to 8 mllseconds). Numercal data n able 2 ndcate that algorthm MDD3 roughly ncreased lnearly n computaton tme wth the total number of vertces and faces. able 2. Runnng tme (n mllseconds) of MDD3 between two platonc solds A and wth / wthout computng and A c. A \ etrahedron Cube Octahedron Icosahedron etrahedron 7.44 / / / / 6.1 Cube / / / / 7.2 Octahedron 9.78 / / / / 7.0 Icosahedron / / / / 8.4 In the frst applcaton, we wll llustrate the procedure used n applyng the proposed MDLD functon to detect the ntersecton between a lne segment and a convex polygon n a two-dmensonal x-y space. he lne segment may represent a lne path of a pont object whle the polygon represents ether a polygon obstacle or a confguraton-space obstacle. he lne segment n two-dmensonal space can be represented by a degenerated polygon as shown n Fg. 9(a). hus, the ntersecton detecton problem becomes a standard problem whch algorthm MDD2 can solve. Consder a lne segment wth two end ponts p 0 and p 1, and a unt vector u whch ponts to pont p 1 from pont p 0, as shown n Fg. 9(a). Lne segment p 0 p 1 n 2D can be represented by a polygon wth two vertces, p 0 and p 1, and satsfyng two planar half-spaces as below: n = z u x p 1 p 1 u u p 0 -n p 0 (a) (a) 2D case (b) 3D case Fg. 9. Collson detecton between a lne segment and a convex polyhedron ; (a) 2D case, and (b) 3D case.

18 370 n (x - p 0 ) 0 and -n (x - p 0 ) 0, where n = z u, s the vector cross-product operaton, and z s the z-axs unt vector. herefore, the collson ntersecton problem between a lne segment and a convex polygon s reduced to that of fndng the MDLD. hs methodology can be drectly appled to threedmensonal cases, where a lne segment n three-dmensonal space s represented by a convex polyhedron havng one edge, p 0 p 1, and two vertces, p 0 and p 1, as shown n Fg. 9(b). In the second applcaton, we wll llustrate a generate-and-test 2D path-plannng algorthm for a pont robot by usng the proposed MDLD as a key functon. he nput s a set of statonary convex polygonal obstacles, the ntal poston, and the fnal poston. If the robot s not a pont object, then we can reduce the problem to a pont robot problem by workng on the confguraton space. he procedure s as follows. Algorthm 2DAH: 2D path plannng Input: ntal and fnal postons Output: Collson-free lne-segment path passes the subgoals n the subgoal lst {S 0 = ntal,s 1,..., S N = fnal } Intal: startng pont S = ntal, and target pont G = fnal, subgoal lst = {S 0 = ntal } Step 1: Detect ntersecton wth a lne segment path. Connect startng S pont and target G pont. Identfy the obstacles ntersectng the straght lne path between S and G. If no obstacle s found, then go to Step 2; otherwse go to Step 3. Step 2: Update subgoal lst. If target pont G s equal to goal, then place goal nto the subgoal lst and fnsh. Otherwse, place target G nto the subgoal, let the new startng pont S = G and the new target pont G = goal, and go to Step 1. Step 3: Search the nearest obstacle. Search the nearest obstacle among colldng obstacles to the startng pont and go to Step 4. he nearest obstacle s the one whch has the mnmum dstance to startng pont S. Step 4: Group connectng obstacles. Does the nearest obstacle ntersect other obstacles? If so, add these obstacles to the nearest obstacle lst. Repeat addng of those obstacles whch ntersect obstacles n the lst to the nearest obstacle lst. Go to Step 5. Step 5: Select mdpont. Straght lne L, whch passes through S and G, dvdes all the vertces n the nearest obstacle lst nto two groups. Search the farthest vertex from the group whch has the smaller average dstance from vertces to the lne L. he farthest pont s the one wth the largest dstance to straght lne L. Assgn the selected farthest vertex as the new target pont G and go to Step 1.

19 COMUING DISANCES BEWEEN CONVEX OLYHEDRA 371 In the above algorthm, Steps 1, 3, and 4 are easly solved by the MDLD functon developed here. It s noted that Step 4, whch connects overlappng obstacles, can be done n advance and requres very lttle tme durng path plannng. An example of the result of the proposed algorthm n selectng one subgoal s shown n Fg. 10. he 2DAH algorthm s qute fast, and the planned trajectores are near the shortest paths. Fg. 11 shows a result of path plannng done by the 2DAH algorthm wth only seven vertces vsted and ffteen lne segments generated. Algorthm 2DAH was tested n the envronment as shown n Fg. 11 wth 100 randomzed ntal and fnal ponts. he average runnng tme on an IBM C was about 45 mllseconds. he algorthm proposed here dffers from prevous approaches n three mportant ways, (1) detecton of the trajectory n admssblty, (2) the means of generatng alternatve mmedate ponts, and (3) the generated subgoal n forward order. he above algorthm can be modfed for use n the 3D path-plannng problem. 3D path plannng can be reduced to 2D path-plannng n a 2D path plane that passes ntal and fnal ponts. he 2D polygonal obstacle s vertces are obtaned from the ntersecton of the edges of obstacles and the 2D path plane. Fg. 10. Example of decson made by algorthm 2DAH. 5. CONCLUSIONS Computaton of the proposed mnmum drected dstance has been recast as the Mnkowsk sum of a two convex object problem. he proposed boundary-model approach can generate analytcal expressons for the Mnkowsk sum of two convex objects 2D; hence, MDLD can be expressed n analytcal form. For the 3D case, only a few edge-edge pars need be consdered durng computaton of the mnmum dstance between two convex polyhedra. he algorthm developed here can be used for collson detecton, computaton of the ds-

20 372 Fg. 11. A path planned by the 2DAH algorthm n 2D. tance between two polyhedra n three-dmensonal space, and robotcs path-plannng problems. he advantages of usng the Mnkowsk sum n computng dstance measures between two convex polyhedra are: (1) the measure of the separatng dstance or degree of penetraton can be consdered; (2) MDLE, MD and MDED functons can be computed n one routne; and (3) computaton complexty s lnear n 2D, and computaton s fast n 3D. REFERENCES 1. J.E. Bobrow, A drect mnmzaton approach for obtanng the dstance between convex polyhedra, Internatonal Journal of Robotcs Research, Vol. 8, No. 3, June 1989, pp C.E. Buckley, A foundaton for the flexble-trajectory approach to numerc path plannng, Internatonal Journal of Robotcs Research, Vol. 8, No. 3, June 1989, pp S.A. Cameron and R.K. Culley, Determnng the mnmum translatonal dstance between two convex polyhedra, IEEE Internatonal Conference on Robotcs and Automaton, 1986, pp F. Chn, and C.A. Wang, Optmal algorthms for the ntersecton and mnmum dstance problems between planar polygons, IEEE ransactons on Computers, Vol. C-32, 1983, pp E.G. Glbert, D.W. Johnson and S.S. Keerth, A fast procedure for computng the ds-

21 COMUING DISANCES BEWEEN CONVEX OLYHEDRA 373 tance between complex objects n three-dmensonal space, IEEE ransactons on Robotcs and Automaton, Vol. 4, No. 2, 1988, pp M.C. Ln and J. Canny, A fast algorthm for ncremental dstance calculaton, IEEE Internatonal Conference on Robotcs and Automaton, Aprl, 1991, pp Lozano-erez, Spatal plannng: a confguraton space approach, IEEE ransactons on Computers, Vol. 32, No. 2, 1983, pp V.J. Lumelsky, On fast computaton of dstance between lne segments, Informaton rocessng Letters, Vol. 21, August, 1985, pp W. Meyer, Dstances between boxes: applcatons to collson detecton and clppng, IEEE Internatonal Conference on Robotcs and Automaton, 1986, pp J.. Schwarz, Fndng the mnmum dstance between two convex polygons, Informaton rocessng Letters, Vol. 13, 1981, pp Chng-Long Shh (Iy )receved the B.S. and M.S. degrees from Natonal Chao ung Unversty, and the h. D. degree n electrcal engneerng from the Oho State Unversty n Currently, he s a professor n the Department of Electrcal Engnerng, Natonal awan Insttute of echnology. Hs research nterests nclude moble robot moton plannng and legged robot locomoton. Jane-Yu Lu (BØü)receved the B.S. degree n electronc engneerng from Chung-Yuan Chrstan Unversty n 1992 and the M.S. degree n electrcal engneerng from the Natonal awan Insttute of echnology n he subject of hs masters thess was mnmum drected dstance and path plannng n 2D space. He currently s employed by the Wnbond Electroncs Corp., workng on devce relabllty.