Int J Adv Manuf Technol (1998) 14:70-76 © 1998 Springer-Verlag London Limited

The International Journal of

[Idvanced I1]anufacturino Technoloou

Collision Detection in Static and Dynamic Complex Scenes not Having Geometry Coherence U. Benchetrit, E. Lenz and M. Shoham Technion - Israel Institute of Technology, Center for Manufacturing Systems and Robotics, Faculty of Mechanical Engineering, Haifa, Israel

invoked with a random sequence of configurations (i.e. without

The goal of this paper is to improve the efficiency of collision detection algorithms applied to highly complex geometry scenes not having geometry coherence. Efficiency is improved by reducing the number of objects to be checked for collisions and accelerating the fundamental repeating checks on which the collision detection process relies. The number of objects to be checked is decreased by combining hierarchical representation techniques with incremental methods. The fundamental repeating check, actually a collision check between two bounding boxes, is accelerated by using 3D bounding boxes each having an associated trans¢brmation for mapping its vertices from a local to a world coordinate system (CS). This acceleration is based on the observation that two boxes (A and B) intersect if and only if the projections of A and B intersect on all three axis-aligned orthographic views, both for A in the local CS of B and for B in the local CS of A. These algorithms have been successfully implemented in simulating dynamic scenes with high geometry complexity and as a virtual collision detection sensor for off-line robot motion planning.

In this paper, we present an efficient algorithm for collision detection, in both static and dynamic complex scenes. The goal of this paper is, therefore, to minimise the number of required collision checks and to improve the efficiency of basic, frequently repeated collision detection operations. A separate paper will discuss implementation of these algorithms in robot motion planning. First, hierarchical representation techniques and their associated bounding volumes are described. Next, methods for calculating near-optimal bounding volumes are reviewed. Then, methods are presented for efficient collision detection in static and dynamic complex scenes using hierarchical bounding volumes. A rapid method for collision detection between two bounding boxes is then described. An application implementing these algorithms is described, followed by a summary.

Keywords: Collision detection; Complex; Dynamic; Static

2.

1.

Introduction

The efficiency of a collision detection algorithm is extremely important in complex scenes composed of objects described by thousands of geometric elements. It is even more crucial in dynamic scenes for which the algorithm must be executed repeatedly and rapidly at each instant. Several existing collision detection algorithms exploit the geometry coherence between successive checks for collisions [1-4]. Performance of these algorithms has been optimised based mainly on the idea that they are applied for collision checks during continuous simulation. However, these algorithms suffer from performance degradation when they are

Correspondence and offprint requests to: Professor E. Lenz, Faculty of Mechanical Engineering, Technion - Israel Institute of Technology, Technion City, Haifa 32000, Israel.

any coherence between successive calls).

Hierarchical Representation

One common technique for accelerating collision detection algorithms uses hierarchical representation of bounding volumes [5-7]. In this scheme, every object is represented by a tree whose root is the bounding volume (for instance a bounding box) of the entire object. Each child of the root is a bounding volume of parts of the object. The actual geometry is described only at the leaves of the tree. This hierarchical scheme repeats itself recursively for each child and can be formed manually or automatically. A node in a tree (not a leaf) is actually a grouping of its children. Thus, it is only natural to group geometric features that have a functional connection or are adjacent in space. A common method for improving the hierarchical representation is to add transformation operators to the tree [8]. A transformation operator can be included as a child of a node acting on all the subsequent nodes in the hierarchy. Embedding transformation operators in the hierarchy allows an additional local grouping of parts of the objects. In each local group, the geometry is described with respect to a local coordinate system

Coil&ion Detection in Complex Scenes

(CS) on which operates the concatenation of all the transformations along the path back to the root.

2.1

Hierarchical Representation of Dynamic Objects

