MERL { A MITSUBISHI ELECTRIC RESEARCH LABORATORY
http://www.merl.com
Ecient Algorithms for Two-Phase
Collision Detection
Brian Mirtich
TR-97-23 Decemb er 1997
Abstract
This article describ es practical collision detection algorithms for rob ot motion
planning. Attention is restricted to algorithms that handle rigid, p olyhedral ge-
ometries. Both broad phase and narrow phase detection strategies are discussed.
For the broad phase, an algorithm using axes-aligned b ounding b oxes and a hi-
erarchical spatial hash table is describ ed. For the narrow-phase, the Lin-Canny
algorithm is presented. Alternatives to these algorithms are also discussed. Fi-
nally, the article describ es a scheduling paradigm for managing collision checks
that can further reduce computation time. Pointers to downloadable software
are included.
To appear in Practical Motion Planning in Rob otics: Current Approaches and Future Directions,
K. Gupta and A.P. del Pobil, editors.
This work may not b e copied or repro duced in whole or in part for any commercial purp ose. Permission to
copy in whole or in part without payment of fee is granted for nonpro t educational and research purp oses
provided that all such whole or partial copies include the following: a notice that such copying is by p er-
mission of Mitsubishi Electric Information Technology Center America; an acknowledgment of the authors
and individual contributions to the work; and all applicable p ortions of the copyright notice. Copying,
repro duction, or republishing for any other purp ose shall require a license with payment of fee to Mitsubishi
Electric Information Technology Center America. All rights reserved.
c
Copyright Mitsubishi Electric Information Technology Center America, 1997
201 Broadway, Cambridge, Massachusetts 02139
1. First printing, TR97-23, Decemb er 1997
1
1 Intro duction
Collision detection algorithms are fundamental in rob ot motion planning. They
serve di erent purp oses dep ending on the application. In ne motion planning,
collision detection algorithms determine the time and lo cation of contacts, a
necessary step in computing contact forces. In gross motion planning, collision
detection algorithms are used to rep ort the distances b etween ob jects, with the
aim of constructing feasible motion paths, or keeping the rob ot maximally clear
from obstacles, or building maps of the environment. In any case, collision de-
tection is a low level pro cess in a motion planning system, whichmust execute
b efore higher level pro cesses can pro ceed. For this reason, collision detection
algorithms must b e fast. In the context of rigid-b o dy simulation, Hahn found
collision detection often required over 95 of the computation time [12]. Algo-
rithms have improved since then, however, they remain a b ottleneck in many
situations.
Collision detection algorithms have ro ots in computational geometry, where
the basic problem is to rep ort intersections among a group of static ob jects. In
motion planning, the problem is more dicult b ecause the ob jects are moving.
Their tra jectories might be closed form functions of time, as in the case of a
kinematically controlled manipulator. Alternatively, the tra jectories mightbe
describ ed through the di erential equations of Newtonian dynamics.
This article describ es some practical algorithms for collision detection, pre-
senting b oth theoretical and empirical results. To limit the scop e, we restrict
attention to situations with rigid, p olyhedral ob jects. These restrictions allow
signi cant optimizations and are tolerable for many motion planning applica-
tions. In particular, the p olyhedral restriction is a mild one since more general
shap es, such as parametric surfaces or CSG-style solids, can b e approximated to
arbitrary closeness with p olyhedral mo dels. The algorithms discussed here are
quite insensitive to the complexity of the p olyhedra. We also restrict attention
to exact algorithms: those that ultimately use the underlying p olyhedral mo d-
els in their computations. The alternatives are approximate metho ds, which
are less concerned with the true geometry and more concerned with real-time
1
p erformance. A go o d reference list of work in collision detection is in [14].
1.1 Coherence and lo cality
Early collision detection algorithms for moving p olyhedral ob jects solved prob-
lem instances from scratchatevery time step. These algorithms required run-
ning time that was quadratic in the complexity of the p olyhedra to determine
if the ob jects were disjoint [21,12]; they are to o slow for many motion planning
applications. Some early linear time algorithms were also prop osed [10].
The biggest improvementover these early algorithms came from the use of
coherence: the fact that the collision detection system solves a series of related
problems, each one only slightly di erent than the one b efore. This is the usual
1
See, for example, Hubbard's work on collision detection for time-critical applications using
b ounding sphere hierarchies [15].
MERL-TR-97-23 Decemb er 1997
2
case in motion planning as well as dynamic simulation. Caching results from
previous invo cations greatly reduces the amortized cost of collision detection.
Gilb ert, et. al. describ e an adaptation of their algorithm to take advantage of
coherence when it exists [10]. Coherence is also used in Bara 's witness plane
algorithm [3], Cameron's enhancement of the Gilb ert algorithm [4], the Lin-
Canny closest features algorithm [18,17], and the V-Clip algorithm [20]. Use
of locality often go es hand-in-hand with coherence in many algorithms. This
prop erty allows an algorithm to verify that the cached result is still valid using
only lo cal geometric tests. Lo cality usually relies on convexity of the ob jects.
Coherence and lo cality combine to form almost constant time algorithms for
verifying disjointness or computing the distance b etween ob jects.
1.2 Two-phase collision detection
When there are n distinct ob jects in a motion planning problem, collision detec-
2
tion must b e p erformed b etween O n di erent pairs of ob jects. Most ecient
collision detection systems use a two-phase approach to reduce the computa-
tional costs of making these checks. The broad phase culls away most of the
pairs using a trivial rejection test based on b ounding b oxes, b ounding spheres,
o ctrees, or the like. The culled pairs fall b elow a relevance threshold for the
higher level planning pro cesses. The pairs that survive this culling are passed
to the narrow phase, which uses a more re ned algorithm to check for collisions
or to compute distances. Since the broad phase basically acts as a lter for the
narrow phase, choices for the two algorithms can usually b e made indep endently.
This article describ es collision detection from a two-phase p ersp ective, ad-
dressing the broad phase in Section 2 and the narrow phase in Section 3. In
b oth cases, the article fo cuses on one particular approach, but brie y mentions
alternatives and their relative merits. Section 4 describ es a novel scheduling
paradigm for invoking the collision detection system that has advantages over
the more traditional scheme. Many of the algorithms discussed have imple-
mentations in the public domain. Section 5 contains p ointers to downloadable
co de.
It is imp ossible to recommend collision detection algorithms that are b est
for all applications. The ones emphasized here were e ectively used in the Im-
pulse simulation system, describ ed in detail in [19]. Impulse places demanding
requirements on the collision detection system that are similar to the require-
ments of many motion planning systems.
2 Broad phase collision detection
The function of the broad phase collision detection algorithm is to quickly re-
move most of the pairs of ob jects from consideration. Axes-aligned b ounding
boxes provide a simple but e ective approach for many applications. Boxes can
b e used to b ound the region of space o ccupied by an ob ject at a single instantin
time, but when the ob jects are moving, it is more useful to b ound the region of
MERL-TR-97-23 Decemb er 1997 3
Bi
ri
time t 0 ri
ri ri f ri ∆ time t 0 + t
ri
Figure 1: The b ounding b ox for ob ject i's swept volume during a segmentofa
parab olic tra jectory.
space o ccupied by the ob ject over a time interval. This swept volume approach
is easiest when ob jects follow simple tra jectories. Consider nding a b ounding
box B that encloses ob ject i over the time interval [t ;t +t]. Let r be
i 0 0 i
the maximum distance of any p oint on the ob ject from its center of mass, and
supp ose the center of mass follows a parab olic tra jectory over the interval in
question. B is found by noting the p osition of the center of mass at the time t ,
i 0
at the time t +t, and p ossibly at the ap ex of its parab olic tra jectory, should
0
this o ccur during the interval [t ;t +t]. The b ox which b ounds these twoor
0 0
three p oints is grown by r to obtain B Figure 1. In this case, computing B
i i i
is simple and takes constant time.
Other situations may not a ord such a simple approach to b ounding the
swept volume. One example is b ounding the swept volume o ccupied by the end
e ector of a serial chain manipulator with several links. In these situations,
one can use discretized p oints along the tra jectory to compute a b ounding b ox;
these p oints may be available from an integration routine at no extra cost.
Larger safety margins can be used to compute the box when there are larger
gaps between intermediate p oints. In motion planning applications, many of
the ob jects are often xed in space. For these ob jects, the tightest axes-aligned
enclosing b ox can b e computed once for all time.
If b oxes B and B do not intersect, then the corresp onding ob jects i and j do
i j
not p enetrate over the time interval [t ;t +t]. Ob ject pairs with intersecting
0 0
b ounding boxes are passed to the narrow phase collision detector for further
analysis. There are di erentways to nd intersections among the set of swept-
volume b ounding boxes. We rst describ e an algorithm based on a hashing
scheme for nding intersections among a set of static boxes in space. Next,
we mo dify the algorithm to exploit coherence among moving boxes. We also
MERL-TR-97-23 Decemb er 1997
4
discuss alternative metho ds based on co ordinate sorting.
2.1 Finding static box intersections
Point lo cation problems o ccur frequently in computational geometry. One vari-
ant is expressed as follows:
Problem 1 Point Lo cation Given a number of non-intersecting cel ls in
space, store the arrangement such that for a given query point p, the cel l con-
taining p if any can be determined eciently.
Here, a cel l is a connected region of space. Overmars presents two solutions
to this problem under certain restrictions in the cell shap e [23]. The more
ecient solution involves surrounding each cell by an axes-aligned b ounding
box, and storing the lo cation of these b oxes in a hash table. The technique can
b e extended to solve a more useful problem for collision detection:
Problem 2 Static boxintersections Given n axes-alignedrectangular boxes
B ;:::;B , xed in space, store this arrangement such that al l pairs of boxes
1 n
that intersect can be determined eciently.
To attack this problem, consider partitioning space into a cubical tiling with
resolution . Any p ointx; y ; z in space b elongs to a unique tile, sp eci ed by
integer co ordinates, under the tiling map :
3 3 2 2
bx=c x
5 5 4 4
1 ! by=c y
bz=c z
The tiles that the box B intersects are found by computing the images
i
under of two of B 's corners: the one of minimum x, y , and z co ordinates,
i
and the one of maximum x, y , and z co ordinates. The two tiles containing
these corners, and the other tiles \b etween" them, are the tiles that intersect
B . A tile with co ordinates a; b; c is between the two tiles with co ordinates
i