In this paper, a dynamic object refers to a mechanism. A mechanism is constructed by connecting rigid bodies together with joints that constrain their relative movement [9]. An articulated robot is used here as an example of a mechanism. In a hierarchical representation of an articulated robot, composed of a kinematic chain of links, the root represents the entire robot. Hence, the bounding volume of the root completely encompasses the geometry of the robot in its current configuration (see Fig. 1). The root of the tree, which is considered as the first level of the hierarchy, is split into several nodes composing the next level of the hierarchy. The first node at this level is a transformation from the world coordinate system (CS) to the CS attached to the base of the robot. Following the transformation node, there are nodes at the same level that represent the first grouping of the robot geometry. Each group is described in its own local CS. Natural grouping is based on link partitioning. For instance, between every two adjacent links there is a transformation node describing the relation between their local CSs. The bounding volume of a robot link is described relative to the local CS of that link. The pose (position and orientation) of this bounding volume relative to the world CS is obtained by chaining all the transformations along the path back to the root. Since the bounding volume for a robot link is attached to the local CS of the link, the link and its bounding volume move together. As a result, the description of the bounding volume of the link relative to its local CS remains valid even when the link moves. This is true only for bounding volumes

71

calculated for a rigid body (single link). On the other hand, a bounding volume calculated for more than one link must be recalculated when a link moves relative to the others. The geometry of a single link could be also re-partitioned into subgroups. For example, all the links of the robot in Fig. 1, though rigid, were further partitioned into subgroups. Each such subgroup will be represented in its own local CS, and each will be associated with a transformation operator node related to the local CS of its link. Bounding volumes are also calculated for these subgroups and also remain valid while these subgroups move.

2.2 Bounding Volumes for a Hierarchical Representation The bounding volume of each node in the hierarchy could be calculated from the actual geometry beneath it. To accelerate the calculations, the bounding volume of a node is obtained from the union of the bounding volumes of its children and not their actual geometry. For most purposes, this approximation is sufficient. However, in cases where a minimal bounding volume is required, the actual geometry of each node can still be used instead. This method of calculating bounding volumes for hierarchical representation is valid for the hierarchy of either static or dynamic objects. The difference is that for dynamic objects the bounding volume of the root of the hierarchy will always be calculated using the approximated method. This is true since the bounding volume of the root accommodates more than one link and thus becomes invalid when one of the links moves. In practice, the bounding volume for the root of dynamic objects is used only for initial culling. Hence, the approximated bounding volume based on bounding volumes of its children is sufficient even if it is not a minimal bounding volume.

3. Computing a Bounding Volume

Fig. 1. Hierarchical bounding volumes of a SCARA type robot.

A box and a sphere are two common bounding volumes used in collision detection algorithms. The computational complexity involved in detecting collisions among boxes or spheres is relatively simple. An effective bounding volume must have a minimal volume when used in collision detection. Thus, N given points in 3D space should be bounded by a box or a sphere with minimal volume. The problem of minimal bounding volume has received considerable attention in the computational geometry community [10,11]. An algorithm for calculating a minimal bounding sphere in O(N) was proposed by Megiddo [10]. This algorithm produces an optimal bounding sphere having a minimal volume. However, it is difficult to implement and involves large time constants in its computation. Practically, it is sufficient to use near-optimal bounding boxes or spheres with volumes slightly larger than the optimal. These near-optimal bounding volumes are as efficient as the optimal bounding volumes in collision detection, and are calculated much faster. Fast algorithms for computing near-optimal bounding volumes are important. An efficient method for finding a

72

U. Benchetrit et al.

near-optimal bounding sphere for any set of N points in 3D space was proposed by Ritter [t2]. It is also of Order (N), but its computations require only small time constants, and the sphere calculated is only about 5% larger than the optimal minimum-radius sphere. Another algorithm for computing a near-optimal bounding box and sphere with linear-time complexity was proposed by Wu [13]. Wu's algorithm is adopted in this work, and for the sake of completeness the main ideas behind Wu's algorithm are quoted. A direct method for computing a bounding volume is based on scanning the set of the given points S = {(xi,y~,z~): O--
I. Composing a covariance matrix out of the given point set. 2. Extracting the eigenvalues X~, X2, ~3 and their corresponding eigenvectors u, y, w from the covariance matrix. The first step involves a straightforward calculation yielding a 3 × 3 matrix. The second step uses the algorithm described in [15]. The three eigenvectors computed describe the new CS, with one axis coinciding with the principal axis of the point set. Points from the old CS (x,,y~,zi) are transformed into points

in the new CS (x'~,y~,z~) by a rotational transformation defined by the orthogonal eigenvectors u, v and w. Once the new point set is obtained relative to the new CS, an orthogonal bounding box for the new set of points S ' = {(x'i,y'j,z'i): 0--
4.

The bounding volume hierarchy can be used to detect collisions between static or dynamic scenes, each described by a hierarchical representation. Starting from the roots, two hierarchies are checked recursively, one against the other, for a possible collision. Unless the hierarchy represents static objects (having no joints at all), objects embedded in a single hierarchy are also checked for a possible collision between them. To minimise the number of collision checks, it is also possible to include an exception list of pairs of objects that should not be checked for collision. Next, the considerations for expanding two colliding bounding volumes are described, followed by an incremental method to accelerate the collision detection process in dynamic scenes.

4.1

Node Expansion Considerations

A bounding volume hierarchy is represented as a tree for which each node is capable of holding a bounding volume of the nodes below it in the hierarchy. Only the leaves hold the actual geometry (see Fig. 1). As mentioned earlier, non-shape data such as transformations, may be also represented by leaves. When two nodes (bounding volumes) in one or two hierarchies are found to collide, they should be expanded in order to examine their children for collision. The simplest way is to keep expanding the nodes of the same hierarchy recursively, until there is no collision with another node or until the path is fully explored. Another method is to alternate the order of the expansions of the two hierarchies. A node could also be expanded to more than one level at a time depending on some heuristics. A better way seems to be the one that expands the node evaluated as the most promising node. This evaluation could rely, for instance, on a volume comparison between the bounding volume of the parent against the sum of volumes of its children. The largest difference will be chosen as the most promising.

4.2

Fig. 2. Bounding boxes of the shoulder link of a SCARA type robot. (a) Result of the direct method. (b) Minimal bounding box.

Collision Detection

Incremental Collision Detection

In simulating dynamic scenes, the collision detection algorithm is activated after every position update (called moving-step) of its objects and just before the display is updated. Such scenes are composed of many objects, and one or more objects moves relative to other static objects. In most cases, only some objects move relative to others, while many objects retain their relative positions. The collision detection algorithm can avoid checking possible collisions among all the objects that did not move in the last moving step. Thus, the collision detection algorithm is accelerated by using incremental checks only on object

Collision Detection in Complex Scenes moved. The collisions status of the objects not moved, as welt as their bounding volumes, is carried over in successive moving-steps. In order to know how to mark the objects that must be checked for collision, we must understand what causes objects to move. Objects move in dynamic scenes either by explicit commands or by being attached to other moving objects. The first objects can be marked directly. The other moving objects (the attached objects) could be marked after analysing the dependencies dictated by their place in the hierarchy (i.e. every object that moves also moves all subsequent objects in the kinematic chain).

5. Collision Detection Between Two Bounding Boxes In general, for any two non-colliding convex polyhedra, a separation plane exists between them which is either: 1. Parallel to a face of the first polyhedron. 2. Parallel to a face of the second polyhedron. 3. Parallel to an edge of the first polyhedron and to an edge of the second one [16]. A by-product of the bounding box calculation is a transformation matrix needed to transform the vertices from the local CS to the world CS. Thus, collision detection between two bounding boxes lying at arbitrary locations and orientations in space can be accomplished very efficiently by using boxes, each having an associated transformation matrix. This process is based on the observation that two boxes (A and B) intersect if and only if the projections of A and B intersect on all three axis-aligned orthographic views, both for A in the local CS of B, and for B in the local CS of A. Let A and B be the two bounding boxes to be checked for collision (see Fig. 3). Let Fw be the world CS. Let FA and F~ be the local CS attached to box A and B, respectively. Boxes A and B are axis-aligned relative to Fa and Fs, respectively.

Fig. 3. Two boxes lying at an arbitrary location and orientation.

73

Let 7A and T8 be the transformations of box A and B from FA, and, respectively, from F~, to Fw. Collisions are detected between boxes A and B according to the following steps: 1. Transform box B into Fa (in which box A is axis-aligned), and check if there exists a separation plane parallel to one of the faces of box A (type ! separation). If found, indicate that there is no collision between box A and box B and stop the algorithm. Otherwise proceed to the next step. 2. Transform box A into F~ (in which box B is axis-aligned), and check if there exists a separation plane parallel to one of the faces of box B (type 2 separation). If found, indicate that there is no collision between box A and box B and stop the algorithm. Otherwise proceed to the next step. 3. Transform one box into the local CS of the other one (select arbitrarily which one to transform or use results of the last transformation in the last step), and check if there exists a separation plane parallel to an edge of A and an edge of B (type 3 separation). If found, indicate that there is no collision, otherwise indicate that there is a collision between box A and box B. Stop the algorithm.

5,1

Algorithm Details

Although intuitively simple, some aspects of the above algorithm are still too informal and must be made more precise. In the following subsections the method for checking whether a separation plane of one of the three types exists is described in detail. In preparation for the following checks, both boxes should be referenced to the same CS by mapping one box into the local CS of the other box. For instance, mapping the vertices of box B into Fa is performed in two steps. First, the transformation matrix TSA =T~TA -~ is calculated. Then, each of the vertices of B is transformed to FA using TeA.

5. 1.1

Type 1 and type 2 Separation

Type 1 separation exists if there is a separation plane parallel to one of the faces of box A (see Fig. 4). We assume that the vertices of box B were mapped into the local CS of box A (FA), in which box A is axis-aligned. The faces of box A define six infinite planes which are refered to as face-planes (using Greene's semantic). Since box A is axis-aligned, its face-planes are defined merely by its bounds. The outwardpointing normats of the faces of box A point to the positive half-spaces created by these face-planes. A separation plane parallel to one of tile faces of box A exists if and only if box B lies entirely within at least one of the positive half-spaces defined by the face-planes of box A. Box B lies entirely in a positive half-space defined by one of the face-planes of A (let it be the plane defined by the u component of the upper bounds of box A) if the u components of all the vertices of box B (with respect to FA) are greater than the u component of the upper bounds of box A (eight inequalities needed). Analogously, box B lies entirely in a positive half-space defined by one of the face-planes of A (let it be the plane defined by the u component of the lower

74

U. Benchetrit et al.

Fig. 4. Separation plane parallel to one of the faces of box A. bounds of box A) if the u components of all the vertices of box B are less than the u component of the upper bounds of box A. At least 8 inequalities are needed to determine that a type t separation plane exists. While, at most 48 inequalities are needed to determine that there is no type 1 separation plane. Given eight vertices of box B with respect to FA, and six faceplanes of box A, for each face-plane F of box A, if all the vertices of box B are in the positive half-space of F, indicate a separation plane was found and stop If box B does not lie entirely in the positive half-space of any F */indicate a separation plane was not found and stop Type 2 separation exists if there is a separation plane parallel to one of the faces of box B. Type 2 separation is checked using the same procedure as in type 1 except that box A and box B exchange roles. 5.1.2

Fig. 5. Separation plane parallel to an edge of box A and to an edge of box B. a point (call this point By) which is not inside the hexagon representing box A in this view (i.e. in this view By is on the righthand side of Be). At least 4 inequalities are needed to determine that a type 3 separation plane exists. While at most 72 inequalities are needed to determine that there is no type 3 separation plane. For each of the orthographic views and for each edge of the silhouette of box A (hexagon) calculate the line equation of the edge (called edge-line) If all vertices of the projection of box B (rectangle) tie outside of the edge-line, then indicate a separation plane was found. If no separation plane was found in all orthographic views, indicate there is no separation plane parallel to an edge of box A and to an edge of box B.

Type 3 Separation

Type 3 separation exists if there is a separation plane parallel to an edge of box A and an edge of box B (see Fig. 5). This test is symmetrical, so the decision as to which box is axisaligned has no meaning. For efficiency, the last transformation is used and thus an extra transformation of vertices is avoided. It is assumed that the vertices of box A were mapped to the local CS of box B (FB) in which box B is axis-aligned. In this case, all the orthographic projections of box B yield rectangles and all those of box A yield hexagons (not necessarily regular). Each hexagon is actually the silhoutte of box A lying at an arbitrary orientation in space. A hexagon will degenerate into a rectangle if box A has edges parallel to the projection direction relative to Fe. If there is a separation plane parallel to an edge of box A (call this edge Ae) and to an edge of box B (call this edge BE), then in one of the orthographic views edge BE is represented as

5.1.3

The Silhouette of a Box

In the previous subsection dealing with checking type 3 separation, the vertices of the silhouettes of box A had to be calculated for each orthographic view. The general algorithm for finding the silhouette edges in an orthographic view for a convex polyhedron is as follows [16]. In a view down an axis (called the u-axis), if the u components of the outward-pointing normals of two adjacent faces have opposite signs, their common edge is a silhouette edge. While the above algorithm is general for any convex polyhedron, we could use a simpler one for finding silhouette edges in an orthographic view of a box. For each orthographic view find the vertex having the minimal value along the projection direction (call it i: 0---i-<-7). It] each orthographic view of box A, two vertices do not belong to its silhouette. The two vertices that disappear are vertex i and vertex ( 7 - i ) which are opposite.

Collision Detection in Complex Scenes

75

0 and 7, 1 and 6, etc.) differ only in the direction of the specification of the vertices.

5.1.4

The Line Equation of a Silhouette ,Edge

The line equation of an edge of the hexagon representing the silhouette of the orthographic projection of box A is calculated in the following robust way. The equation for line in the plane can be represented by, c~u + 13v + ~ = 0 where, u and v are the abscissae of the projection plane (i.e. for the x, y-plane: u=x, v=y, for the y, z-plane: u=y, v=z, and for the z, x-plane; u=z, v=x). Let (u0, ul) and (v0, vl) be the vertices bounding the edge of interest. Arbitrarily select, ~ - - _ F I -- 120

Fig. 6. The silhouettes of box A in 3 orthographic views.

B=Uo--

Ul

As a result, 6

z

= -(~Uo +

!t

3

1

X Fig. 7. The vertices indexing used in Tahle 1. Actually they are the nearest and the farthest vertices from the projection plane. The remaining six vertices of box A define the six-sided polygon which is the silhouette of box A in this orthographic view (see Fig. 6). The order of the vertices of this six-sided polygon is important for the rest of the check. The order of the vertices is derived from a simple table with eight rows, each composed of a cyclic list of six vertices. A table row is accessed using the value of i. The order of the vertices forming the six-sided polygon silhouette for all possible values of i is given in Table 1. The indexing used in the table is for a box having the order of vertices as in Fig. 7. Note that complementary rows (such as

13re)

With this selection, division is avoided so no special care has to be taken to avoid division by zero. Moreover, this representation is still valid even in the case where the hexagon degenerates into a rectangle with two of its edges degenerating into zero length.

6. Application The algorithms described so far were implemented on simulations of robots and their working environme~Lt. Figure 8 shows one of the simulations used to verify the collision detection algorithms. In this simulation an articulated 6-DOF robot working on a washing machine was modelled. This complex

Table 1. The vertices forming the six-sided polygon silhouette. i

Silhouette vertices

Ignored vertices

0 t 2 3 4 5 6 7

1,3,2,6,4,5,1 0,4,5,7,3,2,0 3,7,6,4,0,1,3 2,0,1,5,7,6,2 5,1,0,2,6,7,5 4,6,7,3,1,0,4 7,5,4,0,2,3,7 6,2,3,1,5,4,6

0,7 1,6 2,5 3,4 4,3 5,2 6,1 7,0 Fig. 8. A simulation of a Mantec robot working on a washing machine.

76

U. Bencheta-it et aL

dynamic scene is composed of thousands of polygons. The geometric models of the robot and the washing machine in Fig. 8 are composed of more than 2000 and 3000 triangles, respectively. In order to evaluate the performance of the collision detection algorithm, a path was planned for the robot in which the robot moves toward the washing machine, causing collisions with it. Then, the playing time for this path was measured, once without on-line collision detection and later with on-line collision detection. The time difference between the two runs is due to the collision checks activated after every position update (frame) of the scene. By knowing the number of total checks, it was found that checking collisions between the robot and the washing machine took an average time of 6 × I0 3 s on Indigo 2 EXtreme. The simulation program, which was called motionPlayer, was written in C + + using the IRIS Inventor 3D development toolkit [8]. The Inventor toolkit is used mainly to read and maintain the input 3D scene and to view the constantly changing 3D scene using a variety of viewers. The 3D scene read by the Inventor toolkit is passed to a parser that looks for kinematic hints embedded in the input files. These kinematic hints are then used to construct the data structures describing the kinematic properties of the dynamic objects in the scene. In addition, the motionPlayer program uses the Inventor Xt Utility library and Motif for its user interface. The motionPlayer program runs on a variety of SiliconGraphics workstations running IRIS 4.05 up to IRIS 5.3 operating systems. It was tested on old hardware such as IRIS 4D/25 and on newer hardware such as Indigo 2 EXtreme.

7,

Conclusions

The algorithms presented in this paper are aimed on the one hand at reducing the number of objects to be checked for collisions and on the other for accelerating the repeated basic checks. Reducing the number of objects to be checked was based on hierarchical representations of bounding volumes and on incremental methods. Accelerating the basic collision detection between two bounding boxes was based on simple inequality evaluations. The algorithms have been successfully implemented on a simulation of dynamic scenes with a random

sequence. These scenes have a highly complex geometry, and lack geometry coherence. It was shown that checking collision between two objects modelled by thousands of polygons, took only an average time of 6 × 10 3 s on Indigo z Etreme.

References 1. E. G. Gilbert, D. W. Johnson and S. S. Keerthi, "A fast procedure for computing the distance between complex objects in threedimensional space", IEEE Journal of Robotics and Automation, RA-4(2), pp. 193-203, 1988. 2. D. Baraff, "Curved surfaces and coherence for non-penetrating rigid body simulation", ACM Computer Graphics, 24(4), pp. 1928, 1990. 3. M. C. Lin and J.F. Canny, Local Methods for Fast Computation of Distance Functions, ESRC 92-28/RAMP 92-11, University of California, Berkeley, 1992. 4. M. C. Lin, D. Manocha and J. F. Canny, "Fast contact determination in dynamic environments", Proceedings of the IEEE International Conference on Robotics" and Automation, San Deigo, CA, pp. 602-608, 1994. 5. S. M. Rubin and T. Whitted, "A 3-dimensional representation for fast rendering of complex scenes", Proceedings of" the Computer Graphics (SIGGRAPlt '80 Proceedings), pp. 110-116, 1980. 6. H. Weghorst, G. Hooper and D. P. Greenberg, "Improved computational methods for ray tracing", ACM Transactions on Graphics, 3(1), pp. 52-69, 1984. 7. R. Featherstone, "A hierarchical representation of the space occupancy of a robot mechanism", Proceedings of the Second Workshop on Computational Kinematics, Sophia Antipolis, France, pp. 183-192, 4-6 September 1995. 8. IRIS-Inventor, IRIS Inventor Programming Guide, Silicon Graphics, Inc. I & II, 1992. 9. J. M. McCarthy, An Introduction to Theoretical Kinematics, The MIT Press, Cambridge, MA, 1990. 10. N. Megiddo, "Linear programming in Iinear time when the dimension is fixed", Journal of ACM, 31, pp. 114-127, 1984. l I. F. P. Preparata and M. I. Shamos, Computational Geometry: An Introduction, Springer-Verlag, New York, 1985. t2. J. Ritter, "An efficient bounding sphere", in A. S. Glassner (ed.), Graphics Gems, Academic Press, 833 pp. 1990. 13. X. Wu, "A linear-time simple bounding volume algorithm", in D. Kirk (ed.), Graphics Gems HI, Academic Press, pp. 301-306, 1992. 14. B. F. J. Manly, Multivariate Statistical Methods, Chapman and Hall, London, 1986. 15. W. H. Press, B. P. Flannery, S. A. Teukolsky and W. T. Vetterling, Numerical Recipes in C, Cambridge University Press, 1988. 16. N. Greene, "Detecting intersection of a rectangular solid and a convex polyhedron", in P. S. Heckbert (ed.), Graphics Gems IV, pp. 74-82, Academic Press, 1994.