of Briankuffner.org/james/software/dynamics/mirtich/mirtich...king p erformed to detect aggressiv e...

Impulse�based Dynamic Simulation of Rigid Body Systems

by

Brian Vincent Mirtich

B�S�E� �Arizona State University� ��M�S� �University of California� Berkeley� ��

A dissertation submitted in partial satisfaction of the

requirements for the degree of

Doctor of Philosophy

in

Computer Science

in the

GRADUATE DIVISION

of the

UNIVERSITY of CALIFORNIA at BERKELEY

Committee in charge�

Professor John F� Canny� ChairProfessor David ForsythProfessor Alan Weinstein

Fall ��

The dissertation of Brian Vincent Mirtich is approved�

Chair Date

Date

Date

University of California at Berkeley

Fall ��

Impulse�based Dynamic Simulation of Rigid Body Systems

Copyright Fall ��

by


�

Abstract

Impulsebased Dynamic Simulation of Rigid Body Systems

by


Doctor of Philosophy in Computer Science

University of California at Berkeley

Professor John F� Canny� Chair

Dynamic simulation is a powerful application of today�s computers� with uses in �elds rang�

ing from engineering to animation to virtual reality� This thesis introduces a new paradigm

for dynamic simulation� called impulse�based simulation� The paradigm is designed to meet

the twin goals of physical accuracy and computational e�ciency� Obtaining physically ac�

curate results is often the whole reason for performing a simulation� however� in many

applications� computational e�ciency is equally important� Impulse�based simulation is

designed to simulate moderately complex systems at interactive speeds� To achieve this

performance� certain restrictions are made on the systems to be simulated� The strongest

restriction is that they comprise only rigid bodies�

The hardest part of rigid body simulation is modeling the interactions that occur

between bodies in contact� The most commonly used approaches are penalty methods�

followed by analytic methods� Both of these approaches are constraint�based� meaning

that constraint forces at the contact points are continually computed and applied to deter�

mine the accelerations of the bodies� Impulse�based simulation is a departure from these

approaches� in that there are no explicit constraints to be maintained at contact points�

Rather� all contact interactions between bodies are a�ected through collisions� rolling� slid�

ing� resting� and colliding contact are all modeled in this way� The approach has several

advantages� including simplicity� robustness� parallelizability� and an ability to e�ciently

simulate classes of systems that are di�cult to simulate using constraint�based methods�

The accuracy of impulse�based simulation has been experimentally tested and is su�cient

for many applications�

The processing of collisions is a critical aspect of the impulse�based approach� E��

cient algorithms are needed for detecting the large number of collisions that occur� without

missing any� Furthermore� the physical accuracy of the simulator rests upon the accuracy of

the collision response algorithms� This thesis describes these essential algorithms� and their

underlying theory� It describes how the algorithms for simple rigid body simulation may be

extended to systems of articulated rigid bodies� To prove the method is truly practical� the

algorithms have been implemented in the prototype simulator� Impulse� Many experiments

performed with Impulse are described�

Professor John F� CannyDissertation Committee Chair

iii

Contents

List of Figures vi

List of Tables ix

� Introduction �

�� Constraint�based contact models � � � � � � � � � � � � � � � � � � � � � � � � �� Non�penetration contact forces � � � � � � � � � � � � � � � � � � � � � �

�� The impulse�based approach � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Impulses versus constraints � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Overview of the thesis � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �

� Collision Detection ��

�� Introduction and related work � � � � � � � � � � � � � � � � � � � � � � � � � � �� Collision detection in Impulse � � � � � � � � � � � � � � � � � � � � � � ��

� The Lin�Canny algorithm � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Collision detection and coherence � � � � � � � � � � � � � � � � � � � � �� Extensions to the Lin�Canny algorithm � � � � � � � � � � � � � � � � �

� Prioritizing collision checks � � � � � � � � � � � � � � � � � � � � � � � � � � � �� The collision heap � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Estimating time of impact � � � � � � � � � � � � � � � � � � � � � � � � �� Bounding ballistic angular velocity � � � � � � � � � � � � � � � � � � �

�� Bounding box techniques � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Finding static box intersections � � � � � � � � � � � � � � � � � � � � � �� Coherence and the tiling scheme � � � � � � � � � � � � � � � � � � � � �� Maintaining the collision heap � � � � � � � � � � � � � � � � � � � � � �� Spatial hashing versus coordinate sorting � � � � � � � � � � � � � � � ��

� Generalizing collision detection � � � � � � � � � � � � � � � � � � � � � � � � � �

� Collision Response ��

�� Assumptions of collision response model � � � � � � � � � � � � � � � � � � � � � Computing collision impulses � � � � � � � � � � � � � � � � � � � � � � � � � � �

�� The equations of collision � � � � � � � � � � � � � � � � � � � � � � � � �� Sliding mode � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Sticking mode � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � ��

iv

� Collision integration � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Work done by collision forces � � � � � � � � � � � � � � � � � � � � � � �� Integrating collisions using di�erent parameters � � � � � � � � � � � � �� Sliding mode under uz and Wz parameterizations � � � � � � � � � � � �� Handling sticking during collision integration � � � � � � � � � � � � � �

�� Sticking stability and rays of constant sliding � � � � � � � � � � � � � � � � � �� The u� ellipse � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Directions of constant sliding � � � � � � � � � � � � � � � � � � � � � � �

� Static contact and microcollisions � � � � � � � � � � � � � � � � � � � � � � � � ��

� Constrained Body Dynamics ��

�� Constrained forward dynamics � � � � � � � � � � � � � � � � � � � � � � � � � �� Velocity and acceleration propagation � � � � � � � � � � � � � � � � � � � � � �� Spatial algebra � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � ��

�� Spatial formulation of acceleration propagation � � � � � � � � � � � � �� The Featherstone algorithm � � � � � � � � � � � � � � � � � � � � � � � � � � � ��

�� Base case � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Inductive case � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Forward dynamics algorithm � � � � � � � � � � � � � � � � � � � � � � ��

�� Extension to tree�like linkages � � � � � � � � � � � � � � � � � � � � � � � � � � �� Velocity and acceleration propagation through trees � � � � � � � � � �� Articulated inertias and z�a� forces for tree linkages � � � � � � � � � � �� Forward dynamics algorithm for tree linkages � � � � � � � � � � � � � ��

�� Extension to �oating linkages � � � � � � � � � � � � � � � � � � � � � � � � � � ��

� Hybrid Simulation ��

�� A spectrum of physical systems � � � � � � � � � � � � � � � � � � � � � � � � � �� Collision detection � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � ��

�� Constrained body swept volumes � � � � � � � � � � � � � � � � � � � � �� Constrained body TOI coe�cients � � � � � � � � � � � � � � � � � � � ��

� Collision response � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Collision response� a robotics perspective � � � � � � � � � � � � � � � � �� Articulated body collision dynamics � � � � � � � � � � � � � � � � � � �� Computing Ki � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Propagating impulses through multibodies � � � � � � � � � � � � � � � ��

�� Supporting control systems � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Types of controllers � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Controller scheduling � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Impulse�s control support architecture � � � � � � � � � � � � � � � � � �

� Computation of Mass Properties for Polyhedral Bodies ��

�� Rigid�body mass parameters � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Derivation of the algorithm � � � � � � � � � � � � � � � � � � � � � � � � � � � � �

�� Reduction to surface integrals � � � � � � � � � � � � � � � � � � � � � � �� Reduction to projection integrals � � � � � � � � � � � � � � � � � � � � ��

v

�� Reduction to line integrals � � � � � � � � � � � � � � � � � � � � � � � � �� Evaluation of integrals from vertex coordinates � � � � � � � � � � � � ��

�� Pseudocode and on�line C code � � � � � � � � � � � � � � � � � � � � � � � � � �� Test results � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � ��

� Examples and Results ��

�� Pure impulse�based simulation � � � � � � � � � � � � � � � � � � � � � � � � � �� Passive hybrid simulation � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Controlled hybrid simulation � � � � � � � � � � � � � � � � � � � � � � � � � � �� Execution times � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Estimating pose statistics � � � � � � � � � � � � � � � � � � � � � � � � � � � � ��

�� Background � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Quasi�static algorithms � � � � � � � � � � � � � � � � � � � � � � � � � �� Dynamic simulation � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Experimental results and discussion � � � � � � � � � � � � � � � � � � � �� Other part feeding experiments � � � � � � � � � � � � � � � � � � � � � ��

� Future Work ��

�� Paradigm switching � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Collision detection issues � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Interpreted control and object encapsulation � � � � � � � � � � � � � � � � � � �� Physical simulation for animation and VR � � � � � � � � � � � � � � � � � � � �

A Mathematical Preliminaries ��

A�� Vectors� matrices� and frames � � � � � � � � � � � � � � � � � � � � � � � � � � �A� Representing cross products as matrices � � � � � � � � � � � � � � � � � � � � �A� Rigid body dynamics � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �A�� Quaternions and integration of orientation � � � � � � � � � � � � � � � � � � �

Bibliography ��

vi

List of Figures

�� A disassembled double pendulum� � � � � � � � � � � � � � � � � � � � � � � � �� A system of falling dominos� � � � � � � � � � � � � � � � � � � � � � � � � � � � �� A table with non�unique contact force con�gurations� � � � � � � � � � � � � � �� A vibrational part feeder� � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Various contact modes between ball and terrain� � � � � � � � � � � � � � � � ��

�� A block diagram of Impulse�s collision detection system� � � � � � � � � � � � �� A polygon and its Voronoi regions� � � � � � � � � � � � � � � � � � � � � � � � � The Lin�Canny algorithm for polygons� � � � � � � � � � � � � � � � � � � � � �� The three�dimensional Lin�Canny algorithm� � � � � � � � � � � � � � � � � � �� The e�ect of coherence on Lin�Canny performance� � � � � � � � � � � � � � � �� A one�sided approach to �nding collision times� � � � � � � � � � � � � � � � � �� Impulse�s narrow phase collision detection system� � � � � � � � � � � � � � � �� Closest point velocities and time of impact� � � � � � � � � � � � � � � � � � � �� Finding a lower bound on the time of impact of convex bodies� � � � � � � � �� The body angular velocity vector remains on an ellipsoid� � � � � � � � � � � �� A one�dimensional example of a hierarchical spatial hash table� � � � � � � � �� A variant of the hierarchical hash table scheme� � � � � � � � � � � � � � � � � �� The swept volume bounding box during a ballistic trajectory� � � � � � � � � � �� The advance algorithm� � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Collision heap hysteresis� � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Broad phase collision check culling during a coin simulation� � � � � � � � � � �� A two dimensional version of I�COLLIDE�s bounding box check� � � � � � � �� A bad case for coordinate sorting� � � � � � � � � � � � � � � � � � � � � � � � �� The axes aligned bounding rectangles in the x�y plane for a cycler body� � � �

�� Velocity� force� impulse� and work during collision� � � � � � � � � � � � � � � �� A collision between two bodies� � � � � � � � � � � � � � � � � � � � � � � � � � �� Solution trajectories of the ODE system of Theorem �� Relative tangential velocity that increases during a collision� � � � � � � � � � � � Trajectories through relative contact velocity space for di�erent collisions� � �� The life cycle of a collision� and the collision integration� � � � � � � � � � � � �� A velocity �ow with two converging rays and no diverging rays� � � � � � � � ��

vii

�� A velocity �ow with two converging rays and two diverging rays� � � � � � � �� A sequence of transformations to generate the u� ellipse� � � � � � � � � � � � �� Testing whether the u� ellipse circles the origin �Lemma �� The lighthouse analogy for rays of constant sliding direction� � � � � � � � � �� Proving there can be no more than one diverging ray� � � � � � � � � � � � � �� The coe�cient of restitution is arti�cially increased� � � � � � � � � � � � � � �� Standard collision impulses will cause the block to creep down the ramp� � � �� Data from the block on ramp experiment using Impulse� � � � � � � � � � � � ��

�� The link and joint indexing conventions for serial linkages� � � � � � � � � � � �� Quantities for propagating velocities and accelerations� � � � � � � � � � � � � �� Expressing velocity and acceleration in di�erent frames� � � � � � � � � � � � �� The compSerialLinkVelocities algorithm� � � � � � � � � � � � � � � � � � � �� The free body diagram of the last link of the serial linkage� � � � � � � � � � �� The free body diagram of an inner link of the serial linkage� � � � � � � � � � �� The initSerialLinks algorithm� � � � � � � � � � � � � � � � � � � � � � � � � �� The serialFwdDynamics algorithm� � � � � � � � � � � � � � � � � � � � � � � �� A complex pendulum with a tree�like topology� � � � � � � � � � � � � � � � � �� The numberLinks algorithm� � � � � � � � � � � � � � � � � � � � � � � � � � � �� The compTreeLinkVelocities algorithm� � � � � � � � � � � � � � � � � � � � �� The free body diagram of a tree�linkage handle� � � � � � � � � � � � � � � � � �� The initTreeLinks algorithm� � � � � � � � � � � � � � � � � � � � � � � � � � �� The treeFwdDynamics algorithm� � � � � � � � � � � � � � � � � � � � � � � � � ��

�� A hinge joint� � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� A spectrum of physical systems� � � � � � � � � � � � � � � � � � � � � � � � � � � � Checking performed to detect aggressive bounding box violations� � � � � � � �� The updateSlack algorithm� � � � � � � � � � � � � � � � � � � � � � � � � � � �� A robot in contact with its environment� � � � � � � � � � � � � � � � � � � � � � �� The path for computing z�a� impulses� � � � � � � � � � � � � � � � � � � � � � �� The impulseResponse algorithm� � � � � � � � � � � � � � � � � � � � � � � � � �� The body frame of a colliding link and the collision frame� � � � � � � � � � � �� The compMultibodyKi algorithm� � � � � � � � � � � � � � � � � � � � � � � � � �� The propagateImpulse algorithm� � � � � � � � � � � � � � � � � � � � � � � � �� The entire procedure for resolving collisions involving a multibody� � � � � � �� A situation in which the multibody collision matrix is rank de�cient� � � � � �� The coupling of a low level controller to a multibody system� � � � � � � � � �� A PD controlled joint loaded with a static weight� � � � � � � � � � � � � � � � � �� The control support architecture of Impulse� � � � � � � � � � � � � � � � � � � �

�� The strategy for evaluating volume integrals� � � � � � � � � � � � � � � � � � � �� Maximizing a face�s projected shadow in the �� plane� � � � � � � � � � � � �� Notation for computing a projection integral as a sum of line integrals� � � � �� The CompVolumeIntegrals algorithm� � � � � � � � � � � � � � � � � � � � � � �� The CompFaceIntegrals algorithm� � � � � � � � � � � � � � � � � � � � � � � ��

viii

�� The CompProjectionIntegrals algorithm� � � � � � � � � � � � � � � � � � � ��

�� Snapshots from the dominos simulation� � � � � � � � � � � � � � � � � � � � � �� Snapshots from the block drop simulation� � � � � � � � � � � � � � � � � � � � �� Snapshots from the block on ramp simulation� � � � � � � � � � � � � � � � � � �� Snapshots from the chain of balls simulation� � � � � � � � � � � � � � � � � � �� Snapshots from the balls in dish simulation� � � � � � � � � � � � � � � � � � � �� Snapshots from the coins simulation� � � � � � � � � � � � � � � � � � � � � � � �� Snapshots from the pool break simulation� � � � � � � � � � � � � � � � � � � � �� The frequency of collision checks during a pool break� � � � � � � � � � � � � �� Snapshots from the bowling simulation� � � � � � � � � � � � � � � � � � � � � � �� Bowling experiments performed with Impulse� � � � � � � � � � � � � � � � � � �� Trajectories of a ball on a spinning platter� � � � � � � � � � � � � � � � � � � �� Snapshots from the rattleback top simulation� � � � � � � � � � � � � � � � � � �� Snapshots from the part feeder chute simulation� � � � � � � � � � � � � � � � �� Snapshots from the exploding text simulation� � � � � � � � � � � � � � � � � � �� Snapshots from the see�saw simulation� � � � � � � � � � � � � � � � � � � � � � �� Snapshots from the part sorter simulation� � � � � � � � � � � � � � � � � � � � �� Snapshots from the triple pendulum simulation� � � � � � � � � � � � � � � � � �� Snapshots from the furniture arrangement simulation� � � � � � � � � � � � � �� Snapshots from the bicycle simulation� � � � � � � � � � � � � � � � � � � � � � �� The kinematic structure of the bicycle� � � � � � � � � � � � � � � � � � � � � � �� The forward velocity pro�le as the bike traversed the obstacle course� � � � �� The roll angle pro�le as the bike traversed the obstacle course� � � � � � � � �� Snapshots from the creature war simulation� � � � � � � � � � � � � � � � � � � �� Side view of a bug and kinematic tree� � � � � � � � � � � � � � � � � � � � � � � �� Side view of the rover and kinematic tree� � � � � � � � � � � � � � � � � � � � � �� The high level state machine controller for the rover� � � � � � � � � � � � � � �� The C functions that implement the rover�s high level control� � � � � � � � � �� The Adept Robotics �exible part feeding system� � � � � � � � � � � � � � � � �� Estimating pose statistics using a sphere projection method� � � � � � � � � � �� The perturbed quasi�static method� � � � � � � � � � � � � � � � � � � � � � � � �� Parts used in the pose statistics experiments� � � � � � � � � � � � � � � � � � � �� A snapshot taken during a MEM motion array experiment� � � � � � � � � � �

�� The evolution of the states does not have to proceed in lockstep� � � � � � � �� Interpreted object speci�cation allows generality and portability� � � � � � � �� A conception of a creature design environment� � � � � � � � � � � � � � � � � �

ix

List of Tables

�� Comparison of hashing schemes� � � � � � � � � � � � � � � � � � � � � � � � � ��

�� Notation used in Chapter � � � � � � � � � � � � � � � � � � � � � � � � � � � � �

�� Theoretical values of volume integrals for simple test polyhedra� � � � � � � �� Data for successive approximations of a unit sphere� � � � � � � � � � � � � � ��

�� Simulation times for examples� � � � � � � � � � � � � � � � � � � � � � � � � � �� How the simulation processing time is spent� � � � � � � � � � � � � � � � � � �� Orange insulator cap data� � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� White stereo button data� � � � � � � � � � � � � � � � � � � � � � � � � � � � � �� Rectangular black stereo button data� � � � � � � � � � � � � � � � � � � � � � �� Square black stereo button data� � � � � � � � � � � � � � � � � � � � � � � � � ��

x

Acknowledgments

This thesis would not have been possible without the help of many people� I thank my

advisor� John Canny� who was an endless source of wisdom and inspiration� I thank the

many great friends who made my seven years at Berkeley an experience to be treasured�

Finally� I thank my family for their support� and especially Caroline� die mein Leben mit

Freude erf�ullt�

�

Chapter �

Introduction

The �eld of classical dynamics is one of the success stories of modern mathematics�

Dynamical systems exhibit a great deal of beautiful structure� and dynamicists have devel�

oped a body of knowledge that can predict the behavior of many of these systems to great

accuracy� The ability to know in advance how our universe or some part of it will evolve

is certainly one of the most powerful skills humans have learned� The modern computer�

with its power to perform computations at blinding speed and display realistic images� has

greatly increased our predictive power� But it has revealed holes in our knowledge as well�

places where our predictive power breaks down� Predicting with certainty the outcome of

a simple coin toss is at the limits of our current abilities� if not beyond them�

This thesis is about predicting the future� or more precisely� physical simulation�

It deals with very simple systems and very simple models� The systems are comprised of

rigid bodies� macroscopic deformations occurring in materials like cloth or rubber are not

allowed� Di�erent rigid bodies may however be connected by joints to form articulated

bodies� Friction is modeled according to the Coulomb law� restitution is modeled using a

single coe�cient� The latter provides a crude way of accounting for microscopic deformation

during a collision� Despite these simpli�cations� the predictive power is quite good for many

applications� One common but signi�cant simpli�cation is also dropped� the quali�cation

that one is only interested in the behavior of the system over some limitedmodeling interval�

Most textbook problems are concerned with the behavior of a system over an interval where

a single model applies� determine the velocity of the wrecking ball before it collides with the

wall� or� determine the acceleration of the hoop down the ramp� assuming slipping does not

occur� For the simulation problems addressed in this thesis� the analysis is not restricted

in this manner� The simulator must produce correct motion for whatever situation and

system the user describes� even in the presence of discontinuities produced by collisions� or

changes in contact state�

The external in�uences that a�ect the motion of physical objects can be divided

into two groups� based on whether or not they arise through physical contact� Forces due

to gravity� external electric �elds� or air resistance are examples of non�contact forces�� The

motion of isolated rigid bodies� subject only to these non�contact forces� is well understood�

For example� an accurate analytic model exists for the motion of a rigid body in a uniform

gravitational �eld� The remaining in�uences on the motion of objects result from contact�

The contacts between di�erent objects are where all of the interesting behavior arises�

and correct modeling of these contacts is critical to accurate physical simulation� Contact

modeling is also where all of the di�cult problems are� The models for contact are less

accurate and more equivocal than those for the dynamics of isolated bodies� Much of the

research in dynamic simulation has been devoted to contact modeling�

In studying contact interactions� there is the typical tradeo� between accuracy and

e�ciency� Finite element methods �FEMs�� and related approaches� are among the most

accurate methods for studying contact interactions� but they require signi�cant computation

time� They are not feasible� for example� in interactive simulation� where a user would like

to watch a system evolve at a speed near the true speed at which it evolves in the physical

world� This thesis is concerned with physically accurate simulation subject to the constraint

of interactive or even real time performance� The accuracy of the methods is less than the

accuracy of� for instance� �nite element methods� but the goals are di�erent� This thesis

demonstrates that dynamic simulations can be computed at interactive speeds� and retain

adequate physical realism and predictive power for real engineering applications� One of

the fundamental assumptions made for computational e�ciency is that all bodies are rigid�

Impulse�based simulation is a departure from the typical approach for performing

interactive dynamic simulation� The greatest di�erence lies in how the contacts are modeled�

Rather than computing explicit constraint forces to be applied at contact points� contact

interactions are modeled exclusively through collision impulses applied between bodies� To

understand why this approach might be useful� it is necessary to understand the standard

constraint�based approaches� and their limitations�

�Air resistance might be modeled as a contact force resulting from collisions of a body with air particles�But it is more common to model it as an aggregate non�contact force based on the body�s state�

�� Constraint�based contact models

Physically accurate� interactive simulation has received much attention in the �elds

of engineering and computer science� The standard approach uses a constraint�based model

for contact� an outgrowth of the classical model for constrained dynamics� The latter is

useful in computing the forward dynamics of articulated bodies� given the current positions

and velocities of all parts of a physical system� �nd the current accelerations of all of the

parts� Consider� for example� the planar double pendulum shown in Figure �� Suppose

x1

x2

x3

x4

f1

f2

f3

f4

Figure �� A disassembled double pendulum� Although the pendulum is an articulated rigidbody� its dynamics can be derived from the formulation for simple rigid bodies� �rst theconstraint forces f i are computed� and then the links are treated as individual rigid bodies�

x� and x� are points that are on di�erent bodies of the pendulum� but which are attached

at a joint� However the two links move� the constraint

x� � x� � ��

�

must be enforced� This is done by introducing constraint forces that are transmitted through

joints� maintaining the constraints� Once the constraint forces are known� the links of the

system may be analyzed separately� and their motion determined from the Newton�Euler

equations of rigid body motion� Some methods do not explicitly solve for the constraint

forces� but produce the link accelerations directly� Usually the value of the constraint forces

is not important� as long as the constraints are enforced� so these methods are suitable� and

even preferable if computations can be reduced� General applications of the constraint�based

approach to dynamic simulation and modeling are in �BB�� CS�� Gle�� IC�� WGW��

�� Non�penetration contact forces

Sometimes bodies are not permanently attached to each other� but come into con�

tact during the course of their motion� The row of dominos in Figure �� is an example�

These contacts give rise to non�penetration constraints� Unlike �� a non�penetration

Figure �� For this system of dominos� there are no permanent constraints between bod�ies� but the contacts �indicated by dots� give rise to temporary non�penetration constraintsbetween bodies�

constraint is fundamentally an inequality� rather than an equality� that involves the acceler�

ations of the bodies� In addition� there are restrictions on the constraint force� it can only

act in a direction which tends to push the objects apart� not pull them together� If a pulling

force is required to hold the objects in contact� the contact will merely break� For these

reasons� non�penetration constraints can not be handled using the classical approaches for

constrained dynamics�

There have been two main approaches to modeling non�penetration with con�

straints� The older and simpler approach is the family of penalty methods� These methods

do not strictly enforce non�penetration� instead� they keep penetrations small enough so

that they are not noticeable relative to the scale of the system� With this practical goal

in mind� it is not surprising that the easy�to�implement penalty methods have dominated

the computer animation �eld� where motion that �looks right� is often su�cient� Penalty

methods vary in details� however the rough idea is the same� A sti� spring is attached

at the contact� so that as two bodies move into one another� the spring attempts to push

them apart� The more penetration� the stronger the restoring force� If the bodies move

apart� the spring is destroyed� Penalty methods are used for rigid body simulation in

�RH�� MW�� MZ�� and are also commonly employed in deformable body simulation�

Penalty methods have a few problems� The very large spring constants that are

needed to keep the penetration su�ciently small generate large forces leading to sti� equa�

tions that are numerically intractable to integrate �WGW�� Integration problems are

exacerbated by the fact that springs can be created and destroyed in a very transient

manner� Furthermore� choosing the spring constants is a black art� Constants that are

satisfactory for one situation may be too high or too low for another� and the problem

becomes worse when multiple bodies are in contact simultaneously� These spring constants

do not correspond to familiar physical properties that can be measured� like the coe�cient

of friction� Another subtle problem of penalty methods arises because of the way collision

detection is typically performed� Usually collision checks are done at discrete times rather

than continuously over time intervals� As a result� contact is not detected until a �nite

amount of penetration has occurred� Under penalty methods� this causes forces that are

larger than the true forces in the continuous system� causing instability�

Alternatives to the penalty methods are the analytic �or exact� methods for com�

puting non�penetration contact forces� �rst studied by L�otstedt �L�ot�� The idea is to cast

the non�penetration constraints as a linear complementarity problem �LCP��

a � Af � b �

f � ��

fTa � ��

Here� a is a vector of the relative normal accelerations at the contact points� and f is a

vector of corresponding normal force components at these contacts� A and b are matrix

and vector constants which are determined from the known con�guration of the system�

�For the cursory treatment given here� Bara��s simpler notation is used �Bar��

�

The constraint a � � prevents motion that would cause penetration at the contact points�

The constraint f � � means that contact forces can only try to push bodies apart� not hold

them together� The complementarity constraint fTa � � means that at each contact point�

either a force is acting and the relative acceleration is zero� or the bodies are separating�

and the force is zero� The former case means the contact will persist beyond the current

instant� the latter means the contact is ending� L�otstedt gives an algorithm for solving the

LCP �� based on the principal pivoting method of Cottle and Dantzig �see �CPS�� for

a detailed treatment of LCPs�� L�otstedt�s algorithm in the context of rigid body simulation

is described in �L�ot�� Bara� has also used LCP approaches for simulation of rigid body

systems �Bar�� Bar�� Bar��

When there is no friction in the system� the LCP �� always has a solution for the

contact forces� and this solution is unique� subject to certain non�degeneracy constraints

�Bar�� When friction is added to the model� the corresponding LCP may not have a

solution� and if a solution exists it may not be unique� L�otstedt �L�ot�� gives existence and

uniqueness conditions� which Bara� �Bar�� generalizes� The most comprehensive results on

the existence and uniqueness of solutions for rigid body contact forces� using the Coulomb

friction model� are given by Pang and Trinkle �PT�� and Trinkle� et� al� �TPSL�� Roughly

speaking� existence and uniqueness can be guaranteed if the coe�cients of friction are small

enough� Certain problems� such as the static stability problem in which the bodies in the

system are at rest� always have solutions�

When there is no friction� the LCP is convex and solutions can be computed using

algorithms that run in worst case exponential time but expected polynomial time in the

number of contacts� Convexity breaks down with the addition of friction� Bara� proved that

the problem of determining the existence of a valid set of contact forces that obey Coulomb

friction laws at the contacts is NP�hard when there is sliding friction� To avoid this problem�

Bara� suggests a new model for consistency that includes non�colliding impulsive forces at

some of the contacts� Under this model� he gives an algorithm to �nd a valid solution to

the contact force problem� Its complexity is not known� although it appears to be practical

in many examples �Bar�� Bar�� None of the algorithms proposed for computing contact

forces with friction are guaranteed to correctly terminate� Trinkle� et� al� give quantitative

results using an approach based on Lemke�s algorithm �this is also Bara��s approach�� and

also a feasible interior point method �TPSL�� In general� the reliability of the algorithms

decrease as the number of contact points increase�

�

Approximations are made to the Coulomb friction law in order to form an LCP

for the contact forces� Under sticking conditions� Coulomb friction imposes the nonlinear

restriction

kf tk � �kfnk�

where f t and fn are the tangential and normal forces at the collision point� To cast this

into the LCP framework� a linear approximation to the friction cone is typically employed�

such as a friction pyramid� This has the e�ect of making friction anisotropic for three�

dimensional systems� Another common approximation is that upon the transition from

sticking to sliding� the frictional forces are only partially �instead of directly� opposed to

the tangential acceleration �Bar�� PT��

Even without friction� analytic methods may not �nd the correct set of contact

forces� due to contact degeneracy� Consider the idealized system of a table resting on a

�oor� where both objects are perfectly rigid� the table leg lengths are equal� and the �oor

is �at �Figure �� Here� there are multiple sets of contact forces satisfying the constraints

of the LCP� the forces are all positive� and prevent acceleration of the table into the �oor�

Without friction� the acceleration of the system is provably unique even when the contact

w w w

w/2 w/4w/2w/4

w/4w/4

(a) (b) (c)

w/8 3w/8

3w/8 w/8

Figure �� For a symmetric table of weight w� there are in�nite contact force con�gurationsthat satisfy the non�penetration LCP� Three are shown here�

forces are not �Bar�� in all three cases of Figure �� the net acceleration of the table

is � Analytic methods may not be reliable� however� for computing the contact forces�

Force con�guration �c� of Figure �� is the correct one� scales placed under each leg of the

table would all read the same weight� Analytic methods can compute di�erent solutions�

such as those shown in parts �a� and �b� of the �gure� because the contact con�guration is

�

degenerate� On the other hand� penalty methods� and the impulse�based method described

in the next section� always compute con�guration �c�� The idealized case is an important one

to handle correctly� it is mathematically natural and the one often employed in simulations�

It is an accurate approximation of certain real situations� for instance� when the table

lengths are approximately equal� and the contacts are soft� This would be the case if the

table leg tips are deformable� as on a folding table� or if the table rests on carpet or linoleum�

If the table and �oor are hardwood� and the leg lengths are di�erent or the �oor is warped�

then the idealized model is not accurate� as the true forces on the legs may vary greatly�

Such a system is not degenerate� and any of the simulation methods described will compute

the correct contact forces�

With friction� there may be multiple contact force solutions that do not lead to the

same accelerations� This indeterminacy that can arise in an analytic solution seems counter�

intuitive to a deterministic view of the world� The inconsistency is even more disturbing�

how can there be no �valid� solutions to the contact force problem for a physical system�

Certainly� these forces always exist and are measurable for any physical system one can

construct� These anomalies of the contact LCP re�ect the limitations of the rigid body

assumption and the Coulomb friction law� which are only approximations to reality��

In dynamic simulation� exploiting coherence of the problem instances becomes

critical for analytic methods� Solving a linear program is more di�cult than solving a

linear system of the same size� and solving LCPs is harder still� Solving a fresh linear

complementarity problem at every time step would be prohibitively slow� Much e�ort can

be saved by using the results from the previous time step� For example� in the pivoting

algorithms of L�otstedt and Bara�� basic solutions to the LCP can be used from the previous

time step� if they change relatively infrequently� Even more important� when friction is

present there may be multiple solutions to the LCP� In order to preserve continuity of

the accelerations of the bodies� the solution at the current time step must be computed

with knowledge of the solution at the previous time step �Bar�� Practically speaking� this

dependence on contact coherence is perhaps the greatest limitation of the analytic methods�

�Tangentially related to this discussion is work of Peshkin and Sanderson �PS�� who have elegantlycharacterized the motion of parts sliding on a at surface in the presence of Coulomb friction� Assumingnothing about the pressure distribution on the part� they compute an envelope of possible instantaneouscenters of rotation� thereby bounding the possible motions of the part� Unfortunately� their method is basedon a quasi�static assumption and the minimum power principle �PS�� which prevents application to manydynamic systems�

�

�� The impulse�based approach

Consider the vibrational part feeder of Figure �� This machine shakes parts into

Figure �� A vibrational part feeder that shakes parts into recesses to be picked up by amanipulator� The system exhibits very transient contact modes�

recesses so that they can be picked up by a manipulator� Simulating this device requires

modeling the extremely transient contacts between the parts and the feeder� The contact

interactions are dominated by collisions� The con�gurations of non�colliding contact exist

only for short time intervals before changing� In short� the coherence between time steps

that the analytic methods need for e�ciency is destroyed� This part feeder example was

one of the original examples that inspired the research described in this thesis� A method

was desired for e�ciently simulating these types of systems� The result is impulse�based

simulation�

The fundamental idea of impulse�based simulation is that all contact between

bodies is modeled through collisions at contact points� Non�penetration constraints do

not exist� collisions are what enforce non�penetration between di�erent bodies� Between

��

collisions� the bodies move along ballistic trajectories� in�uenced only by gravity� As might

be expected� the method adeptly handles systems like the parts feeder described above�

where the bodies truly are unconstrained most of the time� and collisions are frequent� More

surprising is the fact that impulse�based simulation can be used to model cases of continuous

contact� it is a viable and powerful simulation paradigm for many applications� In the case

of a book resting on a table� the corners of the book experience rapid collisions with the

table� which prevent the bodies from penetrating� The rolling ball of Figure �� rolls along

the ground and up the ramp� becomes airborne� then bounces along the ground� eventually

settling into a roll� sliding between the ball and terrain may also occur at various points

during this evolution� Under an impulse�based model� all of these modes and transitions are

handled by processing collisions� A macroscopic rolling constraint is never enforced� Rather�

the macroscopic behavior results from treating the underlying collisions with a physically

accurate model�

Figure �� Under impulse�based simulation� the various contact modes between the ball andterrain� and the transitions between them� are a�ected through a physically accurate modelfor collisions�

Hahn pioneered this simulation paradigm� This thesis expands upon his work� and

improves the methods for collision detection and collision response described in �Hah��

The collision detection and response modules are critical components of an impulse�based

simulator� due to the central role collisions play in the simulation� These modules must be

very fast� due to the frequency of collisions� and also physically accurate� since collisions

are the sole means by which contact forces are transmitted�

The top level impulse�based simulation loop is extremely simple� It comprises

three steps�

�� Collision detection Determine a maximum time interval over which the physical

system may be integrated� such that no collision between two bodies will be missed�

At the end of the interval� a pair of bodies called the critical pair must be checked for

��

collision�

� Dynamic evolution Evolve the system forward in time by integrating the equations

of motion for each body� For rigid bodies� this trajectory is a simple ballistic one�

� Collision response Check to see if the critical pair of bodies has collided� If so�

compute and apply a pair of collision impulses to them�

These three steps are repeated throughout the course of the simulation� The

collision detection module works by maintaining an estimated time of impact for each pair

of nearby bodies� This is only an estimate� and it is re�ned as the bodies move closer� but it

is guaranteed to be conservative� the bodies can not collide any sooner than the computed

time of impact� The integration step is determined by the earliest time of impact between

any pair of bodies� The bodies that have the minimum time of impact become the critical

pair�

Once the integration interval is determined� all of the bodies in the system can

be evolved without regard to collisions� since none will occur during the interval� More

precisely� the surfaces of any bodies that collide during the interval will still be touching

�within some speci�ed tolerance� at the end of the interval� The dynamic evolution step

is very simple because the bodies are treated as unconstrained�� and so their motion is

easy to compute� For example� isolated rigid bodies follow ballistic trajectories during the

integration step� The dynamic integration step is highly parallelizable since the bodies are

moving independently during the integration� In a computer with multiple processors� the

bodies can be parceled among the processors� and the entire system can be integrated with�

out any need for inter�processor communication� This can result in a substantial speedup

for physical systems with many objects� It is an advantage over constraint�based methods�

which need the state information of many bodies in order to compute the motion of each

one�

If the critical pair of bodies are touching after the integration step� and have ve�

locities that indicate a collision should occur at the contact point� the collision response

module computes a pair of equal and opposite collision impulses to apply at the contact

points� These impulses instantaneously change the velocities of the colliding bodies� send�

�This statement will be relaxed slightly in the context of hybrid simulation� But in both pure impulse�based simulation and hybrid simulation� there is no interaction between bodies through non�penetrationconstraints�

�

ing them along new trajectories� For physical realism� the collision response module should

account for friction and energy loss during collision� Much of the work in rigid body sim�

ulation uses a simple model for collision resolution� The typical model assumes pre� and

post�collision velocities can be related with algebraic equations� without regard to the un�

derlying physical processes that occur during collision� Simple restitution laws that can

add energy to the system are typically used� For impulse�based simulation� the collision

response model is critical to the physical accuracy of the simulator� and better methods are

needed� Collisions are resolved by integrating the dynamic state according to di�erential

equations that hold during the colliding contact� These equations are derived directly from

the underlying frictional and restitutional laws� they are more accurate than the algebraic

approximations�

One might argue that impulse�based simulation is not a valid contact model� A rock

sitting on a table is not undergoing a series of collisions� It�s just not moving� period� This

is certainly true� although all approaches to simulation make approximations and deviate

from reality� especially those that achieve interactive simulation� The question is� what

information is desired from the model� Summing the collision impulses delivered from the

ground to the rock over some time interval� and dividing by the time interval� will yield the

same average force delivered to the rock as a constraint�based approach would� The average

velocity of the center of mass of the rock also vanishes� as in constraint�based methods�

Finally� the piecewise ballistic trajectory of the rock results in little displacement of the rock

from its average position� With a small enough collision tolerance� the displacement can be

made less than a pixel� so that the motion of the rock on a graphics display is imperceptible�

Although the impulse�based model deviates from reality� it preserves important physical

quantities related to forces and the motion of bodies�

Based on the literature� the impulse�based method has been more carefully tested

against reality than the constraint�based interactive simulation methods� Many comparisons

of impulse�based simulation to physical experiments or theoretical predictions are given

later in this thesis� More comparisons to physical experiments are certainly needed� for

both simulation paradigms�

�� Impulses versus constraints

There are several advantages of the impulse�based approach�

�

� No contact coherence required Analytic methods rely on contact coherence for

interactive speed� and even more importantly� for evolving the system in a physically

plausible way when multiple solutions exist to the contact LCP �Bar�� Contact

coherence is not required for impulse�based simulation� extremely transient contact

modes cause no slowdown� This is especially useful in situations where objects settle

as they make a transition to a resting state�

� Computational robustness As with the penalty methods� nothing can go wrong

in computing contact interactions� Given the dynamic states of the colliding bodies

and the material dependent parameters� a unique impulse to resolve the collision can

always be computed� the collision resolution algorithm always terminates with a valid

solution� The NP�hard problem of computing �non�colliding� contact forces is avoided�

as are the indeterminacy and inconsistency associated with analytic methods� The

sti� di�erential equations of penalty methods are also avoided�

� Colliding contact naturally handled With constraint�based methods� collisions

must be handled separately� before any contact force analysis begins� But under

impulse�based simulation� colliding contact is naturally handled� since collisions are

used to model all contact�

� Non�penetration strictly enforced Like analytic approaches but unlike penalty

methods� impulse�based simulation strictly enforces non�penetration� In addition� if

a heavy object and a light object are sitting on a table� both will experience collisions

with the table at roughly the same frequency� the heavy object does not require more

impulses�

� Decoupling of bodies Unlike constraint�based approaches� bodies are evolved inde�

pendently during dynamic integration under impulse�based simulation� This natural

decoupling facilitates parallelization� Collisions are also processed using a completely

local model� so the global state of a system of contacting bodies is not needed to

compute the impulses� A description of preliminary work in parallel impulse�based

simulation is in �Pau� ��

� Simple design The impulse�based paradigm is conceptually simpler than analytic

methods� making it easy to code� Advanced numerical methods are not required� All

��

macroscopic contact modes �rolling� sliding� resting� and colliding� are handled with

a single� local collision response algorithm�

� Physically valid contact forces Like penalty methods� the impulse�based method

is not subject to the contact degeneracies that analytic methods are� In the example of

Figure �� the physically valid contact forces will be computed� �Forces are computed

by time averaging impulses over a short interval��

� Extension to constrained bodies The methods of pure impulse�based simulation

are easily extended to handle systems of rigid bodies linked together by joints�

Though not an inherent property of impulse�based simulation� the collision re�

sponse model presented in this thesis is a more accurate model than those typically em�

ployed in interactive simulation� It would be useful in other simulation paradigms as well�

There are also disadvantages to the impulse�based method as compared to constraint�based

methods� In particular�

� Poor handling of stable and simultaneous contact Impulse�based methods are

less e�cient than constraint�based methods for certain types of prolonged� stable or

simultaneous contact� in some cases the impulse�based methods are not even feasible�

The canonical example is a stack of blocks at rest�

� Static friction creep Artifacts of the impulse�based approach appear when mod�

eling static friction in certain cases� A block will creep down a ramp� even if static

friction should hold it in place�

These limitations are signi�cant� and this thesis does not claim that the impulse�based

approach is a replacement for constraint�based approaches� There are cases where impulse�

based simulation is the most natural approach for modeling non�penetration� and where

it leads to faster simulation speeds �based on reported data�� The same can be said of

constraint�based methods� In the literature� it is evident that constraint�based approaches

have been applied to problems they are well suited to� and the same is true of impulse�based

approaches� Any contest between the two methods would be decided before it began by

the choice of physical system to simulate� Furthermore� such a contest would be pointless�

since the methods are complementary more than they are competing� the situations for

which one approach works poorly are exactly those for which the other approach works

�

well� Previous work has demonstrated what can be done with constraint�based techniques�

less has appeared on what can not be done easily� One purpose of this thesis is to describe

what can and can not be done with impulse�based techniques�

Much of this thesis deals with theoretical issues surrounding impulse�based simu�

lation� however� there is a practical component as well� The ideas discussed in the thesis

have been implemented in Impulse� a prototype impulse�based dynamic simulator� Impulse

served as a proof by example during the research phase of this thesis� only through imple�

mentation could the techniques be proven practical� While many of the discussions refer to

this speci�c implementation� they apply to any impulse�based simulator�

�� Overview of the thesis

The remainder of the thesis is organized as follows�

Chapter � describes collision detection in the context of impulse�based simulation�

using the broad phase and narrow phase detection systems in Impulse as examples� In

particular� the chapter discusses how collision checking may be performed conservatively�

so that collisions are guaranteed to be detected�

Chapter � describes the collision response model used in Impulse� A physically

accurate response model is critical to accurate impulse�based simulation� The model incor�

porates friction and restitution� and analyzes the dynamics that occur during a collision�

Chapter � derives Featherstone�s algorithm for the forward dynamics of con�

strained bodies from the �rst principles of Newtonian physics� This is a key ingredient

for the simulation of constrained bodies� and also serves as the basis for constrained body

collision response�

Chapter � describes hybrid simulation� a method of applying impulse�based sim�

ulation to systems of constrained rigid bodies� It discusses necessary modi�cations to the

collision detection and collision response algorithms described earlier� as well as a framework

for supporting control and behavior systems�

Chapter � is self�contained� and presents algorithms for computing the center of

mass and moments of inertia for arbitrary uniform�density polyhedral bodies� The algo�

rithms are very fast and naturally minimize numerical errors� they are useful for any rigid

body simulator�

��

Chapter � discusses many examples of simulations and experiments performed

with Impulse� illustrating the strengths and weaknesses of the impulse�based approach�

Accuracy is analyzed qualitatively and quantitatively� execution times are provided� and

several real applications from the part�feeding domain are discussed�

Chapter � concludes the thesis and discusses future work�

Appendix A provides a variety of basic information relevant to dynamic simula�

tion� including the Newton�Euler equations for rigid body motion� and how orientation is

parameterized and evolved using a quaternion representation�

��

Chapter �

Collision Detection

A fundamental problem in the simulation of physical systems is the collision detec�

tion problem� determining when and where two bodies come into contact� This problem has

its roots in computational geometry and robotics� Typically� in the former setting� a group

of static objects are tested for intersection� while in the latter setting� the time of contact

must be reported between bodies following trajectories that are closed form functions of

time� In simulation� the physical system is certainly not static� and the paths of the objects

are described with di�erential equations instead of closed form trajectories� The speed re�

quirements that interactive simulation demands are also formidable� Hahn found collision

detection to be the bottleneck in simulation of physical systems �Hah�� in his experiments�

collision detection often accounted for over � � of the computation time� Collision detec�

tion algorithms have improved since then� however� they remain the bottleneck in many

situations�

This chapter describes how collision detection may be performed in an impulse�

based simulator� using the system in Impulse as an example� The main contribution is an

algorithm than can e�ciently detect a large number of collisions� and that is guaranteed not

to miss any collisions� To meet this guarantee� lower bounds on the time of impact of two

ballistic rigid bodies are derived from the principles of dynamics� Methods of generalizing

this approach to other types of motion are described� This chapter also develops a fast

bounding box technique for culling most of the collision checks in a simulation with many

bodies� Two variants of the algorithm are described and compared� and the algorithm is

also compared to other bounding box techniques�

��

�� Introduction and related work

Hubbard gives a comprehensive reference list of work in collision detection �Hub��

Early approaches to collision detection for simulation solved problem instances from scratch

at every time step� An example is the polyhedral collision detection algorithm of Moore

and Wilhelms that is O�n�� where n is the number of features on both polyhedra �MW��

Hahn�s method tests every edge of one polyhedron against every face of the other when

bounding boxes can not guarantee the absence of collision� and therefore also has quadratic

complexity �Hah�� These types of algorithms are too slow for many simulation applica�

tions� Using bounding boxes or spheres helps� but is not enough� Gilbert� et� al� proposed a

O�n� algorithm for determining the distance between convex polyhedra� which also provides

a measure of penetration when polyhedra overlap �GJK�� The biggest improvement over

these early algorithms came from the realization that coherence could be used to greatly

reduce the computations� In simulation� the collision detection system solves a series of re�

lated problems� each one only slightly di�erent than the one before� For convex polyhedra�

coherence is combined with locality� using local properties to verify separation between

bodies� or lack thereof� Gilbert� et� al� describe an adaptation of their algorithm to take

advantage of coherence when it exists� however the reported speedups are fairly modest

�GJK�� Coherence is used more e�ectively in Bara��s witness plane algorithm �Bar��

and is thoroughly exploited in the Lin�Canny closest features algorithm �LC�� Lin��

For simulation� the collision detection algorithm is only called at discrete sample

points� Even if invocation occurs more than once per frame� it is still possible to miss

collisions� One pathological example is a bullet speeding toward a thin wall� No matter

what the minimum sampling period of the collision detection system �theminimum temporal

resolution �Hub�� one can choose a bullet speed and wall thickness such that the bullet

passes completely through the wall between collision checks� One correct solution is to

apply detection algorithms to the four�dimensional hyper�polyhedra swept out in space�time

�Can�� These methods are too slow and have not been used in any dynamic simulators

described in the literature� Most simulators systems handle the problem of missing collisions

by ignoring it �CS�� MW�� Hah�� Bar��

Ignoring the problem is not a good solution� especially in an impulse�based paradigm�

where collisions are used to model contact forces� Von Herzen� et� al� �HBZ�� present an

algorithm that uses Lipschitz bounds to derive limits on how far parts of a parametric sur�

��

faces can move over a time interval� their system is guaranteed to catch all collisions� The

bounds must be supplied by the user when the surface is de�ned� The algorithm of Snyder�

et� al� uses an interval version of Newton�s method for root �nding to achieve the same

goal� guaranteeing that the very next collision will be detected �SWF�� Here� Lipschitz

bounds are not needed since the exact trajectories of the surfaces over time are input data

for the problem� These approaches are similar to the one used in Impulse and discussed in

this chapter� Lower bounds on the times of impact between bodies are maintained and used

to ensure collisions are not missed� The user is not required to provide Lipschitz bounds

nor the exact trajectories of the bodies over time� Physical laws� such as the conservation

of momentum� provide enough information to bound the times of impact� Like the systems

described in �HBZ�� SWF�� Impulse uses a heap structure to schedule collision checks

between bodies�

�� Collision detection in Impulse

For computational speed� Impulse restricts bodies in the simulation to be rigid

and polyhedral� Rigidity allows much computation to be performed ahead of time� only

once� The polyhedral restriction is is fairly mild one since any more general shape� such

as a parametric surface or CSG�style solid� can be approximated to arbitrary closeness

with a polyhedral model� Of course the complexity of the polyhedral model may grow very

large� but the algorithms used by Impulse are quite insensitive to this complexity� Impulse�s

collision detection system is exact� meaning that the underlying polyhedral models of the

objects are eventually used if the objects are close enough� These methods are in contrast to

approximate methods� which are less concerned with locating the exact collision point and

more concerned with performance� Hubbard�s method based on bounding sphere hierarchies

is one approximate method which is able to perform collision checking between very complex

bodies at frame rates �Hub�� Hubbard makes a good case for using approximate methods

in many time�critical applications� Such approximate methods were not used in Impulse�

which was designed to have value as a simulation tool� Inaccuracies in the position of

contact points can have large repercussions in the course of the simulation�

E�cient collision detection systems employ some type of multi�level strategy�

Bounding boxes� bounding spheres� octrees� or similar methods are used to prune most

of the potential collision checks between a group of bodies� A more sophisticated but ex�

�

pensive collision detection algorithm is applied to pairs of bodies that can not be dismissed

by the simple technique� This chapter uses Hubbard�s terminology� referring to these two

levels of checking as the broad phase and the narrow phase� For a simulation with n bodies�

the broad phase prunes most of the O�n�� pairs� and the narrow phase is applied to the

pairs that remain�

In the broad phase� Impulse computes bounding boxes for the swept volumes

of bodies over a time interval� and �nds intersections of these boxes using a hierarchical

hash table� The swept volume technique is di�erent than a four�dimensional space�time

intersection test in two respects� First� the bounding boxes are only conservative estimates

of the swept volumes of the bodies� with much simpler geometry than the true swept

volumes� Second� since the boxes are not being used to compute the time of collision� but

only to signal that one might have occurred during some time interval� one can project the

true swept volume in space�time along the time axis� into the physical space� The analysis

is simpli�ed by working in three�dimensional physical space rather than in four�dimensional

space�time� The hashing technique employed for detecting bounding box overlaps is based

on a strategy Overmars proposes for solving the static point location problem �Ove��

Impulse�s narrow phase collision detection is based on the Lin�Canny algorithm�

It is one of the fastest algorithms known for tracking the closest features between convex

polyhedra� in a setting where coherence can be exploited� The algorithm�s output can

easily be used to compute the distance between the polyhedra� which serves as a basis for

collision detection� The very code which implements the Lin�Canny algorithm in Impulse

was subsequently used for the low�level core of the I�COLLIDE collision detection package

for large�scale environments �CLMP� ��

Both the broad and narrow phases of Impulse�s collision detection system rely

heavily on dynamics as well� Dynamics are used to compute bounding boxes that enclose

bodies� swept volumes during the broad phase� Dynamics are used to determine when the

next collision check should be performed between a pair of objects during the narrow phase�

in a way that guarantees collisions will not be missed� This coupling between collision de�

tection and dynamics is unfortunate from a modularity standpoint� Collision detection can

not be isolated as a strict geometric problem� the detection system must have information

about the dynamics of the simulated bodies� The coupling� however� allows for a very

e�cient collision detection system� tailored to the needs of dynamic simulation�

A block diagram of Impulse�s entire collision detection system is shown in Fig�

�

ure �� The Lin�Canny algorithm is used to compute geometric information about the

inactive object pairs

dynamic state

TOIestimator

bounding boxcomputer

31 2 5

2

1 55 64

1−2

active collision heap

2−5

5−6

1−5

1 2 3 4 5 6

close

counters

Lin−Canny

hash tablehierarchical

Figure �� A block diagram of Impulse�s collision detection system� Arrows representdata ows� Numbers designate indices of bodies in the simulation� The Lin�Canny algo�rithm� TOI estimator� and active collision heap perform narrow phase collision detectionthe bounding box computer� hierarchical hash table� and close counters are used for the broadphase�

separation of a pair bodies� it is described in detail in Section �� This geometric informa�

tion� plus the dynamic state of the bodies is used to estimate a time of impact �TOI� for the

pair� Pending collision checks between pairs of bodies are maintained in the collision heap�

sorted by the pair�s TOI� Time of impact estimation and the collision heap are discussed in

Section �� The bounding box computer� hierarchical hash table� and close counters form

the broad phase collision detection system� They typically prune most of the pairs of bod�

ies from the active collision heap using inexpensive tests� These components are described

in Section �� The section also presents a comparison between the spatial hashing tech�

nique used in Impulse�s broad phase� and a common technique based on coordinate sorting�

Throughout most of the chapter� the focus is on collision detection between ballistic bodies�

which are ubiquitous in impulse�based simulation� Section � describes how the algorithms

may be extended to more general types of motion�

�� The Lin�Canny algorithm

The heart of Impulse�s collision detection scheme is the Lin�Canny closest features

algorithm �Lin�� LC�� an extremely fast method for tracking the features �faces� edges� or

vertices� between a pair of convex polyhedra moving through space� The principle behind

the algorithm is best described with a two�dimensional example� A fundamental concept

in the Lin�Canny algorithm is that of a Voronoi region� Consider the polygon shown in

Figure �� The polygon has eight features� four vertices and four edges� For each

e1

3v

4e

e3e2

v2

v1v4

V( )v1

V( )e3

Figure �� A polygon and its Voronoi regions�

feature F � the set of points closer to F than to any other feature of the polygon is called

the Voronoi region of F � and denoted V �F �� The shapes of the Voronoi regions are easily

deduced for polygonal bodies� From each vertex� extend two rays outward from the polygon�

each perpendicular to one of the edges incident to the vertex� These rays form boundaries

between the Voronoi regions� The Voronoi region of a vertex is the in�nite cone lying

between the two rays emanating from that vertex� The Voronoi region of an edge is the

semi�in�nite rectangle lying between two parallel rays passing through the edge�s endpoints�

Collectively� the Voronoi regions partition the space outside the polygon�

Theorem � Given non�intersecting polygons A and B� let a and b be the closest points

between feature Fa of A� and feature Fb of B� respectively� If a and b are the closest points

between A and B� then a � V �Fb� and b � V �Fa��

Proof� Suppose a �� V �Fb�� Then a is in some other Voronoi region� say V �Fc�� and a is

closer to Fc than to any other feature on B� Since b � Fb� b �� Fc� and so a and b can not

be closest points� A similar results holds if b �� V �Fa��

The fundamental basis of the Lin�Canny algorithm is the converse of Theorem �

which is true for convex objects �see �Lin�� for the proof��

Theorem � Given non�intersecting convex polygons A and B� let a and b be the closest

points between feature Fa of A� and feature Fb of B� respectively� If a � V �Fb� and b �V �Fa�� then a and b are the closest points between A and B�

Theorem suggests an algorithm for �nding the closest points between convex polygons�

Consider the situation on the left of Figure �� Here� closest point candidates a and b

a

b

polygon A

polygon B a

byv

polygon B

polygon A

e

Figure �� Left� Theorem � implies a and b are the closest points between A and B� Right�a �� V �Fb�� and so a and b are no longer closest points� The closest feature on B will beupdated from vertex v to edge e�

�For simplicity� degenerate cases where the points are on the boundary of Voronoi regions are ignoredhere� �Lin�� for more details�

�

each lie in the Voronoi region of the other�s containing feature� By Theorem � they are the

closest points� In the situation depicted on the right� b is still in the Voronoi region of Fa�

however� a is no longer in the Voronoi region of Fb� Speci�cally� a lies on the wrong side of

ray y� In this case� the Lin�Canny algorithm speci�es that feature Fb should be updated to

the feature on the other side of ray y� namely the edge e� At this point� the closest points

between the new features are computed� and the Voronoi check is made again� This process

can continue for many iterations� however it is guaranteed to eventually terminate with

a � Fb� b � Fa� and a and b the closest points between A and B� The three�dimensional

version of the algorithm is a natural extension of the two�dimensional case� The bodies in

question are polyhedra� the features are vertices� edges� and faces� the Voronoi regions are

in�nite regions of space bounded by constraint planes rather than rays �Figure �� The

new closest feature

old closest feature

closest point

voronoi plane

Figure �� Tracking closest features of polyhedra with the Lin�Canny algorithm�

basic algorithm remains the same� For details� see �Lin�� LC��

Although designed to track the closest features� the Lin�Canny algorithm is easily

extended to a collision detection algorithm� The distance between two polyhedra is com�

putable from simple geometric formulas� given the closest features� The closest points are

obtained as a by�product of these calculations� With �nite precision arithmetic� a collision

epsilon �c must be used� The collision detection system reports a possible collision when the

inter�polyhedral distance falls below �c� The particular value of �c is not critical� a value is

chosen based on how large a gap is tolerable� For animation purposes� the collision epsilon

should be smaller than a pixel width� so that the objects appear to touch when colliding�

For the simulation examples described later in the thesis� �c was two to three orders of mag�

nitude smaller than the dimensions of the objects� For example� in the bowling simulation�

which used a standard �� foot alley and � inch pins� �c was one millimeter�

�� Collision detection and coherence

For e�cient collision detection for simulation� it is extremely important to take

advantage of geometric coherence �also called temporal or frame�to�frame coherence�� The

problem instances presented to the collision detection algorithm are a series of closely related

problems� In Lin�Canny terms� the closest features between a given pair of objects usually

change relatively infrequently� Even if the features are changing upon every invocation

of the algorithm� due to highly discretized polyhedral models or high velocities� the pair

of closest features from the last invocation of the algorithm is a good starting point for

the search for the current pair of closest features� In Impulse� a two�dimensional table of

closest feature pairs is maintained� For every pair of bodies in the simulation� there is a

corresponding table entry containing the closest feature pair for these bodies� computed

from the last invocation of the algorithm� Figure � illustrates the e�ect of coherence on

tracking the closest features�

The Lin�Canny algorithm has been described as taking �expected constant� time to

report a pair of closest features� This claim stems from coherence� often the closest features

do not change between successive calls� and the algorithm veri�es this fact in constant

time� This is a bit misleading� Consider tracking closest features between a small satellite

orbiting the Earth� over its equator� If the Earth is modeled as a tessellated sphere with N

facets� then during one orbit of the satellite� tracking on the Earth must progress through

O�pN� features� As the resolution of the Earth model increases� more work is clearly

�

5

10

15

20

25

30

35

40

45

50

55

0

0 5 10 15 20rotation between calls (degrees)

aver

age

time

per

call

(mic

rose

cond

s)

Running Time of Lin−Canny Algorithm

80 faces242 features



Figure � � This graph shows the e�ect of coherence on the performance of the Lin�Cannyalgorithm� The algorithm was used to track the closest features between a �xed cube and apolyhedral model of a sphere as the sphere rotated on an axis parallel to the nearest surfaceof the cube� The amount of sphere rotation between successive calls to the algorithm wasvaried from one to �� degrees� in one degree steps� This experiment was performed for threedi�erent discretization resolutions for the sphere� as indicated above� The performance ofthe algorithm decreases as the rotation speed increases� due to a decrease in coherence�Also note the insensitivity of the algorithm to polyhedron complexity� when coherence can beexploited� At a rotational speed of one degree between calls� a ��fold increase in complexityresults in a �� increase in execution time�

being done to track the closest feature as it circumnavigates the planet� even if the satellite

speed remains constant� In this case� the Lin�Canny algorithm is O�pN�� Figure � and

Graph in Cohen� et� al� �CLMP� � also illustrate that the running time of the Lin�Canny

�

algorithm depends on the number of features� One di�culty of assigning a complexity to

the algorithm is that it is very dependent on how the objects are moving� If the satellite

mentioned above falls straight down toward Earth� the algorithm is again constant time�

The claim of �expected constant� time raises more questions than it answers� however� and

almost constant time is a better description� In the satellite example� the coe�cient onpN

is probably extremely small compared to the constant term� In experiments with Impulse�

there is negligible slowdown in simulation speed� when polyhedral models of spheres with

a few hundred facets are replaced with polyhedral models with over �� facets �over

�� features�� The latter models are used in many of the simulations described later in

the thesis�

�� Extensions to the Lin�Canny algorithm

The extension of the basic Lin�Canny algorithm to curved objects has been studied

by Lin and Manocha �LM�� Curved bodies are approximated with a polyhedral mesh�

and closest points are tracked between these meshes� The closest points on the meshes are

projected onto the actual curved surfaces� and a numerical root �nding method uses these

points as a starting point to locate the true closest points� A general form of this algorithm

has not been implemented�

Another extension� used in Impulse� is the extension to non�convex bodies� As�

suming non�convex bodies can be decomposed into a group of convex pieces� Lin�Canny can

still be used to compute the distance between a pair of bodies� If body A is decomposed into

m convex pieces� A�� Am� and body B is decomposed into n convex pieces� B�� Bn�

then the distance between A and B can be computed as

d�A�B� � min

��i�m

��j�n

d�Ai� Bj� ��

In other words� the distance computation is broken into m�n standard Lin�Canny compu�

tations between convex bodies� Because of this reduction of the non�convex case to a group

of convex cases� the rest of this chapter assumes convex bodies�

Signi�cant improvement over this naive scheme is possible by computing the convex

hulls of each of the non�convex bodies� and computing the distance between bodies as the

distances between their hulls� This requires only one Lin�Canny invocation� and since A is

�

enclosed by its convex hull� the distance to the convex hull is a lower bound �a conservative

estimate� on the true distance to A� Only if the distance between the hulls reaches zero�

must the bodies be unwrapped and treated as a collections of convex pieces� For complex

bodies� this scheme can be applied recursively to obtain an entire tree structure for a non�

convex body A� The inner nodes of the tree are convex hulls of various subsets of the entire

body� and if a particular convex hull is pierced� that tree node is replaced by its children�

The leaves of the tree represent the underlying convex decomposition of the entire body�

and the root of the tree is the convex hull of the entire body� With such a structure� only

as much of the body is unwrapped as needs to be to determine distance� the rest of the

concavities remained wrapped in convex hulls� Details of this scheme may be found in

Ponamgi� et� al� �PML� �� At present� a general version of this algorithm does not exist�

although one is being developed as part of I�COLLIDE �CLMP� �� In Impulse� the naive

approach represented by Equation �� is used to handle non�convex bodies�

�� Prioritizing collision checks

For simulation� the collision detection algorithm must determine the time tc at

which a collision occurs between bodies� The typical approach is akin to the approach taken

in numerical root �nding� Assume there exists a function that takes two bodies and returns

a boolean value indicating if they are penetrating or not� If the bodies are not penetrating

at time t�� dynamic integration of the system state continues to some time t�� If at this

point� the bodies are re�tested and penetration is detected� than a collision has occurred

between them at some time tc with t� � tc � t�� In this case� a new point tm � �t�� t��is chosen� and dynamic integration is performed from t� to tm� Based on the result of a

collision test at the time tm� the process is repeated recursively on the interval �t�� tm�� or

�tm� t�� The process is repeated until the width of the interval falls below some tolerance�

The time tm may be chosen as the midpoint of the interval �t�� t�� however Bara� reports

much faster convergence using an interpolation method instead of simple interval bisection

�Bar�� If the penetration function returns a distance of separation or penetration� rather

than a simple boolean value� one can more accurately estimate the moment of collision by

examining this distance at t� and t�� The distance of separation is well de�ned for convex

bodies� distance of penetration can be de�ned as in �SSCK��

A particular trait of the Lin�Canny closest features algorithm �as originally speci�

�

�ed� had a strong impact on the way collision detection is performed in Impulse� When the

algorithm is passed two polyhedra that are penetrating� it cycles between pairs of closest

features� never terminating� The proofs which guarantee convergence and termination rest

on the assumption that the bodies are non�penetrating� Lin motivates the solution to this

problem� based on constructing internal pseudo�Voronoi regions that partition the space in�

side of the polyhedron� and matching each interior region with a feature on the polyhedron

�Lin�� This extension to the original algorithm is nontrivial� and it was not pursued at

the time the collision detection subsystem of Impulse was developed�

Instead� the collision detection subsystem in Impulse was designed to detect col�

lision before it occurred� This is done by obtaining successively closer approximations to

the exact collision time tc� without ever integrating beyond tc� The situation is as depicted

in Figure �� in which the function d�t� represents the distance between two bodies� nega�

tive values and zero crossings correspond to penetration and collisions� respectively� The

t1 t2 t3 t4 tc ct’t

d(t)

Figure �� A one�sided approach to root �nding is used to �nd collision times in Impulse�The function d�t� is the separation distance between two bodies over time the zero at tcindicates a collision� The velocities of the two bodies at time ti are used to compute aparabola that is less than d�t� for t � ti� The next zero crossing of the parabola gives ti��a closer approximation to tc that is guaranteed not to exceed tc�

derivative of d�t� is a measure of the velocity of approach of the bodies� In Impulse� a series

of approximations t�� t�� tn is made to the actual collision time tc� such that no ti exceeds

�

tc� At each ti� the current velocities of the relevant bodies are used to compute a second

order function which is a lower bound on the separation distance between the bodies� for

t � ti� The abscissa of the next zero crossing of the corresponding parabola becomes ti��

The series of closer approximations to tc ends when the distance between the bodies falls

below �c� Using a Newton�Rhapson approach to root �nding� it is possible to miss a zero

crossing in d�t�� for example� the crossing at t�c in Figure �� is likely to be missed� This

root can also be missed if there is a minimum time interval between collision checks� With

the one�sided approach� every root of ��t� is found�

�� The collision heap

When collision checking reveals that the distance between two bodies exceeds the

collision epsilon �c� a lower bound on their time of impact �TOI� is automatically computed

and returned� In terms of Figure �� the current time is ti� and the value returned is ti��

Assume a TOI estimator exists� which takes as inputs the dynamic states of two bodies�

along with the closest points between them� It returns a lower bound on the time of impact�

based on the assumption that the bodies will follow ballistic trajectories until the moment

of impact� The returned TOI estimates can be used to adaptively control the frequency of

collision checks while integrating the dynamic system�

Collision checks are scheduled in a collision heap� Corresponding to each pair of

bodies in the simulation is an element in the heap� containing a �eld with the last computed

TOI for that pair� The TOI �eld is a lower bound on the true time of impact for the bodies�

which is unknown� The heap is sorted on this TOI �eld� so that no collisions can occur

before the time in the TOI �eld of the top heap element �Figure �� The top level

simulation loop is very simple� The system is evolved to the time in the TOI �eld of the top

heap element� at which point collision checking is performed for this top top pair of bodies�

If the distance between the bodies is below the collision epsilon� a collision is declared� and

is handled by the collision resolution system� In any case� the TOI for the body pair is

recomputed� possibly causing it to drop down in the heap� and the process is repeated�

This scheme adapts the frequency of collision checks appropriately� when a pair of bodies

are far apart or moving slowly� collision checks between them will be infrequent� as the

bodies approach� checks increase as necessary�

�

dynamic state

collision heap

TOIestimator

−+

Lin−Canny

1 2 3 4

2−4

1−2

3−4

1−3

1−42−3

0

integrationstep, t∆

TOI

closest points

current time, t

Figure �� Impulse�s narrow phase collision detection system prioritizes collision checks ina heap� based on estimated times of impact� The time of impact of the top heap element de�termines the size of the next integration step� Numbers designate the indices of the di�erentbodies in the simulation�

�� Estimating time of impact

For the remainder of this chapter� the following notation is used�

t A general time variable� The positions and velocitiesof all bodies are functions of t�

t� The current time in the simulation� The positions andvelocities of all bodies are known at time t��

tc The time of the next collision between two bodies�

�t Some time step into the future� Often the motion ofbodies over the interval �t�� t� ��t� is reasoned about�

The key problem that must be solved in order to apply the collision heap scheme is�

Problem � Given� The current positions and velocities of two ballistic� convex bodies� and

the closest points between the bodies� Compute� A lower bound on the time of impact of the

two bodies� assuming they continue their current ballistic trajectories�

One di�culty is that the points on the bodies that will ultimately collide are not readily

found from the given information�� Figure �� illustrates a bad case� the linear and angular

velocities of the bodies are such that the closest points �with total velocities u� and u�� are

moving apart� However� another pair of points� not even considered by the closest features

algorithm� are almost as close as the closest points� and are approaching each other quickly�

ω

u

ω u2

v2

2

1v1

1

body 1

body 2

Figure �� The velocities of the closest points only is insu�cient for predicting time ofimpact�

One method of bounding the time of impact relies on the convexity of the bodies�

Consider the bodies shown in Figure �� Call the closest points on the two bodies c�

and c�� and let d � c� � c�� Since body � is convex� it must lie entirely on one side of the

plane passing through c� and perpendicular to the vector d� The same is true for body

and the corresponding plane through c�� Let x� and x� be the points at which the bodies

will ultimately collide� No matter where these points are located and what their path to

�There are closed form solutions for the orientation of a rigid body as a function of time that use ellipticintegrals �MR�� Using these equations with some method of evaluating elliptic integrals might lead totighter bounds than the ones discussed here�

body 1

body 2

d

x1

2xc1

c2

Figure �� Finding a lower bound on the time of impact of convex bodies�

impact will be� they will have to cover the distance d � kdk in the direction of d� Let �dbe a unit vector along d� Let D��t� be an upper bound on the distance traveled by any

point on body � along �d during the interval �t�� t�� Let D��t� similarly bound the distance

traveled by any point on body along ��d� If a collision occurs at time tc�

D��tc� �D��tc� � d�

A lower bound on the time of impact can be found by replacing this inequality with an

equality� and solving for tc� To facilitate this� the functions Di�t� should be kept simple�

Impulse uses quadratic functions�

Problem � TOI coe�cients� Given� The position and velocity of a body at the current

time t�� and a direction speci�ed by the unit vector �d� Find� Coe�cients A and B such

that the distance traveled by any point on the body in the direction �d over the interval �t�� t�

satis�es

D�t� � A�t� t�� B�t� t��

�

Consider the case of a ballistic body� The total velocity u�t� of an arbitrary point

x on the body is given by

u�t� � v�t� ��t�� r�t��

Here� v and � are the linear center of mass velocity and angular velocity of the body� and r

is the position vector from the body�s center of mass to x� all speci�ed in a �xed reference

frame� Since the body is ballistic�

v�t� � v�t�� g�t� t��

where g is the vector acceleration of gravity in the reference frame� Thus� the velocity of x

in the direction �d is given by

u�t� � �d � �v�t�� g�t� t�� d� ��t�� r�t� � �d�

Letting rmax be the maximum distance of any point on the body from the center of mass�

and max be the maximum magnitude of the body�s angular velocity during the current

ballistic phase�

u�t� � �d � �v�t�� g�t� t�� d� rmaxmax�

The quantity rmax may be pre�computed and stored for each body� The next section dis�

cusses the computation of max� Integrating the above equation over time�Z t

t�

u�� d d � ��g � �d��t� t��

� �hv�t�� d� rmaxmax

i�t� t��

The function on the right hand side is a suitable choice for D�t�� For ballistic bodies� the

TOI coe�cient routine returns

A ��

g � �d ��

B � v�t�� d� rmaxmax� ��

Other types of motion besides ballistic motion can be accommodated by the same scheme�

and are discussed later�

To compute a lower bound on the time of impact between body � and body � a

TOI coe�cient routine is called for each of the bodies �In computing the TOI coe�cients

for body � ��d is used in place of �d�� Call the coe�cients for body � A� and B�� call those

for body A� and B�� The lower bound TOI is the smallest real root t � t� of the quadratic

equation

�A� �A��t� t�� B� �B��t� t�� d�

If both bodies are ballistic� a common case in impulse�based simulation� the gravitational

contributions for each body cancel� and A� � �A�� In this case� only a linear equation must

be solved� If the above equation has no real roots greater than t�� then the bodies can never

collide� given their current trajectories� In this case� the time of impact routine returns

in�nity� If one of the bodies collides with some other body in the environment� however�

the time of impact between bodies � and must be recomputed and may be reduced to a

�nite value�

�� Bounding ballistic angular velocity

The TOI calculations of the last section assume knowledge of the maximum mag�

nitude of the angular velocity of a body during a ballistic trajectory�

Problem � Given� The current angular velocity � of a ballistically moving body� Find� A

bound on the maximum magnitude of � while the body moves in its current trajectory�

Since only the magnitude of the body�s angular velocity vector is needed� � can be expressed

in the most convenient frame� in this case� the body frame� Below� vectors and tensors are

expressed in this frame� unless otherwise noted�

The angular momentum of the body is given by

L�t� � I��t��

where I is the body frame inertia tensor �see Appendix A�� There is no time dependence in

this matrix� Since the only external force acting on the body is gravity� which acts through

the center of mass� the angular momentum is conserved in the inertial frame� Although

angular momentum is not conserved in the body frame� it is related to the constant inertial

angular momentum through a time�varying rotation matrix� Hence� the magnitude of the

body angular momentum is conserved� Therefore�

kL�t��k � kI��t�k�

The body inertia tensor I is diagonal� Calling its diagonal elements Ix� Iy� and Iz� the above

equation can be rewritten

x�t��

kL�t��kIx

�� y�t�

��kL�t��k

Iy

�� z�t�

��kL�t��k

Iz

��

�

Thus� the body angular velocity vector is constrained to lie on an ellipsoid in R� �Fig�

ure �� The maximum value of k��t�k is just the length of the semi�major axis of this

ωx

ωy

ωz

ω(t)

Figure �� The body angular velocity vector remains on an ellipsoid over a ballistictrajectory�

ellipsoid� thus

k��t�k � kL�t��kmin�Ix� Iy� Iz�

�

qI�xx�t��

� � I�yy�t�� I�zz�t��

�

min�Ix� Iy� Iz��

This bound was reported in �MC� b�� however a slight improvement is possible�

The conservation of energy de�nes a di�erent ellipsoid on which the angular velocity must

lie� Since there is no net torque acting on a body in a ballistic trajectory� the rotational

kinetic energy is constant� given by

E ��

��t�T I��t��

Since the body inertia tensor is diagonal�

E � Ixx�t�� Iyy�t�

� � Izz�t��

Writing this in the general form of an ellipsoid gives

x�t��q

�EIx

� �y�t�

�q�EIy

� �z�t�

�q�EIz

� � ��

�

The maximum value of k��t�k is the length of the semi�major axis of this ellipsoid�

k��t�k �s

E


sIxx�t�� Iyy�t�� Izz�t��


Let Imin � min�Ix� Iy� Iz� and Imax � max�Ix� Iy� Iz�� The ratio of the angular

kinetic energy bound �� to the angular momentum bound �� is

r �

s�Ixx�t�� Iyy�t�� Izz�t��Imin

I�xx�t�� I�yy�t��

� � I�zz�t��

�

This implies sImin

Imax� r � ��

The angular kinetic energy bound is always at least as tight as the angular momentum

bound� The bounds are the same when the body is rotating exactly about the axis of

minimum inertia� and the bounds are most di�erent when the rotation is exactly about the

axis of maximum inertia� The two bounds are always equivalent if the diagonal entries of

the body inertia tensor are all equal� as is the case for a uniform density sphere or cube�

�� Bounding box techniques

The TOI estimation described above assumes that bodies follow ballistic trajec�

tories between collisions� Suppose collision detection is performed on bodies � and � in�

dicating they are not yet colliding� and let tc be the computed time of impact for these

two bodies� The time tc re�ects the soonest time these bodies may collide� assuming they

continue along their current ballistic trajectories� It is possible� however� that body � may

collide with some other body� say body � before tc is reached� A collision impulse will be

applied to body �� sending it on a new ballistic trajectory� Thus� tc is invalid� and must

be recomputed� In fact� all TOIs that involve body � or body must be recomputed� The

collision heap is actually a priority queue� since the keys are not static� but can increase or

decrease� causing heap elements to rise or fall before ever reaching the top�

Early versions of Impulse performed O�n� TOI updates upon every collision in an

n body simulation� but the method scales poorly� Consider a group of coins tossed onto a

large� �at surface� Even if the coins are separated by large distances� as they begin to settle

on the surface� collisions become frequent� Every time a coin collides with the surface� the

TOIs between that coin and every other coin must be recomputed� even when the coins are

�

in no danger of colliding� The problem is that spatial locality is not being exploited� When

a coin experiences a collision with the surface� it is reasonable to recompute its TOI with

nearby coins� but not with ones that are far away� A related ine�ciency is that collision

checks are performed between bodies very far apart and in no danger of colliding� The heap

scheme reduces the frequency of checks between such bodies� however they are still made

at regular intervals� Bounding box techniques may be used to reduce these super�uous

collision checks and TOI updates� The method used in Impulse is based on a hashing

scheme proposed by Overmars for solving point location problems �Ove�� The technique

is �rst discussed in a static context� and then the extension to moving bodies is described�

�� Finding static box intersections

Point location problems occur frequently in computational geometry� One variant

is expressed as follows�

Problem � Point Location� Given a number of non�intersecting cells in space� store

the arrangement such that for a given query point p� the cell containing p �if any� can be

determined e�ciently�

Here� a cell is a connected region of space� Overmars presents two solutions to this problem

under the restriction that the cells are fat �Ove�� The more e�cient solution involves

surrounding each cell by an axes�aligned bounding box� and storing the location of these

boxes in a hash table� The technique can be extended to solve a more useful problem for

collision detection�

Problem � Static box intersections� Given n axes�aligned rectangular boxes B�� Bn�

�xed in space� store this arrangement such that the boxes that intersect a speci�ed query box�

Bq can be determined e�ciently�

To attack this problem� consider partitioning space into a cubical tiling with resolu�

tion �� Any point �x� y� z� in space belongs to a unique tile� speci�ed by integer coordinates�

under the tiling map � ��x

y

z

��

��bx��cby��cbz��c

��

�

The tiles that box Bi intersects are found by computing the images under of two

of Bi�s corners� the one of minimum x� y� and z coordinates� and the one of maximum x�

y� and z coordinates� The two tiles containing these corners� and the other tiles �between�

them� are the tiles that intersect Bi� A tile with coordinates �a� b� c� is between the two tiles

with coordinates �a�� b�� c�� and �a�� b�� c�� if and only if a� � a � a�� b� � b � b�� and

c� � c � c�� There are an in�nite number of tiles in unbounded space� but only a �nite

number that are intersected by at least one box� For each tile that a box intersects� the box�s

label is stored in the hash table� hashed under the tile�s integer coordinates� The query

box Bq can also be mapped to a set S of hash buckets according to which tiles it overlaps�

The union of all boxes whose labels appear in buckets of S can be quickly determined� and

this set forms a candidate set of boxes for Problem � The candidate set is further checked

against Bq using more expensive box intersection tests�

How to choose the tiling resolution � is not obvious� and in fact this can be prob�

lematic when the sizes of the boxes vary widely� If � is small� the larger boxes may intersect

a huge number of tiles� and thus require a large amount of storage in the hash table� In ad�

dition� when the static assumption is relaxed� updating the positions of these large� moving

boxes will be ine�cient� On the other hand� if � is large� the tiling will have poor resolution

power for the smaller boxes� Many small boxes may hash to the same tile� so the initially

computed set S is large� and the more expensive box intersection test will be performed

on many pairs� Overmars solves this problem by partitioning the set of cells into groups of

similar size� and creating one hash table for each group� For collision detection� however�

any one of the boxes may need to be checked for intersection with any other this is not

true in the point location problem and so partitioning boxes into disjoint groups is not

helpful� The solution is to build a hierarchical hash table� comprising several resolutions�

and checking for intersections among boxes at di�erent resolutions�

To understand the method� consider the one dimensional example� shown in Fig�

ure �� Here� the six �boxes� �that is� line segments� populating space are labeled A

through F � Let X denote an arbitrary box from this set� and de�ne sz�X� as the size �in

this case� length� of X� As a preprocessing step� one chooses constants � and �� and a

minimal sequence of tiling resolutions� �� n� such that

� � � � �

� � �

��

D

1

2

3

4

A B C D E F

A B B

C

E E

F F

r e

s o

l u t

i o n C

ρ1

4ρ

Figure �� A one�dimensional example of a hierarchical spatial hash table� with four reso�lutions� The value �i is the size of the tiles at resolution i� The cells which must be checkedwhen box E is stored in the table are shaded�

�� n � ��

and so that for each box X there exists an integer � � k � n with

� � sz�X��k

� ��

The integer k is called the resolution of box X� abbreviated res�X�� In Figure ��

and � � �� This means that each box X must have a length that is from �� to �� times

the width of the cells at resolution res�X�� The constraints �� are met by choosing four

tiling resolutions� as shown in the �gure� with res�A� � res�B� � res�D� � �� res�E� � �

res�F � � � and res�C� � �� The location of box X is hashed at tiling resolution res�X��

In two �or three� dimensions� the idea is the same� The cells are squares �or cubes� of side

length �i� and the boxes to be stored are rectangles �or rectangular prisms�� For box X�

sz�X� is the maximum distance between two opposite edges �or faces� of the box� In what

follows� d denotes the dimension of the boxes and ambient space�

When a box is stored in the hash table� overlap with other boxes must be checked�

some of which may be stored at other resolutions� Assume the boxes are hashed in order

of increasing resolution� When box X is hashed� all enclosing cells at resolutions less than

or equal to res�X� must be checked for other boxes� In Figure �� the cells that must be

checked when box E is stored are shaded� Since body C overlaps one of these cells� the

bodies E and C are reported as close� meaning the hash table is unable to verify that the

boxes do not overlap� Formally�

��

De�nition � Boxes X and Y are close if and only if they overlap a common cell at

resolution min�res�X�� res�Y ��

When boxes are close� full collision detection must be performed between the corresponding

bodies� The storage requirements and relevant time complexities for a hierarchical spatial

hash table scheme are now analyzed� assuming perfect hashing�

Lemma � Let szmin and szmax be the sizes of the smallest and largest boxes to be stored in

the hierarchical hash table� If n is the the number of resolutions required�

n ��log �

�

szmax

szmin

�

Proof� Choose �� szmin�� and subsequent tile resolutions such that the constraint

��i�� i ��

is satis�ed for � i � n� In this way� an appropriate k can be found to satisfy �� for any

box dimension in the interval �szmin� ��n�� an n is needed such that �n � szmax�� From

��

�n �

��

�

�n��

��

Substituting szmin�� for �� and using �n � szmax�� yields��

�

�n�� szmin

�� szmax

��

The lemma follows� �

Lemma � gives an important theoretical bound� but it is not always tight� For

instance� if all boxes are one of three sizes �small� medium� or large�� than at most three

resolutions are required for the hierarchical spatial hash table� regardless of the ratio of the

dimensions of the largest to smallest box�

Theorem � For a set of boxes to be stored in a hierarchical spatial hash table� let R be the

ratio of the largest to smallest box dimension� Then the total number of hash buckets which

must be checked for other boxes when storing a box in the hash table is O��d logR��

Proof� When box X is stored� cells at resolutions i � k� where k � res�X�� must be checked

for other boxes� By �� sz�X� � ��k� and so box X overlaps at most �� d cells at

�

resolution k� The number of cells overlapped at a given resolution i � k can not be more

than this� and since there are O�logR� resolutions by Lemma �� the number of overlapped

cells is O��d logR�� Each cell check corresponds to one bucket check in the hash table� and

the theorem follows� �

Theorem � Treating �� and R as constants� a hierarchical hash table can report all pairs

of close boxes among n boxes in O�n� c� time� where c is the number of close pairs�

Proof� By Theorem � a constant number of hash buckets must be examined upon storing

each box� the total number of buckets checked is O�n�� The time spent reporting closest

pairs among these buckets is O�c��

A �nal theorem relates the resolution power of the hierarchical hash table to the

parameter ��

Theorem � The hierarchical spatial hash table can guarantee that two boxes X and Y do

not intersect if the distance between them exceeds

�

�

pdmax�sz�X�� sz�Y ��

Proof� Without loss of generality� assume sz�X� � sz�Y �� so that res�X� � res�Y �� AssumeX and Y are reported as close� Then they overlap a common cell at resolution res�X�� The

maximum distance between any two points in this cell ispd�resX � and so the distance d

between boxes X and Y satis�es

d �

pd�res�X�

sz�X�sz�X��

From ��res�X�

sz�X��

and the theorem follows� �

The tradeo�s involved in choosing the parameters � and � are now apparent�

Recall that � � � � � and � � �� By Theorem � the closer � is to the minimum value of ��the fewer cells must be checked when storing boxes� By Theorem � the closer � is to the

upper bound �� the better the resolving power of the hash table� Finally� Lemma � implies

that the larger the ratio �� the fewer resolutions are required to store all of the boxes�

�

�� Coherence and the tiling scheme

In simulation applications� a slight variant of the hierarchical hash table scheme

can improve performance by taking advantage of coherence between problem instances�

Instead of storing box X�s label in buckets at res�X�� and only checking the appropriate

buckets at lower resolutions� the label is stored in all buckets that are checked� Using the

example of Figure �� the labels are stored as shown in Figure �� Boxes X and Y are

1

2

3

4

A B C D E F

A

A

A

B B

B

B

C

D

D E E

E F F

FA B C D E

D E

r e

s o

l u t

i o n

ρ1

4ρ

Figure �� In a variant of the hierarchical hash table scheme� the labels of each box arestored in every bucket that is checked for other boxes�

reported as close if and only if their labels appear in a common hash bucket at resolution

min�res�X�� res�Y �� the close pairs returned by the algorithm are exactly the same as with

the original version� A two�dimensional array of close counters tracks which boxes are

close� Each time X is stored into a hash bucket at resolution i that already contains Y � if

i � min�res�X�� res�Y �� then the counter corresponding to the pair �X�Y � is incremented�

When X is removed from such a bucket� the counter is decremented� Pairs of boxes for

which the corresponding counter is zero are not close� and narrow phase collision checking

is not performed between the corresponding bodies� When a counter is incremented from

zero to one� the pair enters the set of bodies on which narrow phase collision checking is

performed�

This scheme retains the state of the boxes between invocations� If a box intersects

the same cells that it did on the last call� no additional work needs to be done to store

that box on the current call� If the box has moved into new cells or left old ones� only

buckets corresponding to these cells must be changed� Boxes corresponding to �xed bodies

��

need only be stored into the hash table once� The disadvantage of this scheme is that more

processing is sometimes required to store a box�s label into a bucket� In Figure �� even

though D and E are veri�ed as not close at resolution � they must be stored in common

buckets at resolutions and �� requiring extra processing and adjustment of close counters�

For this reason� the claim of Theorem � is not valid �or at least not readily apparent� for

this variant on the algorithm� However this variant is quite e�cient in practice� as shown in

Table �� The table shows the number of cycles spent on broad phase collision detection for

millions of cyclesstandard coherence

example hashing hashing ratio

coins �� bowling �� rattleback top �� part feeder chute ��

Table �� Comparison of hashing schemes�

some example simulations� using both the standard hashing algorithm and the coherence

hashing algorithm� The simulations themselves are described in detail in Chapter �� From

the table� the coherence hashing algorithm is signi�cantly better� consistently running six

or more times faster than the standard hashing algorithm� For this reason� the coherence

hashing algorithm is used in Impulse�

�� Maintaining the collision heap

The hierarchical spatial hash table described in the last section can be used to cull

unnecessary collision checks and TOI updates for a group of moving bodies in a simulation�

Upon each call to the integrator� the state of the system is evolved from the current time

t� to some future time t� ��t� The step �t is determined by the earliest TOI �eld on the

top of the collision heap� After �t is computed� but before the integrator is called� for each

body an axes aligned bounding box is computed that is guaranteed to enclose the swept

volume of the body during the upcoming integration�

Consider ballistic body i� with bounding box Bi� The center of mass of body i

follows a parabolic trajectory that is known from the current state of the body� Bi is found

by noting the position of body i�s center of mass at the current time t�� at the time t��t�

�

and possibly at the apex of its parabolic trajectory� should this occur during the interval

�t�� t��t�� The box which bounds these two or three points is grown by ri� the maximum

radius of body i� to obtain Bi �Figure �� Clearly� computing Bi involves a �xed number

f

ri

ri

ri

ri

ri

ri

Bi

time t 0

time t + t0 ∆

Figure �� The bounding box for body i�s swept volume during a segment of a ballistictrajectory�

of operations� and can be done in constant time� This fact and Theorem � imply that all

bounding boxes can be computed� stored in the hash table� and close pairs reported� all in

O�n � c� time for an n�body simulation �c is the number of close boxes�� This is done at

the beginning of each integration step� For static bodies such as walls or platforms� it is not

necessary to grow the bounding box by the body radius� and the resulting bounding boxes

are much tighter� also� boxes for �xed bodies need never be updated�

For each pair of bodies that the hash table deems close� there is a corresponding

element maintained in the collision heap� Pairs not deemed close are in no danger of

colliding during the next integration step� When the close counter for a particular pair is

incremented from zero to one� the TOI for that pair is computed� and the pair is added

to the heap� if it is not there already� Since pairs in the heap are kept sorted on the TOI

�eld� no pair can collide any sooner than the pair at the top of the heap� The advance

algorithm �Figure �� is performed at the current integration step� As can be seen from

the algorithm� new pairs are inserted into the active collision heap before any integration

occurs� This is because the computed TOI for such a pair might precede the TOI previously

��

advance

�t TOI at top of heap � t�

for each body i

compute bounding box Bi over interval �t�� t� ��t�

store Bi in hierarchical hash table

for each j such that closeCounts�i� j� was incremented to �

if pair �i� j� is not in heap

compute TOI between bodies i and j

store pair in collision heap

�� since top TOI might have decreased due to new pairs��

�t TOI at top of heap � t�

integrate state of system over �t�� t� ��t�

Figure �� advance� Advance the state of the system forward in time� as far as possible�while guaranteeing no collisions are missed�

at the top of the heap� in which case the integration step must be shortened�

The question of when pairs should be removed from the heap remains� One ap�

proach would be to remove pairs as soon as the hash table indicates the bodies� boxes are

no longer close� This can cause pairs of bodies to rapidly move in and out of the heap�

especially when one of the boxes is close to the boundary of a tiling cell �Figure �� To

avoid this ine�cient behavior� hysteresis is applied to the collision heap� A body pair is

added to the heap whenever the boxes become close� but the pair may not be removed

before it bubbles up to the top of the heap� After a collision check for the top heap element

is made� the closest counter status for that pair is examined� If it is nonzero� the boxes

are still close� and the pair is reinserted into the heap after computing the new TOI� If the

closest counter for that pair is zero� the pair leaves the heap�

The hierarchical hashing scheme also reduces TOI updates upon collision� Recall

that without the hashing scheme� whenever bodies i and j collided� all TOIs involving

either one of these bodies must be recomputed� However� with the hashing scheme� TOIs

��

body 1

body 2

spatialpartitioning

Figure �� If body � is bouncing on �xed body � the pair of boxes corresponding to thesebodies may rapidly toggle between close and not close status� To avoid this jittering� hys�teresis is applied to the collision heap�

are only maintained for pairs in the heap� As a result� if bodies i and j collide� TOIs are

only recomputed between these bodies and nearby bodies� This greatly reduces the cost of

processing collisions�

The sizes of the bounding boxes depend on the current �linear� velocities of the

bodies� These can not be known for all time at the beginning of the simulation� As a result�

tiling resolutions can not be chosen as described in Lemma �� In practice� this is not such

a problem� In Impulse� the tiling resolutions are based simply upon the maximum radii of

the bodies� Unless the bodies are moving at an extremely fast speed� the number of tiles

intersected by the various boxes remains small� Figure �� shows the reduction in narrow

phase collision detection due to the hierarchical hashing scheme�

�� Spatial hashing versus coordinate sorting

In addition to the hashing scheme described above� there is another algorithm for

�nding intersections of axes�aligned bounding boxes� The algorithm is based on sorting the

coordinates of edges of the bounding boxes along each of the three coordinate axes� it is used

in the I�COLLIDE system �CLMP� � and also in �Bar�� The algorithm works as follows�

The minimum and maximum x�coordinates of each axes�aligned box are maintained in a

sorted list� The same is done for the y and z coordinates� Two boxes overlap if and only

if their coordinates overlap in each of the three coordinate directions� A two�dimensional

��

Figure �� Snapshots taken during the simulation of eight coins tossed onto a at surface�The lines indicate the tracking of closest points between bodies each line corresponds toone element in the active collision heap� The left �gure was produced with the broad phasecollision detection deactivated� and the right one with the broad phase activated� The broadphase greatly culls collision checks� Also� when a body experiences a collision impulse� onlythe TOIs with bodies connected to it by lines need be updated�

example is shown in Figure �� For example� since x�� the maximum x�coordinate of box

B� is less than x�� the minimum x�coordinate of box B�� these boxes can not overlap� On

the other hand� x� � x�� x�� and y� � y� � y�� Thus� boxes B� and B� overlap in both

the x and y coordinates� and therefore the boxes themselves overlap� Cohen� et� al� discuss

the relative merits of using a �xed size� cubical box that can accommodate a body at any

orientation versus tighter �tting boxes that change in shape as the body rotates �CLMP� ��

Coherence is exploited by updating previously sorted lists to obtain new sorted

lists� In this way� the number of exchanges needed to obtain the new sorted list is expected

to be O�n�� It can� however� be O�n�� Consider the situation depicted in Figure �� The

maximum and minimum y�coordinates of all the boxes are clustered closely together� Even

with very small motions from one time step to the next� O�n�� exchanges result in resorting

the y�coordinates� coherence breaks down� This example is not contrived� Imagine throwing

a group of dice onto a �at horizontal surface� As the dice come to rest� their bounding

boxes will tend to cluster along the vertical coordinate� Since coordinate sorting is based

on dimension reduction� the coordinates may be clustered even when the original boxes are

not� the clustering becomes worse in higher dimensions� One way of handling the clustering

problem is to perform a less drastic dimension reduction� projecting the three�dimensional

boxes �rst into two�dimensional rectangles in the plane� and reporting intersections among

��

x’1 x’3x’4 2x’x1

y’1

y1

x2

y2

y’2

x3

y3

y’3

x4

y4

y’4

1

2

3

4

Figure �� A two dimensional version of I�COLLIDE�s bounding box check� Two boxesoverlap if and only if their projections onto the x� and y�axes overlap�

the rectangles inO�n log n�k� time� where k is the number of intersections �Ede�� Hashing

schemes do not su�er from the clustering problem� Coherence always results in e�cient

updating of the hash table� unless the number of box overlaps in three dimensions is large�

Coordinate sorting does have one advantage over hashing� no hashing scheme culls

as many body pairs as coordinate sorting� Cohen� et� al� claim that choosing a near�optimal

cell size is di�cult� and failing to do so results in large memory usage and computational

ine�ciency� These claims are largely mitigated with a hierarchical hash table based on

multiple cell sizes� A very e�cient collision detection scheme based on boxes that are not

axes aligned is described in �GLM��

�

x

y

1 2 34 5

Figure �� A bad case for coordinate sorting� The dense clustering of box extrema alongthe y�axis results in O�n�� exchanges for each new sort of the coordinates�

�� Generalizing collision detection

The collision detection techniques described thus far are tailored to ballistic bodies�

which are ubiquitous in impulse�based simulation� The techniques are extensible to other

types of motion with the de�nition of two routines�

�� A swept volume routine� This takes the state of the body at the current time t��

and a time interval �t� and returns an axes�aligned box that encloses the center of

mass�s trajectory during the time interval �t�� t� ��t��

� A time of impact coe�cients routine� This takes the current state of the body�

and a directional vector �d� It returns two coe�cients� A and B� such that the distance

any point on the body travels in the direction of �d is bounded by the expression

A�t� t�� B�t� t�� t � t��

The swept volume routine is needed during the broad phase of collision detection and the

TOI coe�cient routine is needed during the narrow phase� Previous sections described

how the swept volume and TOI coe�cients are estimated for ballistic bodies� For �xed

�

bodies� the routines are trivial� the swept volume is always the current volume occupied by

the body� and the TOI coe�cients are both zero� The swept volume and TOI coe�cient

routines for bodies connected by joints are discussed in Chapter �

Another type of motion is scripted motion� Scripted bodies follow unalterable

trajectories through state space� they are impervious to external forces like gravity� and

collisions with other bodies� An example application of a scripted body is a vibrating part

feeder� Since the mass of the feeder is much greater than the mass of the small parts it

vibrates along a track� one could neglect the e�ects on the feeder of collisions with the parts�

Instead� the feeder might execute some prescribed� sinusoidal motion� Impulse provides

several types of scripted bodies� and others could be added for speci�c applications� Some

simulations with scripted bodies are described in Chapter �� One type of scripted body is a

cycler� A cycler�s center of mass follows an elliptical path through space� while the cycler�s

orientation remains constant� For illustration� consider the case where the path is a circle

in the x�y plane� Let r be the radius of this circle� and let ! be the angular speed at which

the center of mass moves around the circle� This is not the same as the angular velocity of

the body� which is zero since the orientation is held constant�

First consider the swept volume routine� Since the center of mass remains in the

x�y plane� the minimum and maximum z�coordinates of the center of mass are both �� The

minimum and maximum x� and y�coordinates are inferred from the smallest axes aligned

rectangle in the x�y plane which encloses several points� p�t�� the current location of the

center of mass� p�t� � �t�� the location of the center of mass at a time �t in the future�

and any crossings of the trajectory with the x� and y�axes over the interval �t�� t� � �t�

�Figure �� All of these points are easily computed since the trajectory in the x�y plane

y

x

y

x

y

x

p(t )0

(t + )0 ∆ tp

(t + )0 ∆ tpp(t )0

p(t )0

(t + )0 ∆ tp

Figure �� The axes aligned bounding rectangles in the x�y plane for a cycler body� Threedi�erent cases are shown� The point p�t�� is the current position of the center of mass� andthe point p�t� ��t� is the position of the center of mass a time �t in the future�

is known in closed form� �� x

y

�� r cos !t

r sin!t

�� Now consider the TOI coe�cients for this cycler� Since the angular velocity of

a cycler is zero� every point on the body has the same velocity as the center of mass at

all times� Thus it su�ces to bound the motion of the center of mass along the directional

vector �d� There are di�erent ways to do this� Let v�t� be the linear velocity of the center

of mass of the body� The magnitude of v�t� is a constant� equal to r!� Therefore� if D is

the distance that any point on the body travels in the direction �d�

D � r!�t� t��

This bound suggests choosing the TOI coe�cients in �� to be A � � and B � r!�

A second method for bounding motion is to observe that the current velocity of the

center of mass in the direction �d is given by v�t�� d� The magnitude of the acceleration ofthe center of mass is bounded by r!�� This is the centripetal acceleration� always directed

toward the center of rotation� Thus�

D � �r!��t� t��

� � �v�t�� d��t� t��

This suggests choosing the TOI coe�cients A � ��r!

� and B � v�t�� d�The coe�cients indicated by �� and �� are for a linear and quadratic model

for distance traveled� respectively� In general� the quadratic model has a smaller linear term

and will be a tighter bound on the distance traveled up to some time� After that time� the

quadratic term will dominate� and the quadratic bound will overtake the linear one� To

decide which model to apply� one can use the current distance d to the other body� Impulse

checks the distance at which the quadratic model overtakes the linear one� If this is greater

than d� the quadratic coe�cients are returned� otherwise the linear coe�cients are returned�

The time at which the two models are equal is obtained by equating the right hand sides

of �� and ��

�r!��t� t��

r!��t� t��

� � �v�t�� d��t� t��

which yields

t� t� �hr!� v�t�� d

ir!�

�

At this time� the distance bound given by both models is

D �hr!� v�t�� d

i!

�

If this bound is less than the distance to the other body� the cycler TOI coe�cients routine

returns the coe�cients of t� and t in �� otherwise the coe�cients in �� are returned�

This strategy gives whichever bound is likely to be tighter�

Other types of motion may be handled in the same way� the analyses are similar

to the one for cyclers described above� It is not necessary to perform analyses based on

pairs of motion types� for instance for cycler�ballistic pairs� cycler��xed pairs� ballistic��xed

pairs� and so on� Rather� the swept volume and TOI coe�cient routines are de�ned for

each single motion type� and then collision detection can be performed between this type

and all other motion types� For n types of motion� only O�n� analyses are required instead

of O�n�� This feature is also shared by the collision detection algorithm of Von Herzen�

et� al� �HBZ�� Lipschitz bounds need only be computed on a per body basis� not on a

per body pair basis� The bounds for individual types are combined to form the bound for

a particular pair�

�

Chapter �

Collision Response

The collision response problem is concerned with computing the pair of equal and

opposite impulses that should be applied to the colliding bodies in order to prevent pene�

tration� Like contact forces� collision impulses are subject to frictional constraints� as well

as other constraints governing the energy dissipation during collisions� Collision response

algorithms fall into two broad categories� The �rst category makes the constant sliding di�

rection assumption� the direction of the relative tangential velocity between colliding bodies

at the contact point remains constant during a collision� This assumption is convenient be�

cause the direction of the frictional force varies with the sliding direction� Holding these

directions �xed allows one to solve for the collision impulse and post�collision velocities of

the bodies by solving a system of algebraic equations� Many variations on this theme are

described by Brach �Bra�� In the context of interactive dynamic simulation� this approach

is used almost exclusively �Bar�� CS�� Hah�� LRK�� MW�� NM�� It is often a fairly

strong approximation� particularly for collisions between three�dimensional bodies�

Just as a collision impulse changes the relative normal velocity between colliding

bodies� it also changes the relative tangential velocities� the direction of the latter is not

constant� The second class of collision response methods use di�erential equations that

describe the collision process� They account for the change in sliding direction and the

possibility of transient sticking that can occur during impact� These approaches date back to

Routh �Rou� �� and have been studied by many others �BK�� Kel�� MC� a� Str�� WM��

For impulse�based simulation� an accurate collision model is required for physically valid

results�

This chapter describes a suitable collision resolution algorithm for an impulse�

based simulator� which has been implemented in Impulse� The main contribution is a

computational model for the di�erential analysis of the collision process� this approach is

more accurate than the algebraic one� Building on work of Routh and Keller� new equations

are derived that are suitable for numeric integration� The algorithm presented here also

incorporates Strong�s improved model for restitution� To accomplish this� the collision

integration is divided into three phases� and two di�erent parameterizations are used� This

chapter also presents new results concerning the determinacy of collision dynamics under

the Coulomb friction model� Recently� Bhatt and Koechling have explored these issues

�BK�� BK��b�� and in �BK��a� they report some of the same results� The geometric

proofs given in this chapter di�er signi�cantly from theirs� which are based on the roots of

polynomials� in our opinion� the geometric proofs are simpler and more intuitive� Finally�

this chapter discusses some approaches for modeling static friction in an impulse�based

context�

�� Assumptions of collision response model

The physical phenomena that occur between bodies in contact result from com�

plex interactions at the atomic level� Finite element methods are a �rst step toward a

tractable model� but are still too computationally expensive to be used in the context of

interactive simulation A much greater approximation� but one which signi�cantly reduces

computational costs is the rigid body model� This model is reasonable for many everyday

physical systems� it is most tenuous during collisions between bodies� which involve surface

deformations� Still� many physical phenomena can be captured with a rigid body model� it

has wide application in engineering� and it is the model used by Impulse�

Assumption � Rigid bodies� All physical objects in the simulation are perfectly rigid�

This model has several implications� When two objects collide� the duration of the collision

is in�nitesimal� and the forces needed to prevent penetration are impulsive� instantaneously

changing the velocities of the colliding bodies� Since velocities are �nite� the positions of

the colliding bodies are constant during the collision� Non�impulsive forces �like gravity�

have no e�ect over in�nitesimal intervals� and may thus be ignored in collision analysis�

The rigid body assumption is made frequently in collision analysis�

�

When two real bodies collide� there is a period of deformation during which elastic

energy is stored in the bodies� followed by a period of restitution during which some of this

energy is returned as kinetic energy� as the bodies rebound o� each other �Figure �� The

ttmc tf0

0 ttmc tf

zf

pz

0 ttmc tf

zW

compression restitution

1

2

1

2

1

2

zu

Figure �� The normal velocity� force� impulse� and work during the compression andrestitution phases of a collision� The �z� subscript denotes the normal direction at thecollision point�

rigid body assumption is usually coupled with a model to approximate this process� The

simplest model� and the one used in most elementary physics texts� is Newton�s impact law�

uz�tf � � �e uz��

Here uz is the normal component of the relative velocity between the bodies� uz�t� � ��

means the bodies are moving toward another at time t� and uz�t� � � means they are

separating� The collision starts at time zero� and ends at time tf � The point of maximum

compression� tmc� is the point at which the normal velocity changes sign� and the phase

changes from compression to restitution� The constant e is called the coe�cient of resti�

tution� and make take values between � and �� depending on material properties of the

�

colliding bodies� Newton�s law is linear� and lends itself nicely to an algebraic solution for

the post�collision velocities�

Poisson�s hypothesis� an alternative restitution law� uses e as a ratio between

impulses rather than velocities�

pz�tf �� pz�tmc� � e pz�tmc��

Here� pz is the normal impulse� the time integral of the normal force fz delivered from

one body to another� Poisson�s hypothesis states that the normal component of impulse

delivered during the restitution phase is e times the normal component of impulse delivered

during the compression phase� This is equivalent to Newton�s law for frictionless collisions�

Keller uses Poisson�s hypothesis to derive equations of collision with friction �Kel��

Both Newton�s impact law and Poisson�s hypothesis can cause the total energy of

the colliding bodies to increase during a collision� when friction is present� To correct this

defect� Stronge proposes a new de�nition of the coe�cient of restitution� as a ratio of the

work done by the normal components of impulse �Str��

Assumption � Stronge�s hypothesis� Let Wz�t� be the work done by the normal com�

ponents of the �equal and opposite� collision impulses during a collision� Then

Wz�tf ��Wz�tmc� � �e� Wz�tmc��

The positive work done during the restitution phase is �e� times the negative work doneduring compression� Unlike the other models� Stronge�s model guarantees that the e�ects

of the normal forces� like the tangential frictional forces� are always dissipative� they can

not add energy to the system� This agrees with the description of the physical process of

collision� Stronge�s model is employed by the simulator Impulse�

The tangential components of fore and impulse that develop during a collision are

governed by a friction law� A very common friction formulation� and the one employed in

Impulse� is the Coulomb friction law�

Assumption � Coulomb friction law� At some instant during a collision between bod�

ies � and � let u be the contact point velocity of body � relative to the contact point velocity

of body � Let ut be the tangential component of u� and let �ut be a unit vector in the di�

rection of ut� Let f z and f t be the normal and tangential �frictional� components of force

�

exerted by body � on body � respectively� Then

ut �� f t � ��kfnk�utut � kf tk � �kfnk

where � is the coe�cient of friction�

In order� the equations above correspond to dynamic and static friction� While the bodies

are sliding relative to one another� the frictional force is exactly opposed to the direction

of sliding� If the relative tangential velocity is zero� all that is known is that the total force

lies in a friction cone�

�� Computing collision impulses

Collision processing is initiated when two criteria are met�

�� The collision detection system reports that the distance between two bodies is less

than the collision epsilon�

� The velocities of these bodies are such that the distance between the closest points is

decreasing�

The job of the collision resolution system is to compute a pair of equal and opposite collision

impulses to be applied to the colliding bodies at the contact points� These impulses must

prevent penetration of the bodies� and also satisfy the physical laws discussed in the previous

section� This section describes a method for computing these impulses� The situation is as

depicted in Figure �� The collision frame Fcoll has its origin at the collision point and itsz�axis aligned with the mutual surface normal at this point� pointing from body toward

body body �� For non�smooth bodies� such as polyhedra� the surface normal is de�ned to

lie along the line between the closest points� For body i� mi is the mass of the body� Ii is

the � mass matrix� described in Appendix A�� vi is the linear velocity of the enter ofmass� �i is the angular velocity of the body� ui is the absolute velocity of the contact point

on the body� and ri is the o�set vector from the body�s center of mass to the contact point�

Also� p is the collision impulse delivered from body to body �� and �p is the reactionimpulse delivered from body � to body � Throughout this section� it is assumed that all

vectors and tensors are resolved in the collision frame�

�

−p

r1ω1

v1

u 1

body 1

body 2

1m , I1

+p

Fcoll

x

yz

Figure �� A collision between two bodies� indicating some of the quantities used duringcollision analysis� The collision frame Fcoll is chosen so that the z�axis is aligned with thesurface normals at the collision point�

�� The equations of collision

Basic Newtonian physics dictate how collision velocities and impulses evolve� Dur�

ing a collision� all non�impulsive forces acting on body � are negligible� only the collision

force f�t� needs to be considered� which also induces a torque of r� f�t�� From the Newton�Euler equations �Appendix A��

f�t� � ma��t�

r� � f�t� � I��t� � ��t�� I��t��

where a� and �� are the linear and angular accelerations of body �� The last term in

the second equation comprises inertial forces� which are also negligible during collision� it

��

may be dropped� Also� m�� I�� and r� are constants� With these facts in mind� the above

equations can be integrated over time to give

p�t� � m��v��t� ��

r� � p��t� � I��t��

where p is the collision impulse�

p�t� �

Z t

�f�� d� ��

These equations relate the impulse delivered up to some time t to body ��s veloc�

ity changes up to that point� Using time as a parameter is valid for the above derivation�

in which a collision is a event of �nite duration� during which large but bounded forces

act� For the rigid body model� however� a collision is really the limit of this process� as

tf approaches zero and the collision forces become in�nite� Di�culties in analysis arise

because the collision occurs over a zero time interval� and the velocities become discontin�

uous functions of time� These problems are remedied by choosing a new parameterization

for the collision� A collision parameter is chosen� which monotonically increases during

the course of the collision� All velocities of the colliding bodies as well as the accumulated

impulse are expressed as functions of � One natural choice � pz� the normal component

of p� Clearly pz begins at zero and monotonically increases during a collision� it is the

integral of the normal component of force exerted by body on body �� and this force

component is always positive in Fcoll �Figure �� because the bodies can only push oneach other� Furthermore� velocities will be shown to be continuous functions of this pa�

rameter during a collision� Other collision parameters will be used as well� when generality

is required� denotes an arbitrary collision parameter� A prime �� is sometimes used todenote di�erentiation with respect to the collision parameter�

Rewriting �� and �� and changing to the new collision parameter �

�v��

m�p� �

�� I�� r� � p�t��

The contact point velocity u� is given by

u�� v�� r��

��

Combining the above three equations�

�u��

m�p� � �

I�� r� � p�t�

�� r�

�

��

m�� "r�I�� "r�

�p� ��

where "r� is the cross�product matrix corresponding to r� �Appendix A�� and � is the �identity matrix� The same derivation can be performed for body � the only di�erence is

that p is replaced with the reaction �p� The result is�

�u��

m�� "r�I�� "r�

�p��

Letting u� � denote the relative contact point velocity� that is u� � u��

�u� � �

��

m��

m�

��

"r�I

�� "r� � "r�I

�� "r�

� � �z �

K

p� ��

or more succinctly�

�u� � � Kp� ��

The � matrix K is called the collision matrix� it plays a central role in computing theimpulse to resolve the collision�

Theorem � properties of K� For a given collision� the collision matrix K de�ned by

�� is constant� nonsingular� symmetric� and positive de�nite�

Proof� Constancy of K is evident from �� it depends only on the masses� mass distribu�

tions� and contact point locations of the colliding bodies� all of which are constant during a

collision� For the other claims� note that K is the sum of three matrices� a positive scalar

multiple of the identity matrix� which is clearly symmetric positive de�nite� A� � �"r�I�� "r��

and A� � �"r�I�� "r�� The mass matrices Ii are also symmetric positive de�nite �see Chap�

ter �� therefore so are their inverses� Since "ri is skew�symmetric� it follows that ATi � Ai�

so Ai is symmetric� For an arbitrary vector x�

xTAix � �xT"riI��i "rix � �"rix�T I��i �"rix� � wT I��i w�

where w � ri � v� Since I��i is positive de�nite� the right hand side is non�negative� and

therefore Ai is positive semi�de�nite� Finally� since K is the sum of three symmetric ma�

trices� of which one is positive de�nite and two are positive semi�de�nite� K is symmetric

�

positive de�nite� which also implies nonsingularity� �

The main result of this section is embodied in the following theorem�

Theorem � equations of collision� During a collision parameterized by � the relative

contact point velocity and the collision impulse evolve according to�

d

d u� � � K

d

d p� �

d

d p� � � K�� d

d u� ��

Proof� From �� u� � � u� � � u�� Kp� �� Since K is constant� di�erentiating

with respect to yields the �rst equation of the theorem� Since K is also nonsingular� this

equation may be inverted� yielding the second equation� �

�� Sliding mode

Bodies � and are sliding relative to one another when the tangential component

of relative contact velocity u� � is nonzero� In this case it is possible to derive a �rst order

di�erential equation for u� using pz as the collision parameter�

De�nition � Let � be the relative sliding direction at some point during a collision� that

is� � � Tan�� uy� ux�� where the arctangent returns values in four quadrants� De�ne the

sliding vector �� as

��

�� cos �� sin ��

�� ux�

qu�x � u�y

��uy�qu�x � u�y

�

��where � is the coe�cient of friction�

The signi�cance of the sliding vector is seen in the next lemma�

Lemma � Let ��pz� be the relative sliding direction during a collision� While the bodies

are sliding relative to each other�

d

dpzp�pz� � ��pz��

�

Proof� By the chain rule�

d

dpzp�pz� �

d

dtp�pz�

dt

dpz�pz� � f�pz�

dt

dpz�pz��

where f�pz� is the collision force� the time derivative of impulse� By the Coulomb friction

law�

f�pz� �

�� cos ��pz�fz�pz�� sin ��pz�fz�pz�

fz�pz�

��under sliding conditions� Combining these equations and noting that �dt�dpz��pz� � ��fz�pz�

proves the lemma� �

Combining Theorem � and Lemma produces��

Theorem � sliding ODE wrt pz� While two colliding bodies are in sliding contact�

the derivatives of u with respect to pz are given by

d

dpzu � K �� K

��ux�

qu�x � u�y

��uy�qu�x � u�y

�

��This system of three nonlinear� �rst�order ODEs describe how the relative velocity

evolves during a collision� By numerically integrating these equations using pz as the inde�

pendent variable� u can be tracked over the course of the collision� as long as the sliding

velocity is nonzero� Figure � shows solution trajectories of the ODE system projected into

the ux�uy plane� For this example� � was chosen as �� and

K �

��

��The diamonds in the �gure mark di�erent initial values for ux and uy at the beginning

of the collision� The �ow lines show how the relative tangential velocity evolves during the

collision� For a given collision� only one of the �ow lines is followed�

One counterintuitive aspect of Figure � is that many of the �ow lines diverge from

the origin� indicating that the magnitude of the sliding velocity increases during collision�

�The explicit dependence of u and � on pz is eliminated for clarity�

��

-10

-8

-6

-4

-2

0

2

4

6

8

10

-10 -8 -6 -4 -2 0 2 4 6 8 10

y ve

loci

ty

x velocity

Evolution of Tangential Velocity

Figure �� Solution trajectories of the ODE system of Theorem � projected into the ux�uyplane�

This behavior seems contrary to the laws of friction� which stipulate frictional force oppose

sliding� The behavior is understood from Figure �� In this case� the tangential velocity

is zero at the beginning of the collision� but nonzero at the end of the collision� Although

the frictional force opposes the motion� it can not overcome the e�ects of the normal force�

which induces an angular velocity that causes the rod to slip�

�� Sticking mode

When the tangential component of relative contact velocity u vanishes� the bodies

are sticking� and the analysis of the previous section �� is not valid� Under sticking

conditions� the Coulomb friction model does not completely determine the frictional force�

but only requires that its magnitude not exceed � times the magnitude of the normal force�

�

v

ω = 0

before collision

= 0ut

α

a

fn

ft

during collision

ω

vut

after collision

Figure �� A situation where the tangential relative contact velocity of the rod �ut� starts atzero and increases during the course of the collision� even though the frictional force resiststhis change in velocity�

When sticking occurs� we assume� as does Routh� that if the frictional forces are strong

enough to maintain the sticking condition� they will do so �Rou� �� If they are not strong

enough� sliding will resume� The following theorem provides the means to test which of

these behaviors occurs�

Theorem � Stable sticking� For a particular collision with collision matrix K� the fric�

tional collision forces can maintain sticking if and only if

�K��

� � �K��

� � ��K��

��

where K��ij denotes the �i� j� element of the inverse collision matrix K��

Proof� Consider the collision force f�pz� at point pz in the collision� From the Coulomb

friction law� if there is no sliding the components of f must satisfy

fx�pz�� fy�pz�

� � ��fz�pz��

or� expressing force as the time derivative of accumulated impulse��dpxdt�pz�

��

�dpydt�pz�

��

�dpzdt�pz�

��

Multiplying the above inequality by �dt�dpz��

dpxdpz�pz�

��

�dpydpz�pz�

��

�dpzdpz�pz�

��

��

If the sticking condition is stable� the derivatives of relative tangential velocity vanish�

u�x�pz� � u�y�pz� � �� In this case� Theorem � implies

d

dpzp�pz� � K

��

��

�

u�z�pz�

�� K��

��

K��

K��

��u�z�pz��

Combining the previous two equations proves the theorem� �

�� Collision integration

The collision response calculation is based on tracking the relative contact veloc�

ity u� � during a collision� Figure � depicts relative contact velocity space� each point

�ux� uy� uz� in this space corresponds to a particular relative contact point velocity between

body � and body � The �gure also depicts the trajectories u� � for three di�erent col�

lisions� For all of these collisions� uz � � at the beginning of the trajectory since that the

bodies are moving toward each other� and uz � � at the end of the trajectory since the bod�

ies are separating� The plane uz � � is the plane of maximum compression� as u� � crosses

this plane� the collision phase changes from compression to restitution �c�f� Figure ��

As long ux and uy are not both zero� u� � is evolved according to sliding equations� such

as those given by Theorem �� Path A is a case for which sliding occurs throughout the

collision� If while tracking u� �� ux and uy both vanish� then u� � lies on the uz axis� also

called the line of sticking� In this case� Theorem � is not applicable� instead the test of

Theorem � is performed to determine if the sticking condition is stable� If it is� u� � will

remain on the line of sticking for the duration of the collision� path C exhibits this behavior�

If the sticking is not stable� meaning the frictional forces are not strong enough to prevent

tangential slip� the point u� � leaves the line of sticking� again moving according to sliding

equations� Path B exhibits this behavior� The question of exactly how u� � leaves the line

of sticking has not been discussed� this is a deep question that will be treated in Section ��

If the paths of Figure � are projected into the ux�uy plane� one obtains �ow lines like the

ones in Figure ��

This process of tracking the point u� � during collision is called collision inte�

gration� Given u� �� the initial relative contact point velocity� and u� f �� the value of

��

ux

uy

uz

plane of max.compression

medium frictionB

low frictionA

high frictionC

line

of s

ticki

ng

Figure � � Trajectories through relative contact velocity space for three di�erent collisions�

relative contact point velocity at the end of the collision� the collision impulse may be easily

determined� Inverting ��

p� � � K��u� � � K�� u� f �� u� ��

Since p� f � is the total impulse delivered during the collision� computing p� f � is the

ultimate goal of the collision resolution system� The collision integration process described

above is somewhat simpli�ed� and the purpose of this section is to describe the process in

more detail� Procedures to handle stable and transient sticking are developed� as well as

the termination conditions that indicate when the integration is to stop� The latter are

based on Stronge�s hypothesis for restitution�

��

�� Work done by collision forces

Consider a force f applied to a particle that moves along a continuous path x�t�

through space� The total work performed by this force on the particle over the time interval

�� tf � is given by

W �

Z tf

t�

f�t� � #x�t� dt

If x�t� is a line segment and f�t� a constant force directed along this segment� this boils

down to the familiar �work equals forces times distance� rule� During a collision� f is a

collision force applied to a body at the contact point� and #x is the absolute velocity of the

contact point� for example u� on body �� The formulation for work given above is not

usable during a rigid body collision� because the collision forces are in�nite and the time

interval is in�nitesimal� This can be remedied with the familiar tactic of re�parameterizing

the collision with a di�erent variable � With this change of variables�

W� �

Z �f

�f� � � u��

�dt

d

�d �

Replacing f� � with �d�dt�p� �� applying the chain rule� and rearranging gives

W� �

Z �f

�u�� d

d p� � d �

Z �f

�u�� p�� d �

This expresses the work done by the collision impulse on body �� Similarly� the work done

by the reaction impulse on body is given by

W� � �Z �f

�u�� p�� d �

Combining the above two equations� and recalling that u � u� � u�� the total work done

by the collision impulse on both bodies is

W �

Z �f

�u� � � p�� d �

The di�erential form of this equation will be useful�

Theorem � The derivative of work done by the collision impulse with respect to the col�

lision parameter is given by

dW

d � � � u� � � p�� u� � �K��u��

��

Since K�� is a known constant� and the path u� � completely determines u��

Theorem �� implies that knowing the complete path u� � is enough to specify the total

work done on the colliding bodies� In fact� knowing even less is su�cient�

Theorem �� path independence of W� Suppose the relative contact velocity u� � pro�

ceeds from u� �� to u� f � during a collision� over some arbitrary path let �u� f � �

u� f ��u� �� The total work done by the collision forces is independent of the path taken�

and is given by

W ��

�u� f � � u� ��

T K�� u� f ��

Proof� From Theorem ��

dW

d � � � u� �T K�� u��

�

hu� �T K�� u�� u� �T K�� u��

i�

Transposing the second term in brackets a scalar and observing the symmetry of K��

dW

d � � �

�

hu� �T K�� u�� u�� T K�� u� �

i�

Since K�� is a constant�

dW

d � � �

�

d

d

hu� �T K�� u� �

i�

By the Fundamental Theorem of Calculus� the total work done over the interval �� f � ishu� �TK��u� �

ij�f� � Evaluating this expression�

W � u� f �TK��u� f �� u� ��

TK��u� ��

� u� f �TK��u� f �� u� f �

TK��u� �� u� f �TK��u� �� u� ��

TK��u� ��

� u� f �TK��u��uTK��u� ��

Finally� transposing the second scalar term of the �nal equation yields the desired result�

�

An important corollary concerns a special energy preserving collision�

Corollary � If during a collision� the relative contact velocity is exactly reversed� that is

u� f � � �u� �� then the collision forces do no net work on the colliding bodies� and the

energy of the system is therefore unchanged by the collision�

��

Stronge�s hypothesis is used to determine when the collision integration should

stop� This hypothesis is not a statement about the total work done by collision forces� but

about the normal work� that is� the work done by the normal component of the collision

force� The normal work is denoted Wz�

Theorem �� The derivative of the normal work with respect to the collision parameter

is given bydWz

d � � � uz� � p

�z� ��

Proof� From Theorem �� and the chain rule�

dW

d � � � u� � � p�� u� � � f� � dt

d �

The di�erential work done only by the normal �z�component� of the force is therefore

dW

d � � � uz� �fz� �

dt

d � uz� �p

�z� ��

�

�� Integrating collisions using di�erent parameters

To this point� the only quantity mentioned as a possible collision parameter is pz�

the normal component of impulse� This is a natural choice since pz is clearly monoton�

ically increasing during the collision� There are� however� other choices for the collision

parameter � and most of the results derived thus far apply equally well for other valid pa�

rameterizations� For a parameter to be valid� it must be monotonically increasing during

the collision� and the quantities being integrated� such as u� � must be continuous� These

properties have already been shown for the parameter pz� and by the chain rule implies that

any other parameter with the property that d �dpz is positive and �nite is also valid� As

an example� consider the parameter uz� the normal component of relative velocity� From

Theorem ��d

dpzuz � �K��K��K��

As long as the right hand side is positive� uz is a valid parameter�� As a second example�

consider the parameter Wz� the normal work done during the collision� Applying Theo�

�This is true for almost all collisions� independent of the sliding angle �� Section �� deals with the casewhen it is not�

��

rem � with � pz�d

dpzWz � uz

d

dpzpz � uz� ��

ThereforeWz is a valid parameter as long as uz � �� this is the case� for instance� throughout

the restitution phase of the collision�

Since the equations of collision have already been developed using pz as a param�

eter� why bother with other parameterizations� The reason has to do with how collision

integrations are terminated� Stronge�s hypothesis �Assumption � dictates the process by

which the collision integration should proceed�

�� Integrate the compression phase of the integration� which terminates at the point of

maximum compression when uz � �� Record the normal work done during this phase�

� Integrate the restitution phase of the integration� which terminates when the nor�

mal work done during restitution reaches �e� times the normal work done duringcompression�

Numerical integrators integrate over an interval �a� b�� and the endpoints must be speci�ed

when the integrator is called� The values of pz at the end of the compression phase and at

the end of the restitution phase are not known a priori� hence� pz is not a useful parameter�

On the other hand� the value of uz is known at the end of the compression phase� it is

zero� Thus� uz is a suitable parameter for the compression integration� At the end of the

compression phase� the normal work done during compression is known� and from this the

total work which must be done over the entire collision can be computed� The normal

work Wz is therefore a suitable parameter for the restitution integration� The next sections

reformulates the equations of collision for these new parameters� and also show how sticking

is handled� It will be useful to partition the collision matrix K into row vectors�

K �

��kx

ky

kz

�� The expressions Kij and K

��ij respectively denote the �i� j�th entries of K and K

��

�� Sliding mode under uz and Wz parameterizations

The equations of sliding must be reformulated for the new parameterizations� The

compression integration is performed with uz as a parameter� During this integration� ux

�

and uy are considered functions of uz� Since the value of Wz at the end of compression

is needed� this quantity must also be tracked as a function of uz� Of course uz need not

be explicitly integrated� since it is the independent variable� From the chain rule and

Theorem ��duxduz�dux�dpzduz�dpz

�kx��

kz��

The derivative duy�duz is derived analogously� Lastly� from Theorem � with � uz�

dWx

duz� uz

dpzduz�

uzkz��

�

Summarizing the results�

d

duz

��ux

uy

Wz

��

kz��

��kx��

ky��

uz

��

The restitution integration is performed with Wz as a parameter� The three com�

ponents of u must be integrated with respect to this variable� From Theorem � and the

chain rule�d

dWz

u � Kd

dWz

p � Kd

dpzp

dpzdWz

�

From �� dpz�dWz � u��z � as a result�

d

dWz

u ��

uzK��

�

uz

��kx��

ky��

kz��

��

The above equation is valid for uz � �� but it is not well behaved near uz � ��

To avoid using this equation near this singularity� uz is retained as the integration variable

slightly beyond the compression phase� into the early restitution phase� After uz is safely

positive� the restitution integration can complete using Wz as a parameter� Care must be

taken that the small extension integration with respect to uz does not cause Wz to exceed

its �nal value� The following lemma computes a safe upper limit for uz for the extension

integration� The complete life cycle of a collision is depicted in Figure ��

Lemma � Let Wr denote the normal work which to be done during the entire restitution

phase� Suppose integration with respect to uz is extended into the restitution phase� until

uz � b� where

b �

sWr

�K��

qK�

�� K��

��

�

Then the normal work will not exceed Wr during this extension integration�

Proof� Suppose dWz�duz � Cuz for some positive constant C� Separating the variables

and integrating as uz varies over �� b� yields

�Wz � �Cb��

To insure that �Wz does not exceed Wr� it su�ces to choose

b �qWr�C�

All that is needed is C� From the last of Equations ��

dWz

duz�

uzkz��

��K��

qK�

�� K��

��uz�

The coe�cient of uz on the right hand side is a valid choice for C� the lemma follows� �

compression phase

u < 0z zu = 0 zu > 0

compression integration restitution integration

extension integrationlatch Wz

restitution phase

zx yintegrate u , u , W

w.r.t. uz

zx yintegrate u , u , u

w.r.t. Wz

start ofcollision

end ofcollision

max. compression

Figure �� The physical life cycle of a collision� and the corresponding collision integration�The compression and extension integrations are performed using �� and the restitutionintegration is performed using �� The value of Wz latched at the point of maximumcompression is used to determine the upper limit of Wz that ends the restitution integration�according to Stronge�s hypothesis� The value of uz that terminates the extension integrationis given by Lemma ��

�� Handling sticking during collision integration

We have not yet discussed what happens if sticking occurs at some point during the

compression� extension� or restitution integrations� Practically� sticking is detected when

the magnitude of the tangential component of relative contact velocity falls below some

��

tolerance �s� At this point� the collision resolution algorithm determines if the sticking is

stable or not �Theorem �� and proceeds accordingly� In either case� the normal sliding

integration halts� and the evolution of the variables for the remainder of the collision may

be computed by solving simple algebraic equations� The solutions of three �rst order ODEs

will be useful� the claims of the following observation are veri�ed by separating variables

and integrating �BD��

Observation � solutions of �rst order ODEs� Let y be a di�erentiable function of an

independent variable that varies over the �a� b�� and C be a constant� Then�

dy

d � C y�b� � y�a� � C�b� a� ��

dy

d � C y�b� � y�a� �

�

Cb� � a�

��

dy

d �C

y y�b� �

qy�a�� C�b� a� ��

First consider the case of stable sticking� There are two varieties� depending on

whether the sticking was detected during integration with respect to uz or Wz integration�

Suppose a uz integration was in progress� Let a be the current value of uz� when sticking was

detected� and let b be the upper limit of uz for the current integration �b � � if sticking was

detected during the compression integration� or a positive number if sticking was detected

during the extension integration�� The goal is to determine ux�b�� uy�b�� and Wz�b�� Stable

sticking implies ux�b� � uy�b� � �� By Theorem ��

dWz

duz� uz

dpzduz

�

Since du�duz � �� T under stable sticking� Theorem � implies dpz�duz � K��

�� Thus

dWz

duz� K��

�� uz� ��

and applying the second rule of Observation � gives

Wz�b� �Wz�a� ��

K��

��

b� � a�

��

Next suppose that stable sticking prevails at the beginning of the restitution in�

tegration� or that it occurs during it� Let a be the current value of Wz and b be the �nal

value� The goal is to compute u�b�� and again ux�b� � uy�b� � � because sticking is stable�

Rewriting �� asduzdWz

��K��

��

uz�

�

and applying the third rule of Observation � gives

uz�b� �

suz�a��

K��

�b� a��

Instable sticking occurs if the contact force required to maintain the sticking lies

outside the friction cone� in this case sliding must resume� How this happens is not obvious�

but the trajectory plots in Figure � give a clue� Sticking occurs for this collision if the

tangential velocity point reaches the origin� For the K and � that generate that plot�

sticking is instable and the tangential velocity point must leave the origin� In the plot�

there are two rays along which the sliding direction is constant� Referenced to the positive

x�axis� there is one ray at �� and one ray at �� If the tangential velocity point is oneither of these rays� the direction of sliding remains constant and the point never leaves the

ray� The �� ray is called a converging ray� since velocity point moves along it toward the

origin� and the �� ray is a diverging ray� since the velocity point moves along it away

from the origin� If the velocity point is not on one of these rays� the direction of sliding

is continuously changing� The tangential velocity point can only leave the origin along a

diverging ray� Leaving in any other direction would mean crossing �ow lines or �owing in

the wrong direction along a converging ray� For any K and �� the sliding plot resembles

the one in Figure �� although the number of converging and diverging rays may vary�

After instable sticking� sliding can only resume along a diverging ray� which implies that

the direction of sliding is constant after instable sticking� Fortunately� resuming sliding is

a deterministic process� When sticking is instable� there is exactly one diverging ray along

which sliding may resume� The next section proves this claim� and gives an algorithm for

computing the direction of the ray� Taking the claim on faith for now� call the direction

of the unique diverging ray �� which can take on any value modulo �� To resume sliding

after instable sticking� one option is to set the values of ux and uy to lie on the diverging

ray� very close to the origin�

ux � � cos �

uy � � sin�

for some very small �� Then the normal sliding mode integration could be resumed as the

tangential velocity �ows away from the origin� A better approach is to use the knowl�

edge that the sliding direction is constant after instable sticking� in order to solve for the

subsequent motion of the velocity point algebraically�

��

First consider the case of instable sticking occurring during integration with respect

to uz� Let a be the current value of uz� and b the upper limit of the integration� The goal

is to compute ux�B�� uy�b�� and Wz�b�� From the chain rule and Theorem ��

duxduz�dux�dpzduz�dpz

�kx��

kz��

Since � � � after instable sticking� the right hand side is constant� Applying the �rst rule

of Observation ��

ux�b� � ux�a� �kx��

kz��b� a��

The value uy�b� is derived analogously�

uy�b� � uy�a� �ky��

kz��b� a��

From Theorems � and ��

dWz

duz� uz

dpzduz�

uzduz�dpz

�uz

kz��

Applying the second rule of Observation � gives

Wz�b� �Wz�a� �b� � a�

kz��

Finally� consider instable sticking occurring before or during the restitution inte�

gration� Let a be the current value of Wz� and b be the upper limit of the integration� The

goal is to compute u�b�� Inverting ��

duzdWz

�kz��

uz�

Applying the third rule of Observation � gives

uz�b� �quz�a�� kz��b� a��

Once uz�b� is known� ux�b� and uy�b� can again be computed using the same technique as

before�

ux�b� � ux�a� �kx��

kz��uz�b�� uz�a��

uy�b� � uy�a� �ky��

kz��uz�b�� uz�a��

��

�� Sticking stability and rays of constant sliding

The previous section introduced the problem of deciding how to resume sliding

when instable sticking is detected during a collision integration� the tangential velocity

point leaves the origin along a diverging ray of constant sliding direction� For the plot of

Figure � this is possible because there is exactly one such ray� Consider the tangential

velocity �ow in Figure �� Here� there are no diverging rays� and so it is impossible for the

-10

-8

-6

-4

-2

0

2

4

6

8

10

-10 -8 -6 -4 -2 0 2 4 6 8 10

y ve

loci

ty

x velocity


Figure �� A tangential velocity ow plot with two converging rays and no diverging rays�

tangential velocity point to escape from the origin� For the K and � used to generate this

plot� sticking is stable� and so the lack of diverging rays is of no consequence� But can this

situation arise in an instable sticking case� Next consider the velocity �ow in Figure ��

Here� there would appear to be indeterminacy after instable sticking because there are two

diverging rays� The K used to generate this plot is symmetric but not positive de�nite�

��

-10

-8

-6

-4

-2

0

2

4

6

8

10

-10 -8 -6 -4 -2 0 2 4 6 8 10

y ve

loci

ty

x velocity


Figure �� A tangential velocity ow plot with two converging rays and two diverging rays�

hence it is not even a valid choice� But it is important to verify that such indeterminacy

can never occur with valid K� This section addresses these questions�

The issues addressed in this section have also been extensively studied by Bhatt

and Koechling �BK�� BK��b� BK��a�� Their approach is to parameterize the space of all

three�dimensional rigid body collisions into regions that are qualitatively similar� such as the

set of collisions where there are no diverging rays� From their partitioning� the qualitative

character of the collision can be determined a priori from the initial conditions� One of the

main results of this section is a proof that the behavior is determinate in cases of instable

sticking �Theorem �� Bhatt and Koechling have also recently proved this long standing

conjecture� using a purely algebraic method� the proof in this section is more geometric�

Wang and Mason also partitioning collision space based on qualitative behavior� but for the

case of two�dimensional collisions �WM��

��

�� The u� ellipse

For the remainder of this section� the entries of the collision matrix are given by

K �

��a f e

f b d

e d c

��

To study the evolution of sliding velocity during collision� the most convenient parameteri�

zation to use is the one based on pz� and speci�ed by Theorem �� Using the above de�nition

of K� the theorem implies�� u�x

u�y

��

�� a f

f b

�� z �

M

�� cos �� sin �

�� e

d

�� z �s

� ��

where M and s are the designated sub�matrices of K� To simplify notation� the explicit

dependence of u and � on pz is dropped in the rest of the analysis� primes denote derivatives

with respect to pz� Since only the tangential velocity is being studied� the the third �normal�

component of relative velocity is dropped� so that u � �ux� uy�T � and u� � �u�x� u

�y�T � Since

M is a major sub�matrix of the positive de�nite matrix K� it is also positive de�nite�

Therefore�

M � R�MdRT� � ��

whereMd is a � diagonal matrix with positive entries along the diagonal� and R� is the

orthogonal two dimensional rotation matrix for some angle ��

R� �

�� cos� � sin�sin� cos�

�� See �HJ�� for justi�cation of these assertions concerning positive de�nite matrices�� Com�

bining results� �� u�x

u�y

�� R�MdRT�

�� cos �� sin �

�� s�As the sliding direction � varies over �� u� traces out an ellipse in a counter�

clockwise direction� It is not the case that � or u� actually vary in this manner during a

collision� but the analysis to come is based on how the point u� behaves as a function of ��

Calling the diagonal entries ofMd ad and bd� the series of transformations indicated by the

above equation is shown in Figure ��

��

− cos− sin

θθµ

µθ

(A)

µθ−α

(B) (C)

µad

µbd

α

(D)

d

e

(E)

− cos− sin

θθµ + sRα

TRαMd

− cos− sin

θθµRα

TRαMd

− cos− sin

θθµ TRαMd

− cos− sin

θθµ TRα

Figure �� A sequence of transformations applied to the point �� cos �� sin ��T in orderto produce the velocity derivatives �u�x� u

�y�T � The latter trace out a counter�clockwise ellipse

as � varies over ��

Lemma � The u� ellipse �shown in Figure ��E�� circles the origin if and only if the

entries of the collision matrix K satisfy

�b� � f��e� � �a� � f��d� � �a� b�fde � ��jMj��

Proof� Establish a new frame G located at the center of the u� ellipse and rotated an angle� relative to the original frame F � as shown in Figure �� The coordinates of the origin�of frame F� expressed in frame G are given by

o �

�� ox

oy

�� R��s� � �RT�s�

The ellipse encircles this point if and only if�oxa�

��

�

�oyb�

��

� ��

��

αa’µ

b’µ

e

d

F

G

Figure �� Testing whether the u� ellipse circles the origin �Lemma ��

Since the diagonal elements of M��d are ��a� and ��b�� this criterion can be written

kM��d ok � �M��

d o�T �M��d o� � oTM��

d o � ��

From �� M��d � R

T�M

��R�� Again rephrasing the criterion��RT

�s�T

RT�M

��R�

�� RT�s��

which conveniently reduces to

sT M�� s � ��

M�� is given by

M��

jMj

�� b �f�f a

�� and so the criterion becomes

sT

�� b �f�f a

��s � ��jMj��

�

Straightforward computation with s � �e� d�T gives the lemma� �

The criterion of this lemma seems cryptic� but it can be expressed much more simply�

Theorem �� The u� ellipse circles the origin if and only if sticking is stable�

Proof� By the direct inversion formula�

K��

jKj

�� f e

b d

�� a e

f d

�� a f

f b

��

��

where the dots mark entries which are irrelevant� Notice that the determinant in the lower

right corner of the matrix is jMj� In terms of the entries of K� the stable sticking criteriongiven by Theorem � can be written�BBBBBBB�

�� f e

b d

��jKj

�CCCCCCCA

�

�

�BBBBBBB�� a e

f d

��jKj

�CCCCCCCA

�

� �� jMjjKj

��

�

Multiplying through by jKj� and expanding the determinants on the left hand side leads tothe same criterion given in Lemma ��

�� Directions of constant sliding

Let Tan�� y� x� denote the four quadrant arctangent function which returns the

unique angle � in �� such that y � r sin� and x � r cos� for some positive constant r�

The sliding direction � is given by

� � Tan�� uy� ux� �

If u� is in the same direction as u or exactly opposed to it� Then one of the following holds�

Tan��u�y� u

�x

��

Tan��u�y� u

�x

�� mod ��

�

If either of these relations hold� then the direction of u remains constant during collision

integration� Values of � which satisfy these relations de�ne the location of diverging and

converging rays� respectively� Let �u � �cos �� sin ��T be the unit vector aligned with u�

Then one of the above relations is satis�ed if and only if �u�u� � � The particular relationwhich holds can be determined by the sign of �u � u�� if it is positive� the �rst one holds�if it is negative� the second one holds� These results� along with �� yield the following

theorem�

Theorem �� location of rays� Rays of constant sliding direction are located at angles

� satisfying the equation

��u� u�� a� b�� cos � sin � � f��sin� � � cos� �� d cos � � e sin � � ��

At such values of �� if

��u � u�� a� cos� � � b� sin� � � f� cos � sin � � d sin � � e cos �

is positive� the ray is diverging� otherwise it is converging�

Corollary � There can be no more than four rays of constant sliding direction�

Proof� The cross product criterion of Theorem �� can be converted to a polynomial equa�

tion by making a standard change of variables� t � tan�� cos � � �� t�� t��

sin � � �t�� t�� This leads to a fourth order polynomial equation in t� having at most

four roots� �

The above corollary also gives an e�cient algorithm for computing the directions

of the converging and diverging rays� The roots of a fourth order polynomial are computed

algebraically� These give the directions of the rays� which are then easily classi�ed as

converging or diverging� based on the test of Theorem �� The lighthouse analogy of

Figure �� is useful in understanding the rays of constant sliding direction� A lighthouse

at the origin rotates counter�clockwise� sweeping a white beam across the plane� It also

emits a red beam in the opposite direction of the white beam� The location white beam

represents the sliding direction �� The u� ellipsoid is also located in the plane� and as the

� varies� the point u�� moves around this ellipsoid in a counter�clockwise direction� Let

�� be the angle this point makes with the positive x�axis� If at a particular value �� the

��

white beam

red beam

θβ(θ)

u’( )θ

lighthouse

u’ ellipse

Figure �� The lighthouse analogy for rays of constant sliding direction�

white beam directly illuminates the point u�� there is a diverging ray at �� If at a particular

value �� the red beam directly illuminates the point u�� then there is a converging ray ��

Lemma � Consider the point u� moving counter�clockwise around the ellipse shown in

Figure �� as � goes from � to �� Then �� implies d��d� � �� In other words�

when the point is illuminated by the white beam� �� can not be increasing�

Proof� � � Tan��u�y� u

�x

�� and so

d�

d��

�

� �u�y�u

�x

�� d

d�

�u�yu�x

��du�y�d��u

�x � �du�x�d��u�y

u�x� � u�y

� �

Therefore�

sgn

�d�

d�

�� sgn

�du�yd�

u�x �du�xd�

u�y

��

�

Di�erentiating �� gives

d

d�

�� u�x

u�y

�� a� sin � � f� cos �

f� sin � � b� cos �

�� and so

sgn

�d�

d�

�� sgn

h�f� sin � � b� cos ��u�x � �a� sin � � f� cos ��u�y

i�

If �� then u�x � r cos � and u�y � r sin � for some positive constant r� Substituting

these values into the above equation� dividing the argument of the right hand sgn by the

positive quantity �r yields�

sgn

�d�

d�

�� sgn��b cos� � � a sin� � � f cos � sin �� z �

D

��

M is positive de�nite� so its determinant ab� f� is positive� andpab � jf j� Thus�

D � �b cos� � � a sin� � � pab j cos �j j sin �j

� ��pb j cos �j � p

a j sin �j��

This implies d��d� � ��

Theorem �� If sticking is stable� there are no diverging rays of constant sliding direction�

Proof� If sticking is stable� the u� ellipse circles the origin �lighthouse� by Theorem �� For

this case� d��d� must be positive during the entire counter�clockwise circuit of the point u�

around the ellipse� and therefore by Lemma � �� can never equal ��

Theorem �� If sticking is instable� there is exactly one diverging ray of constant sliding

direction�

Proof� If sticking is stable� the u� ellipse does not circle the origin �lighthouse� by Theo�

rem �� In this case� �min � �� max� with �max � �min � �� Clearly� as the white

beam sweeps over the range ��min� �max�� it must illuminate the point u� at least once� and

so there is at least one value of � for which �� To see that there is not more than

one� consider the graph shown in Figure �� Lemma implies that the graph of �� can

��

θ

β(θ)

2π

< π

2π0

maxβ

minβ

β(θ) = θ

β(θ)

β(θ) = θdβ dθ 0</

Figure �� Proving there can be no more than one diverging ray when sticking is instable�

never cross the line �� while �� is increasing� and therefore it can cross only once�

�More crossings would be possible if the graph of �� wrapped around� over an interval

of �� but this is impossible since �max� �min � �� The conclusion is that there is exactly

one value of � for which ��

Theorem � guarantees that Figure �� is representative of all �ow plot when

sticking is stable� rays only converge to the origin� none diverge� Theorem �� proves that

when sticking is not stable� there is exactly one diverging ray� as in Figure �� Thus� there

is a well de�ned direction along which sliding resumes after instable sticking� This result

had been previously conjectured in �BK�� MC� a�� Recently� Bhatt and Koechling have

devised an alternative� algebraic proof of this result �BK��a�� The previous two theorems

also prove that �ows like the one of Figure �� never occur for valid K� which are positive

de�nite�

��

A �nal loose end to tie up is how to handle situations where duz�d is nega�

tive at the beginning of a collision� This paradoxical situation occurs when the collision

force initially accelerates the bodies into each other instead of away from each other� The

phenomenon is well known� see �Bar�� for a speci�c example�

Theorem �� For values of � for which the dot product discriminant of Theorem � is

positive� duz�dpz is also positive�

Proof� The hypothesis can be rephrased as

�a�� cos� � � b�� sin� � � f�� cos � sin � � d� sin � � e� cos � � ��

Since K is positive de�nite� ��TK�� is positive� Expanding this product yields

a�� cos� � � b�� sin� � � f�� cos � sin � � c� e� cos � � d� sin � � ��

Adding these two inequalities�

�e� cos � � d� sin � � c � ��

From Theorem �� duz�dpz is positive exactly when the above inequality is satis�ed� �

As the collision is integrated� the relative tangential velocity point asymptotically

approaches the rays of constant sliding direction� This is evident in the �ows of Figures ��

�� and �� If it approaches a diverging ray� then in some neighborhood of the ray the

conditions of Theorem �� will be met� and uz will increase for the remainder of the collision�

If it approaches a converging ray� the point will eventually reach the origin� If sticking is

not stable� the point leaves along a diverging ray� and uz will again increase for the duration

of the collision� If sticking is stable� then by de�nition� u travels in the direction �� T

for the duration of the collision� Hence� in any case� uz eventually increases� and remains

increasing for the duration of the collision� To handle cases for which uz is is initially

decreasing and these are extremely rare� pz is chosen as the initial integration param�

eter� and is used until duz�dpz becomes positive� At this point� the normal compression

integration with respect to uz can begin� and the rest of the collision integration is handled

as before�

�In the vast majority of simulations performed with Impulse� including all of those described in theChapter � collisions in which uz is initially decreasing never occur�

��

�� Static contact and microcollisions

Under impulse�based simulation� continuous contact� such as that between a book

and the table is rests on� is modeled through trains of collisions� In this case� the collision

impulses model a constant reaction force which does no work on the stationary objects�

The collision resolution method described thus far is not completely suitable for modeling

such static contact forces� To understand the problem� consider the book on table example�

Through collisions� the energy of the book steadily decreases� as the book sinks further and

further into the collision envelope of the table� The decreased distance between the bodies

causes time of impact estimates to decrease� increasing collision checks between the bodies�

The simulator would be brought to a grinding halt as the separation distance and the time

between impacts approached zero�

To alleviate this problem� microcollisions are proposed as a way of modeling static

contact forces� Microcollision impulses are not computed in the standard way� For a collision

to be a microcollision candidate� the initial relative normal velocity at the contact point

must be small since static contact occurs only as objects settle onto one another� high

relative normal velocities at the contact point correspond to colliding contact� In Impulse�

this criterion is de�ned precisely as

�uz� �� pg�c� ��

where g is the acceleration of gravity� The right hand side is the velocity that an object

acquires as it falls from rest through the collision envelope� Since the distance between

objects in static contact is approximately �c� this seems a reasonable velocity bound�

For microcollisions� the coe�cient of restitution is arti�cially increased based on

the penetration of the bodies within the collision envelope� as shown in Figure �� The

graph shows e� the actual coe�cient of restitution used in resolving the collision� as a

function of the penetration depth �� which is in the range �� c�� The value edef� � �edef � �� is the default value for e based on material properties of the objects� The valueemax is the maximum value e may assume� As shown in the graph� e�� is a second order

curve� with vanishing derivative at � � �� This is so that e can be made fairly large for deep

penetrations� while still remaining close to its default value for most collisions� which have

small penetrations� In Impulse� emax was chosen as �� This value is enough to prevent the

simulation speed from slowing down noticeably when objects come to rest on other objects�

��

ε

emax

edef

0

coef

fici

ent o

f re

stitu

tion

(e)

envelope penetration ( )δ

Figure �� The coe�cient of restitution is arti�cially increased to prevent prolonged� deeppenetration into the collision envelope�

The resting object vibrates on its supporting object with a vibration amplitude on the order

of �c� for most views� this amplitude is less than a pixel� and so the object appears at rest�

A related problem is that of a block on a ramp �Figure �� If the coe�cient

g

f t

fngn

gt

Figure �� Standard collision impulses will cause the block to creep down the ramp� nomatter what the coe�cient of friction is�

��

of friction is high enough� the block should be brought to rest on the ramp� the static

friction force opposing the force of gravity� Again� the contact forces do no work once the

bodies are stationary� At most collisions� the response algorithm described thus far will

bring the tangential velocity at the contact point to zero� As the collision ends� the block

again becomes airborne� following a ballistic trajectory� During this interval� gravity is

unchecked and will impart a linear acceleration with a component down the ramp� Upon

the next collision� the downward velocity will again be halted� but the block motion during

the ballistic phase remains uncorrected� Since the velocity of the block down the ramp

varies between � and some small positive �� the block will creep slowly down the ramp�

much as a real block would if placed on a vibrating ramp�

To correct this problem� a second type of microcollision is employed� When the

initial relative normal velocity satis�es �� the unique impulse pr that reverses this

velocity is computed� This impulse is given by �� with �u � �u� ��

pr � �K��u� ��

The cost of computing pr is much less than that of performing a standard collision inte�

gration� Furthermore� by Corollary �� pr does no net work on the colliding bodies� like

the static force is models� If pr lies within the friction cone it is applied� otherwise the

impulse is computed through collision integration with a boosted coe�cient of restitution�

The reversing microcollisions give the block a small �kick� back up the ramp� canceling

the e�ects of gravity during the ballistic phase� Creep is still present� but is very small

�Figure ��

The decision to classify a collision as a microcollision is based only on local in�

formation at the contact point� This is in keeping with the impulse�based approach of not

explicitly classifying macroscopic state� but instead generating correct macroscopic behavior

through collisions� When one object settles on another� the process is a smooth transition

from a situation in which virtually all collisions are ordinary collisions to a situation in which

virtually all collisions are microcollisions� Having to make explicit decisions about when an

object is resting on another� or whether a contact is of a rolling or sliding nature� creates

arti�cially sharp transitions in system behavior� and leads to the consistency problems that

arise in constraint�based simulation�

The ordinary collision response computations derived in this chapter are on a solid

theoretical framework� given the assumptions made� Microcollisions are harder to justify

��

0 1 2 3 4 5

0

50

100

150

200

250

300

350

400

450

500

550

posi

tion

(cm

)

time (s)

Position of Block

0 1 2 3 4 5

0

50

100

150

200

250

300

350

400

450

500

550

600

time (s)

velo

city

(cm

/s)

Velocity of Block

Figure �� Data from the block on ramp experiment using Impulse� A block is launcheddown a �� ramp with an initial sliding velocity of �� cm�s� with � � �� and g � ��cm�s�� Theoretically� the block should experience constant deceleration for �� seconds�coming to rest after traveling �� cm� The simulator�s results agree closely� although theplots reveal slight creep in the position of the block�

from a physical standpoint� Important properties are preserved when using microcollisions�

for example� the contact forces on the bodies still lie within friction cones� even when micro�

collisions are used� But ultimately microcollisions are an ad�hoc way to make impulse�based

simulation feasible for modeling prolonged contact between bodies� There are limitations of

what can be done� even using microcollisions� The creeping problem� although slight� may

be unacceptable for some applications� The static contact between a book and a table poses

no problem� nor does a stack of two books� By three books� the simulation is noticeably

slower� and four or more causes it to grind to a halt� Too many collision impulses must

be propagated up and down the stack of books� and the weight of the other books on the

bottom book causes deep envelope penetration between the bottom book and the table�

This penetration could be reduced by increasing edef� but too high a value adds instabil�

ity to ordinary situations� Furthermore� parameter tuning has been eschewed through the

development of Impulse� A stack of books on a table is a situation which is much more

naturally and e�ciently handled with a constraint�based approach�

�

Chapter �

Constrained Body Dynamics

Impulse�based simulation can not e�ciently model tightly constrained contact�

such as that occurring at a hinge joint� It is useful� however� when combined with a classical

method for modeling this constrained contact� This combination is called hybrid simula�

tion� and is discussed in Chapter � The present chapter develops the forward dynamics

algorithms for several classes of multibodies� collections of rigid bodies connected by joints�

The algorithms presented here are the original work of Featherstone �Fea��

Featherstone�s algorithm is a generalized coordinate approach� meaning that there

are as many state variables as degrees of freedom in the system� The advantage of this

approach is that there are no invalid state con�gurations� the constraints are automatically

enforced� Maximal coordinate methods� also called multiplier methods� can also be used to

derive the dynamics of constrained body systems� These employ more state variables than

there are degrees of freedom� and so constraints must be continuously enforced� Multiplier

methods are more naturally applied to more general kinematic structures� such as those

with kinematic loops� They su�er� however� from drift problems due to the coordinate

redundancy� Some di�erential�algebraic equation �DAE� solvers� such as MEXX �LNPE��

employ sophisticated methods for maintaining constraints� at the price of added complexity�

A summary of the pros and cons of generalized and maximal coordinate approaches is in

�Bar�� Clear comparisons of the speed and accuracy of the two methods have not been

published� the best choice may be application dependent�

The results of this chapter are not original� but are included for two reasons� The

�rst is tutorial� Featherstone�s algorithm has been presented by him as well as by several

other dynamicists �BJO�� Lil�� JR�� yet the algorithm remains confusing and di�cult to

�

implement for many who are not specialists in the area�� The sources mentioned above have

a tendency to describe the algorithm from a very theoretical viewpoint� or to sacri�ce detail

and completeness for brevity� This chapter derives Featherstone�s equations by taking a very

di�erent path� the equations are derived from the basic principles of rigid body dynamics�

taught in any physics course� Spatial notation is adopted only after a thorough �non�spatial�

treatment of the underlying quantities� Extensions of the basic serial chain algorithm to

handle tree�like linkages and �oating linkages are explicitly described� The second reason

for this chapter is to provide a foundation for multibody dynamics which will be used in

Chapter to develop the collision response model for hybrid simulation�

Section �� describes the basic forward dynamics problem for constrained linkages�

Section �� details how the absolute velocities of all links are determined from the joint

positions and velocities� a procedure which is omitted in most expositions on constrained

dynamics� Acceleration propagation equations are also derived� Section �� introduces

spatial algebra� Section �� derives Featherstone�s complete algorithm for grounded� serial

linkages� from �rst principles of rigid body mechanics� Sections �� and �� extend the basic

algorithm to tree�like linkages and �oating linkages� respectively�

�� Constrained forward dynamics

Consider the serial linkage shown in Figure �� The links are numbered from �

to n� where link � is attached to a �xed base �called link �� and link n is the outermost

link� The joints are also labeled from � to n� For � � i � n� link i has an inboard joint�

which is closer to the base� and an outboard joint� which is closer to the other end of the

linkage� link n has only an inboard joint� The inboard joint of link i is joint i �� i � n��

and the outboard joint of link i is joint i� � �� i � n�� When each joint only allows one

degree of freedom� a compact parameterization of the con�guration space of the linkage is

the vector of joint positions q � �q�� qn�T � The problem to be solved is�

Problem � Forward dynamics� Given� The positions q and velocities #q of the n joints

of a serial linkage� the external forces acting on the linkage� and the forces and torques being

applied by the joint actuators� Find� The resulting accelerations of the joints� �q�

�Bara� also makes this point in �Bar��

��

link 0(base)

link 1

link i

link n

joint 1

joint ijoint i+1

joint n

outboardinboard

Figure �� The link and joint indexing conventions for serial linkages�

A major hurdle in understanding the dynamics of linked rigid body systems is

the complex notation that is required� For convenience� Table �� summarizes the notation

which is introduced and used throughout this chapter�

�� Velocity and acceleration propagation

To solve the forward dynamics problem one must �rst determine the absolute

motion of all of the links of the linkage� The linear and angular velocities of the links

depend only on q and #q� The linear and angular accelerations of the links depend on these

quantities and also on �q� Although �q is initially unknown� the link accelerations can still

be expressed in terms of it�

De�nition � Attached to each link i is a body frame Fi� with origin at the link�s center

of mass� and axes aligned with the principle axes of inertia for the link� Vectors expressed

in link coordinates have coordinates resolved relative to this body frame�

The immediate goal is to solve this subproblem�

Problem � Velocity and acceleration propagation� Given� The joint positions q�

velocities� #q� and accelerations �q� Compute� For each link� the linear velocity vi� angular

velocity �i� linear acceleration ai� and angular acceleration �i� all relative to an inertial

frame�

�

n number of links of serial linkage

vi linear velocity of link i�i angular velocity of link iai linear acceleration of link i�i angular acceleration of link ivrel relative linear velocity of link i �De�nition ��rel relative angular velocity of link i �De�nition ��

mi mass of link iMi matricized mass of link i �Equation �� Ii diagonalized �body frame� inertia tensor of link i �Appendix A��

di vector from link i inboard joint to link i c�o�m� �Figure ��ri vector from link i� � c�o�m� to link i c�o�m� �Figure ��ui unit vector in direction of joint i axis �Figure ��Ii spatial isolated inertia of link i �De�nition ��

�IA

i spatial articulated inertia of link i �Theorem ��Zi spatial isolated zero�acceleration �z�a�� force of link i �De�nition ��

�ZA

i spatial articulated zero�acceleration �z�a�� force of link i �Theorem ��

�fI

i spatial force applied by inboard joint to link i �De�nitions ��

�fO

i spatial force applied by outboard joint to link i �De�nitions �� vi spatial velocity of link i �De�nition ��ai spatial acceleration of link i �De�nitions � ��si spatial joint axis of joint i �De�nition ��ci spatial Coriolis force for link i �De�nition ��

qi scalar position of joint i#qi scalar velocity of joint i�qi scalar acceleration of joint i�i vector velocity of joint i �Equation �� i vector acceleration of joint i �Equation ��

O inertial reference frameFi body frame of link i �De�nition �

G�XF spatial transformation from frame F to frame G �De�nition ��

j�Xi spatial transformation from frame Fi to frame Fj

Table �� Notation used in Chapter �

The quantities vi��i�ai� and �i describe the motion of frame Fi relative to an

inertial frame O� The strategy for solving this problem is straightforward� The velocitiesand accelerations of link i are completely determined by the velocities and accelerations of

link i� �� and the motion of joint i� Since the velocities and accelerations of the base link

��

are � the base link is a starting point� and one proceeds inductively from there�

A few additional vectors are needed� as shown in Figure �� Let ui denote the

Fi

link i−1

link i

joint i

u i

id

i−1F ri

Figure �� Quantities for propagating velocities and accelerations�

unit vector in the direction of the axis of joint i� ri denote the vector from the origin of

Fi�� to the origin of Fi� and di denote some vector from the axis of joint i to the origin of

Fi�

The motion of link i may be divided into two parts� one due to the motion of link

i � �� and one due to the motion of joint i� The latter component is called the relativemotion�

De�nition � Relative velocities� The linear relative velocity vrel is the linear cen�

ter of mass velocity of link i due only to the motion of joint i� that is� the linear center of

mass velocity that link i would have if link i � � were held �xed� The angular relative

velocity �rel is the angular velocity of link i due only do the motion of joint i� that is� the

angular velocity that link i would have if link i� � were held �xed�

The angular velocity of link i is the angular velocity of link i� � plus the angularvelocity induced by rotation at the joint�

�i � �i�� rel� ��

��

Propagating linear velocity is similar� but there is an added term due to the rotation of

link i� ��vi � vi�� i�� ri � vrel� ��

The formula for propagating angular acceleration is obtained by di�erentiating ��

�i � �i�� #�rel� ��

The formula for propagating linear acceleration is obtained by di�erentiating ��

ai � ai�� i�� ri � �i�� #ri � #vrel�

Since #ri � vi�vi�� which by �� is �i��ri � vrel� the above equation can be rewritten

ai � ai�� i�� ri � �i�� i�� ri� � �i�� vrel � #vrel� ��

Equations ��$�� express link i�s motion in terms of link i � ��s motion� andthe relative motion due to joint i� All that remains is to de�ne �rel� vrel� and their time

derivatives for prismatic and revolute joints� To this end� de�ne two vectors along the axis

of joint i�

�i � #qiui ��

�i � �qiui� ��

Suppose joint i is prismatic� Alone it imparts no angular velocity to link i� only a linear

center of mass velocity� From Figure �� and De�nition ��

�rel � ��

vrel � �i� ��

On the other hand� a revolute joint i imparts both an angular velocity and a linear center

of mass velocity to link i� From Figure �� and De�nition ��

�rel � �i ��

vrel � �i � di� ��

The following lemma is useful in computing #�rel and #vrel�

��

Lemma � For prismatic or revolute joints�

#�i � �i � �i�� i�

Furthermore� for revolute joints�

d

dt��i � di� � �i�� i � di� � �i � di � �i � ��i � di��

Proof� Di�erentiating ��

#�i � �qiui � #qi #ui � �i � #qi #ui�

Since the joint axis ui rotates with link i� �� #u � �i�� ui� Substituting this result into

the above equation proves the �rst claim of the lemma� For the second claim� note

d

dt��i � di� � #�i � di � �i � #di� ��

Now #�i is given by the �rst claim� and di rotates with link i�

#di � �i � di � ��i�� i�� di�

�The second equality above follows from �� and �� Substituting for #�i and #di into

�� distributing the cross products� and applying the identity

A� �B�C� � B� �C�A� � �C� �A�B�

proves the second claim� �

The results of Equations ��$�� plus Lemma � are summarized below�

joint i prismatic joint i revolute

�rel �ivrel �i �i � di#�rel �i � �i�� i#vrel �i � �i�� i �i�� i � di� � �i � di � �i � ��i � di�

Substituting these quantities into equations ��$�� produce the propagation formulae

that give link i�s motion in terms of link i� ��s motion� and the motion of joint i� If joint iis prismatic�

�i � �i��

vi � vi�� i�� ri � �i ��

�i � �i��

ai � ai�� i�� ri � �i � �i�� i�� ri� � �i�� i� ��

��

If joint i is revolute�

�i � �i�� i ��

vi � vi�� i�� ri � �i � di ��

�i � �i�� i � �i�� i ��

ai � ai�� i�� ri � �i � di

� �i�� i�� ri� � �i�� i � di� � �i � �� i � di��

In Equations ��$�� all velocity and acceleration vectors are absolute� mean�

ing they are taken relative to the inertial frame O� In computations� however� the co�ordinates of the vectors may be expressed relative to any convenient coordinate frame�

Computations are simpli�ed if all vectors are expressed in Fi� the body frame attached to

link i�

As an example� consider the planar linkage of Figure �� Assuming joint �

link 1

x

y

y

x

O

F1v1

a1

1q

1d

Figure �� The vectors v� and a� describe the motion of the center of mass of link � relativeto the inertial frame O� But the coordinates of these vectors may be expressed in any frame�

rotates counter�clockwise with constant angular velocity� the expressions for v� and a� in

��

O�s coordinates are

v� �

�� #q�d� sin q�#q�d� cos q�

�� a� �

�� #q��d� cos q�� #q��d� sin q�

�� The same vectors expressed in Fi�s coordinates are

v� �

��

#q�d�

�� a� �

�� #q��d��

�� These latter expressions are simpler because F� rotates with link �� Also� the vectors di and

ui are �xed in Fi� so many of the subexpressions appearing in ��$�� may be precom�

puted� The only catch is that vector coordinates expressed in Fi�� must be transformed to

frame Fi before they can be used in computations with other vectors expressed in Fi� This

amounts to multiplying the four vectors �i��vi��i�� and ai�� by a rotation matrix

before using them in ��$�� If joint i is prismatic� this rotation matrix is constant and

may also be precomputed� if join i is revolute� the matrix depends on qi�

The �rst step in the forward dynamics algorithm is the computation of the absolute

linear and angular velocities of all links� These are determined from the known dynamic

state �q� #q�� and determine the centripetal and Coriolis forces acting on the links� The

algorithm for computing these velocities follows directly from Equations ��$�� and

��$�� and is shown in Figure �� The derived acceleration propagation equations

involve the unknown joint accelerations through the term �i� These will be be needed in

deriving the rest of the forward dynamics algorithm�

�� Spatial algebra

Spatial algebra is a powerful notation for describing quantities and relations rele�

vant to the dynamics of three�dimensional systems� A spatial vector is six�dimensional and

replaces two ordinary vectors in R� � The spatial acceleration of a rigid body describes both

its linear and angular accelerations� The spatial force applied to a rigid body describes both

the applied �ordinary� �D force and the applied �D moment� Throughout this chapter�

Featherstone�s convention of denoting spatial quantities with a caret is used�

De�nition � For a rigid body moving through space� with a frame F attached to it� let

v� �� a� and � be the linear velocity� angular velocity� linear acceleration� and angular

��

compSerialLinkVelocities

��v��a�

for i � � to n

R rotation matrix from Fi�� to Fi

r radius vector from Fi�� to Fi �in Fi coordinates�

�i R�i��

vi Rvi�� i � r

if joint i is prismatic

vi vi � #qiui

else �� joint i is revolute ��

�i �i � #qiui

vi vi � #qi�ui � di�

Figure �� compSerialLinkVelocities� Compute the absolute linear and angular veloc�ities of all links of a serial linkage� in link coordinates� Input data are the dynamic state�q� #q�� Output data are the velocities �i and vi for each link�

acceleration of frame F relative to an inertial frame O� Then the spatial velocity �v and

spatial acceleration �a of the body� expressed in frame F � are given by

�v �

��

v

�� a �

��

a

�� Now consider a rigid body moving through space with two frames� F and G at�

tached to it� where G is displaced but not rotated relative to F � Let vF and vG be thelinear velocities of the origins of these frames� let � be the common angular velocity of

the frames� and let r be the o�set vector from the origin of F to the origin of G� ThenvG � vF � � � r� and so

�vG �

��

vG

��

vF � � � r

��

�"r �

��

vF

��

��

�� is the � identity matrix� see Appendix A� for a de�nition of "r�� The matrix onthe right hand side is a � � � spatial matrix� If G is also rotated relative to F � and R isthe rotation matrix that transforms vector coordinates in F to vector coordinates in G� thevector on the right hand side must be premultiplied by the spatial matrix�� R

R

��in order to account for the rotation of G relative to F � This leads to the following de�nition�

De�nition � Let F and G be two frames� let r be the o�set vector from the origin of Fto the origin of G �expressed in G�s coordinates�� and let R be the � rotation matrix

transforming �non�spatial� vectors from F to G� Then the � � � spatial transformationmatrix from F to G is given by

G�XF �

��

�"r �

�� R

R

�� R

�"rR R

�� With this notation� the spatial velocity transformation shown above can be written

�vG � G�XF �vF �

A spatial transformation matrix is the analog of an ordinary rotation matrix� it transforms

the coordinates of a spatial vector from one frame to another� The matrix G �XF transforms

spatial accelerations from frame F to frame G in the same way as it transforms velocities�Spatial vectors are also useful for describing external in�uences acting on rigid bodies� Given

a rigid body with an attached frame F � one can express the total external in�uences on thebody as a force with line of action passing through the origin of F � and a torque�

De�nition � For a rigid body with attached frame F � let the total external inuences on

the body be given by a force f with line of action passing through the origin of F � and a

torque � � Then the spatial force acting on the body� expressed in frame F is

�f �

�� f

�

�� Let G be a second frame attached to the rigid body� and let r be the o�set vector

from the origin of F to the origin of G� The force acting through the origin of F may be

��

displaced to act through the origin of G� if a correction torque of �r� f is added� the torque� requires no adjustment �see �MK�� for details�� Therefore�

�fG �

�� f

� G

�� f

�F � r� f

��

�"r �

�� f

�F

�� The parallel to �� is obvious� the same spatial transformation matrix G

�XF that trans�

forms spatial accelerations from frame F to frame G� also transforms spatial forces fromframe F to frame G�

De�nition � Let

�x �

�� a

b

��be the coordinate representation of a spatial vector� with a�b � R� � The spatial transpose

of �x� denoted by �x�� is

�x� �hbT �aT

i�

The spatial inner product of two spatial vectors �x and �y is given by �x��y�

To transpose a spatial vector� one swaps its two halves and transposes each of them� this

is typically what one wants� For example� if �v is the spatial velocity of a body� and �f is a

spatial force applied to the body� the inner product of spatial force and spatial velocity is

�f��v �

�� f

�

��

v

�� h� T � fT

i ��

v

�� f � v�

which is power� as with ordinary �D vectors�

�� Spatial formulation of acceleration propagation

The acceleration propagation equations developed in Section �� are compactly

expressed with spatial algebra� If joint i is prismatic� equations ��$�� can be written�� i

ai

�� i��

�ri ��i�� ai��

�� qi��

ui

��

�i�� i�� ri� � �i�� i

��

��

If joint i is revolute� equations ��$�� can be written�� i

ai

��

�� i��

�ri ��i�� ai��

�� qi�� ui

ui � di

��

�� i�� i

�i�� i�� ri� � �i�� i � di� � �i � ��i � di�

�� Equations �� and ��are uni�ed with the help of the following de�nitions�

De�nition � The spatial joint axis of joint i is the spatial vector

�si �

��

ui

��if the joint is prismatic� and the spatial vector

�si �

�� ui

ui � di

��if the joint is revolute�

De�nition � The spatial Coriolis force of link i is the spatial vector

�ci �

��

�i�� i�� ri� � �i�� i

��if the inboard joint i is prismatic� and the spatial vector

�ci �

�� i�� i

�i�� i�� ri� � �i�� i � di� � �i � ��i � di�

��if the inboard joint i is revolute�

The �rst vector on the right hand sides of �� and �� is just the spatial

acceleration of link i� � transformed to frame Fi� Using the shorthand j�Xi for Fj

�XFi� this

vector can be written i�Xi��ai�� Thus� �� and �� can both be written as

�ai � i�Xi��ai�� qi�si � �ci� ��

This compact result encapsulates the acceleration propagation equations derived in Sec�

tion �� One advantage of the spatial form is that prismatic and revolute joints are put

��

into a common framework� di�erences only appear in the underlying expressions for �si and

�ci� By the established convention� vectors subscripted i are expressed in frame Fi� Hence�

�si is constant and may be precomputed� The vector �ci comprises Coriolis and centripetal

acceleration terms� It involves velocities of link i�� which must be transformed into frameFi� and velocities of link i� It depends on the positions and velocities of joints � through i�

but not on any joint accelerations�

�� The Featherstone algorithm

The Featherstone algorithm is a method of solving the forward dynamics problem

in O�n� time for an n link manipulator� It is one of a family of methods called structurally

recursive algorithms� These methods are alternatives to the O�n�� algorithms that explicitly

build the mass matrix for the system and must invert it to solve for joint accelerations

�WO��

De�nition �� Articulated body of a serial linkage� For a serial linkage with n links�

sever the linkage at joint i� and consider the connected subchain comprising links i� � � � � n

in isolation� detached from the �xed base� This subchain is called an articulated body

of the original linkage� and link i is called the handle of the articulated body� The trivial

articulated body with link n as a handle is simply the isolated rigid body of link n�

Featherstone�s algorithm works by considering successive articulated bodies of the

original linkage� beginning with the trivial case of link n alone� and adding inboard links

one by one� The key idea is to relate the spatial acceleration of the handle to the spatial

force applied at its inboard joint� The links outboard to the handle will have an e�ect on

this relation�

De�nition �� Per link spatial vectors� The spatial acceleration of link i is denoted

�ai� the spatial force exerted on link i through its inboard joint is �fI

i � and the spatial force

exerted on link i through its outboard joint is �fO

i � The coordinates of all of these spatial

vectors are expressed in frame Fi�

Even though the inboard in�uences on link i are exerted at the joint i� the spatial force �fI

i

is given by �f Ii � �Ii �T � where fIi and �

Ii are the equivalent force and torque applied at the

��

origin of Fi� the line of action of fIi passes through the origin of Fi� The same is true for

the outboard spatial force� fOi �

Theorem �� Serial linkage articulated motion� Consider the articulated body of a

serial linkage that has link i as a handle �� i � n�� There exists a spatial matrix �IA

i � and

a spatial vector �ZA

i such that

�fI

i � �IA

i �ai � �ZA

i � ��

�IA

i is called the spatial articulated inertia of link i� and is independent of the joint

velocities and accelerations� �ZA

i is called the spatial articulated zero�acceleration za�

force of link i� and is independent of the joint accelerations�

The adjective articulated indicates that the entire subchain beginning at the handle

is being considered� as opposed to the handle in isolation� �ZA

i is so named since by �� it

is the force which must be exerted by the inboard joint on link i� if link i is not to accelerate

��ai � �� The proof of Theorem �� is involved� but constructive� an algorithm will follow

directly from the proof� The proof is by induction on i� beginning with the trivial subchain

comprising only the outermost link� and then adding links on the inboard side� one by one�

until the entire linkage is considered�

�� Base case

For the base case of the induction� i � n� and the subchain being considered is

simply the last link of the chain� as shown in Figure �� This free body diagram depicts

all of the forces and torques acting on the link� those applied by the inboard joint �but

resolved at the origin of Fn�� and those due to external in�uences like gravity� The motion

of the body is given by the Newton�Euler equations described in Appendix A�� In link

coordinates�

f In �mng � mnan

� In � In�n ��n � In�n�

where mn is the mass of the link n� and In is the inertia tensor in link coordinates� These

equations can be written as a single spatial equation� If

Mi � mi��

�Featherstone calls these spatial articulated z�a� forces bias forces� and denotes them �Pi�

��

inboardjoint

f In

τIn

gmn

link n

Figure �� The free body diagram of the last link of the serial linkage� It is the handle of atrivial subchain�

then �� fIn

� In

�� Mn

In �

�� n

an

�� mng

�n � In�n

�� or more compactly�

�fn � �IA

n�an � �ZA

n �

Note that �ZA

n is independent of the acceleration of link n� and therefore independent of the

joint accelerations� �IA

n is independent of the joint velocities and accelerations� In fact� it

depends only on the mass properties of link n and may be precomputed� Thus Theorem ��

holds for the base case i � n� Two expressions above will be useful later� and are given

special names�

De�nition �� The spatial isolated inertia of link i is de�ned as the spatial matrix

�Ii �

�� Mi

Ii

��

��

The spatial isolated zero�acceleration za� force of link i is de�ned as the spatial

vector

�Zi �

�� mig

�i � Ii�i

�� These two quantities are similar to their articulated counterparts� �I

A

i and �ZA

i � except that

�Ii and �Zi apply to link i in isolation� with no outboard links attached� For the special case

of i � n� the de�nitions coincide� �IA

i � �Ii and �ZA

i � �Zi�

�� Inductive case

Assume Theorem �� is true for i� and consider link i�� Figure �� In addition

inboardjoint

outboardjoint

O

link i−1

f Ii−1

fOi−1 τO

i−1

gmi−1

τIi−1

Figure �� The free body diagram of an inner link of the serial linkage� It is the handle ofa non�trivial subchain�

to gravity� and the force and torque applied through the inboard joint� there are also the

force and torque applied through the outboard joint� fOi�� and �Oi�� As before� all forces

��

and torques are resolved at the origin of the link frame� Again using the Newton�Euler

equations� the equations of motion of link i� � are given by�� f Ii��

� Ii��

�� fOi��

�Oi��

�� Mi��

Ii��

�� i��

ai��

�� mi��g

�i�� Ii��i��

�� Using spatial notation and rearranging yields

�fI

i�� Ii��ai�� Zi�� fOi��

The force �fO

i�� is the equal but opposite reaction force to �fI

i � Hence�

�fO

i�� i��Xi�fI

i �

Combining the above two equations�

�fI

i�� Ii��ai�� Zi�� i��Xi�fI

i �

Invoking the inductive hypothesis �� for link i gives

�fI

i�� Ii��ai�� Zi�� i��Xi��I

A

i �ai � �ZA

i ��

To put this equation in the form of �� the acceleration �ai must be eliminated from

the right hand side� This is done by expressing �ai in terms of �ai�� using the acceleration

propagation equation� Substituting �� into the above equation and rearranging gives

�fI

i�� Ii�� i��

�Xi�IA

i i�Xi��

��ai�� Zi�� i��

�Xi

h�ZA

i ��IA

i �ci ��IA

i �si��qii� ��

Now �qi must be eliminated from the right hand side� The following lemma will help�

Lemma � At every joint�

Qi � �s�i�fI

i �

where Qi is the magnitude of the force exerted by the prismatic joint actuator� or the mag�

nitude of the torque exerted by the revolute joint actuator�

Proof� Suppose a force f and a torque � are applied to link i at the inboard joint� These

give rise to a spatial inboard force �resolved in the body frame� of

�fI

i �

�� f

� � di � f

��

��

If joint i is prismatic�

�s�i�fI

i �

��

ui

�� f

� � di � f

�� f � ui�The right hand side is the component of the applied force along the joint axis� This force

must be supported by the actuator� hence� it is Qi� If joint i is revolute�

�s�i�fI

i �

�� ui

u� di

�� f

� � di � f

�� f � �ui � di� � �� di � f� � ui�

The right hand side reduces to � � ui� the component of the applied torque along the jointaxis� This torque must be supported by the actuator� hence� it is Qi� �

Substituting equation �� for link i�s spatial acceleration into �� yields

�fI

i � �IA

i �i �Xi��ai�� qi�si � �ci� � �ZA

i �

Premultiplying both sides by �s�i and applying Lemma � gives

Qi � �s�i�IA

i �i �Xi��ai�� qi�si � �ci� ��s�i�ZA

i �

from which �qi may be determined�

�qi �Qi ��s�i�I

A

i i�Xi��ai�� s�i

�ZA

i ��IA

i �ci�

�s�i�IA

i �si� ��

Substituting this expression for �qi into �� and rearranging gives

�fI

i��

��Ii�� i��Xi

��IAi � �IA

i �si�s�i�IA

i

�s�i�IA

i �si

�Ai�Xi��

��ai�� Zi�� i��

�Xi

��ZA

i ��IA

i �ci ��IA

i �sihQi ��s�i

�ZA

i ��IA

i �ci�i

�s�i�IA

i �si

�� Comparing this to the desired form ��

�IA

i�� Ii�� i��Xi

��IAi � �IA


i

�s�i�IA

i �si

�Ai�Xi��

�ZA

i�� Zi�� i��Xi

��ZA

i ��IA

i �ci ��IA

i �sihQi ��s�i

�ZA

i ��IA

i �ci�i

�s�i�IA

i �si

��

��

These equations compute the articulated inertia and articulated z�a� force for link i� � ofthe chain� given those same quantities for link i� �Ii�� and �si are constants� i�� Xi and i

�Xi��

depend only on joint positions� and �ci depends only on joint positions and velocities� It

follows that if �IA

i is independent of joint velocities and accelerations� then so is �IA

i�� Also�

if �ZA

i is independent of joint accelerations� then so is �ZA

i�� Thus� if Theorem �� is true for

a handle at link i� it is also true for a handle at link i � �� By induction� the theorem istrue for a handle at any link� �

�� Forward dynamics algorithm

The proof of Theorem �� gives an algorithm for computing forward dynamics of

a serial linkage �Problem �� The algorithm comprises four main steps�

�� Iterating from the base to the tip of the linkage� compute the velocities of all links

�algorithm compSerialLinkVelocities� Figure ��

� Initialize each link�s articulated inertia and articulated z�a� force to their isolated coun�

terparts� Also� compute the Coriolis vector for each link �algorithm initSerialLinks�

Figure ��

� Iterating from the tip to the base of the linkage� compute the articulated inertia and

articulated z�a� forces of each link �algorithm serialFwdDynamics� Figure ��

�� Iterating from the base to the tip of the linkage� compute the acceleration of each joint�

and the spatial acceleration of each link �algorithm serialFwdDynamics� Figure ��

The common subexpressions �s�i�IA

i �si and Qi��s�i�ZA

i ��IA

i �ci�should only be computed once

per call�

�� Extension to tree�like linkages

The previous sections solve the forward dynamics of serial linkages� such as the one

in Figure �� The results can be extended to encompass a more general class of kinematic

structures� tree�shaped linkages� Consider the complex pendulum shown on the left side of

Figure �� This is not a serial linkage� but it does have a tree�like topology� Attached

��

initSerialLinks

for i � � to n

�ZA

i �� mig

�i � Ii�i

��IA

i �� Mi

Ii �

��if joint i is prismatic �ci

��

�i�� i�� ri� � �i�� i

��else �ci

�� i�� i

�i�� i�� ri� � �i�� i � di� � �i � ��i � di�

��

Figure �� initSerialLinks� Initialize the articulated spatial inertias and articulatedz�a� forces to their isolated counterparts� for all links in a serial linkage� Also compute thespatial Coriolis vector for each link�

to each link i is a unique link h that is closer to the �xed base link� Letting h be the

parent of i� one can construct a tree representing the linkage� as shown on the right side

of Figure �� Each link becomes a node of the tree� and each joint becomes an edge� The

links are numbered using the depth��rst recursion shown in Figure ��

De�nition �� Tree linkage indices� Let the links of a tree linkage be indexed using the

algorithm of Figure �� and assign each joint the index of the unique link outboard to it�

Index the inertial frame as link �� Then the following hold

� The n moving links of the structure and the n joints are numbered � � � � n�

�� For any joint i� the link outboard to the joint �the child link� is indexed i� and the link

inboard to the joint �the parent link� is indexed h� with h � i�

The forward dynamic algorithm for serial linkages was studied in terms of links�

Iterations were performed by visiting each link in tip to base or base to tip order� The

algorithm can also be viewed as enumerating over joints� since in a serial linkage there is

exactly one joint between every two links� and vice�versa� In studying tree linkages� there

��

serialFwdDynamics

call compSerialLinkVelocities �� compute all link velocities ��

call initSerialLinks �� initialize �IA

i ��ZA

i � and �ci for all links ��

for i � n downto

�IA

i�� IA

i�� i��Xi

��IAi � �IA


i

�s�i�IA

i �si

��i�Xi��

�ZA

i�� ZA

i�� i��Xi

��ZAi ��IAi �ci � �IAi �si�Qi ��s�i��ZA

i ��IA

i �ci��

�s�i�IA

i �si

��a� �

for i � � to n


A

i i�Xi��ai�� s�i��Z

A

i ��IA

i �ci�

�s�i�IA

i �si

�ai � i�Xi��ai�� ci � �qi�si

Figure �� serialFwdDynamics� Compute the accelerations of joints �� n of a se�rial linkage� Input data are the joint positions qi� joint velocities #qi� and joint actuatorforces%torques Qi� output data are the joint accelerations �qi �i � �� n��

is an advantage to the joint�centric viewpoint� For these� there is not always a unique

choice for the next link in a base�to�tip recursion� nor is there a unique starting link for

a tip to base recursion� But every joint still has exactly one inboard link and exactly one

outboard link� By describing the algorithms in terms of computations to be done at each

joint� and visiting the joints in a particular order� the serial linkage algorithm generalizes

to tree linkages� Using the joint indexing scheme described above� the algorithm visits the

joints in increasing or decreasing order� Pointers to the unique inboard and outboard links

to each joint should be stored with the joint�

��

2

0

3

45

6

7

8 9

1011

12

1

1

2

3 8 9

10

11 12

4

5 6

7

1

2

4

9

Figure �� Left� A complex pendulum with a tree�like topology� The moving links arenumbered through �� as are the joints� Four of the joint indices are indicated with circlednumbers� Right� The corresponding tree representing the kinematic structure�

int numberLinks�tree� idx�

r root link of tree

r�idx idx

idx idx � �

for each child c of r

idx � numberLinks�c� idx�

return idx

Figure �� int numberLinks�tree� idx�� Recursively number the links of a kinematictree� so that every link has higher index than its parent� The routine returns the integerone greater than the highest index in the tree� The routine should initially be called withthe entire original tree and idx � ��

�� Velocity and acceleration propagation through trees

The �rst step of the forward dynamics algorithm is to compute the absolute ve�

locities of all the links in the structure� This was done in Section �� for serial linkages�

Let h be the index of the parent of link i� A little re�ection on Figure �� reveals that

the absolute velocity of link i may be determined from the absolute velocity of its parent

link h� and the velocity of joint i� Thus� the velocity propagation equations ��$��

��

and ��$�� still hold� with the subscript i� � replaced by h�The order in which the velocities are propagated through links must be chosen so

that the velocity of a link is always computed before it is needed� This is accomplished by

iterating over the joints in increasing order� and at each joint propagating the velocity from

its inboard link to its outboard link� The algorithm is shown in Figure �� The same

quantities may be precomputed as in the serial linkage algorithm�

compTreeLinkVelocities

��v��a�

for i � � to n

h index of link inboard to joint i

�� h � index of parent link� i � index of joint child link ��

R rotation matrix from Fh to Fi

r radius vector from Fh to Fi �in Fi coordinates�

�i R�h

vi Rvh � �i � r

if joint i is prismatic

vi vi � #qiui

else �� joint i is revolute ��

�i �i � #qiui

vi vi � #qi�ui � di�

Figure �� compTreeLinkVelocities� Compute the absolute linear and angular velocitiesof all links of a tree linkage� in link coordinates� Input data are the dynamic state �q� #q��Output data are the velocities �i and vi for each link�

Propagating accelerations through tree linkages is also analogous to the case for

serial linkages� If link h is the parent of link i� the acceleration of link i can be determined

from the positions� velocities� and accelerations of link h and joint i� The derivation for

��

acceleration propagation is identical to that for the serial linkage case� the end result is �c�f�

Equation ��

�ai � i�Xh�ah � �qi�si � �ci� ��

�� Articulated inertias and z�a� forces for tree linkages

The next step of the forward dynamics algorithm is to compute the articulated

inertias and z�a� forces for all links� For serial linkages� this is done using a tip to base

recursion� For a tree linkage� articulated inertias and z�a� forces may be computed directly

for links that are the leaves of the tree� for an inner link� the quantities are computed by

combining the quantities from the link�s children�

De�nition �� Articulated body of a tree linkage� For a tree linkage� break the tree

at joint i� and consider the connected subtree rooted at link i in isolation� detached from

the �xed base� This subtree is an articulated body of the original tree� and link i is the

handle of the articulated body� If link i is a leaf of the tree� the articulated body is trivial�

and is simply the the isolated rigid body of link i�

The central result to be proved is the counterpart to Theorem ��

Theorem �� Tree linkage articulated motion� Consider the articulated body of a tree

linkage that has link h as a handle �� h � n�� There exists a spatial matrix �IA

h � and a

spatial vector �ZA

h such that

�fI

h � �IA

h�ah � �ZA

h � ��

Furthermore� �IA

h is independent of the joint velocities and accelerations� and �ZA

h is indepen�

dent of the joint accelerations�

Theorem �� will be proved by induction on the height of the articulated body �the maximum

number of nodes along a simple path from the handle to some leaf of the subtree�� For the

base case� consider the height to be �� Then the articulated body is trivial� it is simply an

isolated rigid body that is a leaf of the original linkage� There is no di�erence between this

case and the case of the isolated tip link of a serial linkage� Thus the proof of Section ��

goes through unchanged for this base case�

For the inductive case� consider the more general articulated body handle shown

in Figure �� The handle �link h� may have several links outboard to it� these links are

��

fO1 τO1,

fO2 τO2, gm

joint h

link hh h

h h h

fO τO,h h

m m

joint ijoint i

joint i12

m

f τ,h hI I

Figure �� The free body diagram of a non�trivial handle of an articulated body from atree linkage�

indexed i�� i�� im� The in�uences applied to link h come from gravity� as well as forces

and torques transmitted through the one inboard and m outboard joints� Resolving these

in�uences at the origin of frame Fh� the equations of motion of link h are given by the

Newton�Euler equations�� f Ih

� Ih

�� mXj��

�� fOj

h

�Oj

h

�� Mh

Ih

�� h

ah

�� mhg

�h � Ih�h

�� where the superscript Oj denotes forces or torques imparted through the outboard joint ij �

Using spatial notation and rearranging yields

�fI

h � �Ih�ah � �Zh �mXj��

�fOj

h �

Since �fOj

h is the reaction force to �fI

ij�

�fOj

h � � h�Xij

�fI

ij�

Combining the above two equations�

�fI


h�Xij

�fI

ij�

Each of the child links ij is the handle of a subtree of smaller height than the subtree with

link h as a handle� The inductive hypothesis �� may be invoked for these child links�

��

giving

�fI


h�Xij �

�IA

ij�aij �

�ZA

ij��

Using the acceleration propagation equation �� to express �aij in terms of �ah� and rear�

ranging gives

�fI

h �

��Ih � mXj��

h�Xij

�IA

ij ij�Xh

�A�ah � �Zh �mXj��

h�Xij

h�ZA

ij��I

A

ij�cij �

�IA

ij�sij

��qij

i�

The derivation of joint accelerations �see Lemma � and the subsequent discussion� makes

no assumptions about the topology of the linkage� it applies equally well to tree linkages�

The main result �� stated in the current notation is�

�qij �Qij ��s�ij�I

A

ij ij�Xh�ah ��s�ij

�ZA

ij��I

A

ij�cij

��s�ij�IA

ij�sij

� ��

Combining the above two equations and rearranging gives

�fI

h �

��Ih � mXj��

h�Xij

��IAij � �IA

ij�sij�s

�ij�IA

ij

�s�ij�IA

ij�sij

�Aij�Xh

��ah� �Zh �

mXj��

h�Xij

��ZA

ij��I

A

ij�cij �

�IA

ij�sij

hQij ��s�ij

�ZA

ij��I

A

ij�cij

�i�s�ij�IA

ij�sij

�� Comparing this to the desired form ��

�IA

h � �Ih �mXj��

h�Xij

��IAij � �IA

ij�sij�s

�ij�IA

ij

�s�ij�IA

ij�sij

�Aij�Xh ��

�ZA

h � �Zh �mXj��

h�Xij

��ZA

ij��I

A

ij�cij �

�IA

ij�sij

hQij ��s�ij

�ZA

ij��I

A

ij�cij

�i�s�ij�IA

ij�sij

��

These equations compute the articulated inertia and articulated z�a� forces for link h of the

linkage� given those same quantities for its children links i�� im� If the �IA

ijare indepen�

dent of joint velocities and accelerations� than so is �IA

h � If the �ZA

ijare independent of joint

accelerations� then so is �ZA

h � By induction on the height of the subtrees of the handles�

Theorem �� is true for a handle at any link� �

��

�� Forward dynamics algorithm for tree linkages

At �rst glance� the recursive equations ��$�� seem to imply a more complex

computation for tree linkages than for serial ones� multiple articulated inertias and z�a�

forces of child links must be combined to compute the corresponding quantities for the

parent� But in fact� the same number of back propagations are performed� one for each joint�

the algorithm is stillO�n�� The only di�erence from the serial algorithm is the order in which

inertias� forces� and accelerations are propagated� The forward dynamics algorithm for tree

linkages is shown in Figure �� It iterates using a joint centric approach� The articulated

inertia and z�a� force of a given link h are accumulated over several iterations� as the

contributions from all of link h�s children are propagated back� Link h�s articulated inertia

and z�a� force will reach their �nal values before they are propagated back to h�s parent�

When propagating accelerations� link h�s acceleration is known before the accelerations of

its children are computed� These claims follow from the joint indexing scheme described in

De�nition �� The algorithm comprises four steps�

�� Iterating over the joints in increasing order of index� compute the spatial velocities of

all links �algorithm compTreeLinkVelocities� Figure ��

� Initialize each link�s articulated inertia and articulated z�a� force to their isolated coun�

terparts� Also� compute the Coriolis vector for each link �algorithm initTreeLinks�

Figure ��

� Iterating over the joints in decreasing order of index� back propagate the articulated

inertia and z�a� forces from the link outboard to the joint to the link inboard to the

joint �algorithm treeFwdDynamics� Figure ��

�� Iterating over the joints in increasing order of index� compute the acceleration of

each joint� and the spatial acceleration of the link outboard to the joint �algorithm

treeFwdDynamics� Figure ��

�� Extension to �oating linkages

The algorithms discussed thus far assume grounded linkages� link � is attached

to the inertial frame through a one degree�of�freedom joint� A oating linkage is one in

��

initTreeLinks

for i � � to n

�ZA

i �� mig

�i � Ii�i

��IA

i �� Mi

Ii �

��h index of link inboard to joint i �� link h is link is parent ��

if joint i is prismatic �ci ��

�h � ��h � ri� � �h � �i

��else �ci

�� h � �i

�h � ��h � ri� � �h � ��i � di� � �i � ��i � di�

��

Figure �� initTreeLinks� Initialize the articulated spatial inertias and articulated z�a�forces to their isolated counterparts� for all links in a tree linkage� Also compute the spatialCoriolis vector for each link�

which link � is not attached to the inertial frame� An example of such a system is a

mechanical bug� jointed legs are attached to a body �link �� which is free to move around

an environment� Dynamics of such systems are easily modeled with only slight changes to

the previous algorithms� These changes are discussed below for the tree linkage forward

dynamics algorithm� which is more general than the serial linkage one�

A minimal description of the dynamic state of an n�link �oating linkage comprises

the position� orientation� linear velocity� and angular velocity of the base link� plus the

positions and velocities of the joints�

�r��q�v�� #q��

Here� r� is the position of the origin of body frame F�� and �� is some parameterization of

the orientation of this frame� both of these are relative to some �xed frame O� The jointposition vector q is now only �n��dimensional� The vectors v� and �� are the linear and

angular velocities of frame F�� and #q is the �n��dimensional joint velocity vector� If unitquaternions �see Appendix A�� are used to parameterize the orientation of the base link� the

��

treeFwdDynamics

call compTreeLinkVelocities �� compute all link velocities ��

call initTreeLinks �� initialize �IA

i ��ZA

i � and �ci for all links ��

for i � n downto

h index of link inboard to joint i �� link i is outboard ��

�IA

h �IA

h � h�Xi

��IAi � �IA


i

�s�i�IA

i �si

��i�Xh

�ZA

h �ZA

h � h�Xi

��ZA

i ��IA

i �ci ��IA

i �si�Qi ��s�i��ZA

i ��IA

i �ci��

�s�i�IA

i �si

��a� �

for i � � to n



A

i i�Xh�ah ��s�i��Z

A

i ��IA

i �ci�

�s�i�IA

i �si

�ai � i�Xh�ah � �ci � �qi�si

Figure �� treeFwdDynamics� Compute the accelerations of joints �� n of a tree link�age� Input data are the joint positions qi� joint velocities #qi� and joint actuator forces%torquesQi� output data are the joint accelerations �qi �i � �� n��

total dynamic state of an n�link �oating linkage has n�� parameters� as opposed to the n

parameters of a grounded linkage� For a grounded linkage� the forward dynamics algorithm

uses the dynamic state to compute �q� the n�dimensional joint acceleration vector� For a

�oating linkage� the algorithm must compute a� and �� the linear and angular accelerations

of frame F�� plus �q� the �n� ��dimensional joint acceleration vector�Changes to the velocity propagation algorithm to handle a �oating linkage are

minimal� The strategy for computing link velocities is to calculate vi and �i from vh��h�

and #qi� where link h is the parent of link i� For grounded linkages� the inertial frame serves

as the parent of link �� and v� � �� starts the iteration� For �oating linkages� the

�

velocities v� and �� are already part of the dynamic state vector� and so do not need to be

computed� The iteration simply starts at joint � and proceeds to joint n� at which point

the velocities of all the other links will be known�

The calculation of the articulated inertias and articulated z�a� forces for the links

needs no adjustment for �oating linkages� Featherstone�s algorithm always considers artic�

ulated bodies� which are collections of connected links that are detached from the inertial

frame� Note that in the loop which computes articulated inertias and z�a� forces in Fig�

ure �� information about joint �� the connection between link � and the inertial frame�

is never used�

In the second loop� accelerations are propagated outward toward the leaf links of

the tree� For grounded linkages� the algorithm begins by setting �a� to �� and then computes�

�q�� a�� q�� a�� and so on� For a �oating linkage� joint � doesn�t exist� and the iteration must

be bootstrapped in a di�erent manner� The solution comes from Equation �� applied to

link ��

�fI

� � �IA

� �a� � �ZA

� �

Since link � is disconnected from the inertial frame� the spatial force applied through the

inboard joint must be �� Thus�

�a� � ��IA

�

��ZA

� �

As long as the individual links all have non�singular mass matrices� which is the case for

all real objects� �IA

� is invertible �Fea�� Once the spatial acceleration �a� is determined�

the acceleration propagation proceeds exactly as in the grounded case� The second loop

continues� beginning with joint � The algorithm remains O�n� since the inversion of the

�� IA� takes constant time�

�

Chapter �

Hybrid Simulation

While there is a large range of contact interactions that can be modeled via trains of

impulses� there are also limitations of this approach� Constraints are the natural vehicle for

describing many types of contact interactions� This chapter introduces hybrid simulation�

a method of combining the impulse� and constraint�based simulation paradigms� Impulses

are used to transmit unilateral contact forces that arise between separate bodies� while

constraints are used to model forces which can push or pull� such as those occurring in a

joint bearing� Each method is applied to the contact it is most adept at handling� One

main contribution of this chapter is a method for computing lower bounds on the times of

impact between rigid bodies that are part of linked structures� Unlike the case for isolated

rigid bodies� the initial bounds are sometimes violated� however these violations can always

be detected� The collision detection algorithms of Chapter can thus be extended to handle

linked bodies� A second major contribution is a di�erential approach for computing collision

impulses arising between rigid bodies that are part of linked structures� This extension of

the results presented in Chapter depends on computing a � multibody collision matrixthat encapsulates the collision dynamics at the contact point� An algorithm for computing

this matrix is given that runs in O�n�m� time� where n and m are the number of links in

the two colliding linked structures� Finally� the chapter describes how a multilevel control

architecture can be easily added to an impulse�based simulator� using the one in Impulse

as an example�

��

�� A spectrum of physical systems

Consider a simple hinge joint �Figure �� In principle� the joint could be

Figure �� In practice� it is not feasible to model the contact interactions at a hinge jointusing an impulse�based approach�

modeled in an impulse�based way� enforcing the hinge constraint through collisions between

the hinge pin and sheath� Due to the enormous amount of collision processing that would

be necessary� however� impulse�based simulation would be far too slow� Constraint�based

methods should be used instead�

Figure � depicts a spectrum of physical systems� On the right end are jointed

manipulators� a class of systems for which constraint�based simulation approaches are clearly

appropriate� The motion of these systems of rigid links is governed by bilateral �equality�

constraints imposed at the joints� Techniques to handle bilateral constraints are more de�

veloped and robust than those for unilateral �inequality� constraints� The joint constraints

are also permanent� facilitating e�cient constraint�based simulation� Computing the for�

ward dynamics of these systems is a classical problem of robotics� and a variety of methods

exist for doing so� The interaction between the manipulator and the environment �typically

occurring at the end e�ector� is not so clearly a constraint�based interaction� it is often

transient and only expressible through unilateral constraints� Over limited intervals of mo�

tion� however� the contacts can be modeled through bilateral constraints� and closed�loop

methods exist for computing the resulting manipulator dynamics �Lil��

Slightly to the left on the spectrum lie mechanisms� a class of systems which

�

part feeders interactiveenvironments mechanisms jointed

manipulatorsbilliards &

bowling

impulse−based domain constraint−based domain

colliding contact constrained contact

Figure �� A spectrum of physical systems�

are also highly constrained� Unilateral constraints are more common in this case� The

constraints are also more transient� topologically distinct contact con�gurations may occur

during a complete cycle of the mechanism� Nonetheless� there is still a fair amount of contact

coherence� and constraint�based approaches can therefore work well� Bara��s blockfeeder

and double�action jack are good examples of constrained�based simulation of D mechanisms

�Bar��

On the other end of the spectrum are highly unconstrained� collision intensive

systems like part feeders� In these types of systems� the almost complete lack of contact

coherence makes constraint�based methods di�cult to apply� Modeling frictional collisions

accurately is crucial to correct simulation� For these applications� impulse�based simulation

is the natural choice� Slightly to the right on the spectrum lie systems like a bowling alley or

a billiards table� In these systems� collisions still occur frequently� and accurate modeling of

them is important� Some prolonged contact does occur� but contact coherence is still fairly

low� particularly in critical intervals of the simulation� such as when the bowling ball strikes

the pins� Impulse�based simulation can adeptly handle these types of systems� including

the rolling and sliding contacts that occur�

From a dynamic simulation standpoint� the middle ground of the spectrum has

been the least studied� This is probably because no existing simulation paradigms are

��

a natural �t for the interesting applications that lie there� Consider simulating a user�

controlled agent within an interactive virtual environment� A simulator must respond to

erratic� unpredictable behavior on the part of the user�controlled agent or other autonomous

agents in the environment� Collisions provide much of the interaction between agents and

their environment� The agents may bump into each other or other objects� and may move

or throw objects around� Contact modes are transient as agents grab� push� or pull objects�

Settling phases� as objects come to rest after being positioned or dropped by agents� involve

more transient contact� There are also many types of contact best modeled with constraints�

Hinged doors between rooms� simple mechanical structures like levers� and the joints of the

agents all entail permanent� bilateral constraints� There may be stacks of objects� or other

situations where contact coherence is high and constrained�based methods can e�ectively

model the non�penetration between bodies�

An interactive virtual environment is just one example of an application that seems

to require both impulse�based and constraint�based techniques for e�cient simulation� ways

are needed to blend the approaches� This chapter discusses hybrid simulation� which is a

�rst step in this direction� Hybrid simulation extends the pure impulse�based methods

discussed thus far to accommodate permanent� bilateral constraints� Thus systems of rigid

bodies connected by joints may be handled� Adjustments to the collision detection scheme

are required� Also� the calculation of collision impulses must account for the dynamics of

the bodies that are linked to the colliding bodies� A connection to the simple rigid body

collision response algorithm is made through the collision matrix K� much of the the theory

discussed in Chapter then carries over to the case of linked bodies� Finally� once the

impulses are computed� they must be propagated through the linked systems of bodies�

instantaneously changing the velocities of all bodies in the linkage�

Collections of linked rigid bodies are called multibodies� In this chapter� multi�

bodies are restricted to the following de�nition�

De�nition �� A multibody system� or multibody� is a collection of rigid bodies linked

together through one degree of freedom revolute or prismatic joints� with tree�like topology�

That is� the system is a single connected piece with no loops among the links� One link is

speci�ed as the base link� and this link may be connected to an inertial frame via a revolute

or prismatic joint� or via a full six degree of freedom joint� resulting in a �oating multibody�

The restrictions imposed by this de�nition are not essential� but facilitate use of the forward

��

dynamics algorithms discussed in the last chapter� There are extensions of Featherstone�s

algorithm for more general joints �Lil�� JR�� There are also methods of computing linkage

dynamics in the presence of kinematic loops �Lil�� LNPE�� dJB�� Bar�� Multibody

impulses could still be computed in cases of these more general systems� the construction of

the collision matrix K would involve using more the general forward dynamics algorithms

instead of those presented in the last chapter�

�� Collision detection

For collision detection purposes� a multibody is broken into its component rigid

bodies� When a body passes in the vicinity of a multibody� the narrow phase collision

detection system will track closest points between the body and the nearby multibody links�

Links that are not close to the body will be discarded from consideration by the broad

phase detection� The collision detection system can be extended to handle multibodies

using the two interface routines described in Section � � a swept volume routine and a TOI

coe�cients routine�

The motion of a rigid body that is part of a multibody is much more complex that

that of a rigid body or scripted body� Conservative bounds on swept volumes and TOIs are

di�cult to derive� and the requirement that these bounds be conservative is abandoned for

multibodies� Instead� Impulse makes aggressive predictions that are hopefully valid bounds

in the �average� case� If an aggressive bound is violated� this violation must be detected�

and the simulator must recover gracefully�

�� Constrained body swept volumes

Recall the goal of the swept volume routine� Given the state of a body at the

current time t�� and a time interval �t� the routine returns an axes�aligned bounding box

which encloses the body�s center of mass during the interval �t�� t��t�� For bodies moving

ballistically or along simple scripted trajectories� this can be done� For bodies which are

part of a multibody� it is very di�cult� But one can assume the coordinates of the center

of mass r will follow second�order trajectories�

rx�t� � rx�t�� #rx�t��t� t��

�rx�t��t� t��

�

ry�t� � ry�t�� #ry�t��t� t��

�ry�t��t� t��

�

��

rz�t� � rz�t�� #rz�t��t� t��

�rz�t��t� t��

��

The quantities r�t�� and #r�t�� are inferred from the current dynamic state of the multibody�

and �r�t�� is a by�product of the forward dynamics algorithm� it is just the linear acceleration

a of the link� These accelerations can be stored so that their current values are always

available�

Using these second�order approximations make it easy to predict the minimum

and maximum rx� ry� and rz values over the interval �t�� t� � �t�� It makes sense to pad

these values somewhat� because of the higher order terms which are neglected in the above

equations� this means returning a slightly larger bounding box than that predicted by the

model� The tradeo� is that as box size increases� so does the chance that more body pairs

will be promoted to the active collision heap� perhaps unnecessarily� The padding scheme

used in Impulse is to scale #r�t�� and �r�t�� by a factor �� before using them in the above

equations�

The position and velocity of a multibody is numerically integrated� using a deriva�

tive evaluation routine such as treeFwdDynamics �Chapter �� Each time the derivative

evaluation routine is called� the positions of the links are determined� The minimum and

maximum coordinate values of each link�s center of mass are tracked over the entire integra�

tion interval� at the end of the integration� it is easy to check if these coordinates remained

within the bounding box predicted by the swept volume routine�

When a violation does occur� the correct bounding box for the interval is known�

since the minimum and maximum center of mass coordinate values are known� The bound�

ing box in the spatial hash table is updated to re�ect this new information� If the new box

intersects no other boxes than the old box did� then no body pairs should be added to the

collision heap� and the integration is still valid� If new possible collision pairs are discovered�

then the times to impact between these new pairs are computed� and the pairs are added

to the collision heap� If this does not cause the body pair at the top of the heap to change�

the integration is still valid� Otherwise� a new collision check� which was previously missed

because of the incorrect bounding box� has been discovered� and this collision check should

have been performed earlier than the ending time of the last integration� In this case�

the simulation must be backed up� The bodies� states are restored to their values at the

beginning of the last integration� and the integration is redone� this time stopping in time

��

for the earlier collision check� After this second integration� there will be no bounding box

violations� since all of the invalid a priori boxes have been replaced by the valid a posteri

ones� Figure � details the entire process�

predictbounding box

bounding box inhash table

update

was predictedbox violated?

have new possiblecollision pairsbeen detected?

y

n

integrate dynamics,tracking actualbounding box

y

y

n

n

recompute TOIsfor new pairs

has soonestcollision time

changed?back up simulationcontinue

Figure �� The checking performed to detect aggressive bounding box violations� and todetermine if the simulation should be backed up as a result of the violation�

�� Constrained body TOI coe�cients

The narrow phase collision detection system requires a time of impact coe�cient

routine� given the dynamic state of a body at the current time t�� and directional vector �d�

return coe�cients A and B such that the distance that any point on the body travels along

the direction �d is bounded above by

A�t� t�� B�t� t��

Conservative values for A and B can be calculated for ballistic bodies and certain scripted

bodies� for constrained bodies� this remains an open problem� The approach taken in

Impulse is to abandon the requirement that the predictions be conservative� and attempt

to compute reasonable coe�cients which are usually valid� A second order model of motion

��

is again used� The coe�cients computed for constrained bodies are�

A ��

a�t�� d

B � v�t�� d� rmaxk��t��k

These are very similar to the coe�cients for ballistic rigid bodies� ��$�� One di�erence

is that the gravity vector g is replaced with the current acceleration of the body� a�t�� a

more general acceleration which also includes the e�ects of gravity� The max term is

replaced with k��t��k� the current angular velocity� since it is di�cult to compute a tightbound the angular velocity of a multibody link over some future time interval� Because of

the approximations made in this model� the coe�cients are padded by a safety factor� In

Impulse A and B are computed as above� and then increased by �� times their respective

absolute value� This reduces the chance that the coe�cients will be violated�

The strategy for detecting time of impact violations is similar to that for detecting

bounding box violations� the discrete data provided during numerical integration is tracked�

TOI predictions are monitored using slack structures� For each TOI estimate made for a

constrained body� there is a slack structure that tracks the progress of points on this body

in the direction �d� The components of a slack structure are

�� The unit vector �d� along whose direction motion of points of the body is monitored�

� The constants A and B� which bound the distance traveled by points on the body in

the direction �d�

� The base time tb at which the slack was initialized�

�� An upper bound � on the distance traveled by any point on the body in the direction

�d� since time tb� This is a truly conservative bound� based on values tracked during

dynamic integration�

� The time tl at which the slack bound � as last updated�

When a TOI prediction involving a constrained body B is made� a slack is initial�

ized� The coe�cients A and B are computed as described above� the base time tb and last

time tl are set to the current time� and the slack bound � is set to �� During integration� the

maximum and minimum values of each component of the body�s linear and angular velocity

are tracked� After the integration� maximum distance any point on the body could have

��

traveled along �d is computed� using the tracked integration values� this distance is added

to �� A check is made to insure

� � A�t� � tb�� B�t� � tb��

where t� is the current time� If this check fails� a TOI violation has occurred� The algorithm

to update the slack is shown in Figure ��

updateSlack

dt t� � tl

if �dt � �� return

vmin minimum components of linear velocity over last integration

vmax maximum components of linear velocity over last integration

�max maximum magnitude components of angular velocity

over last integration

k �if � &dx � �� k k � vminx

&dx� else k k � vmaxx&dx�

if � &dy � �� k k � vminy&dy� else k k � vmaxy

&dy�

if � &dz � �� k k � vminz&dz� else k k � vmaxz

&dz�

k k � rmax �

�maxx

q�� &d�x � maxy

q�� &d�y � maxz

q�� &d�z

�� k dt

if �� A�t� � tb�� B�t� � tb�� signal violation

else tl t�

Figure �� updateSlack� Update a slack structure and signal a TOI violation if necessary�The variables tl� �� A� B� and tb are all �elds of the slack structure� The variable t� is thecurrent simulation time�

When a violation is detected� the o�ending TOI is recomputed� using the now

known values for its motion during the integration step� The simulation is backed up to the

�

state at the beginning of the integration interval in which the violation occurred� and the

integration is redone� In this way� the collision detection system avoids missing collisions

do to overly aggressive TOI coe�cients�

�� Collision response

The algorithms developed in Chapter � address the dynamics of multibodies in

isolation� but say nothing about their dynamics when they come into contact with other

bodies� Contact forces can be added to the model if one has a way to compute them�

Impulse�based simulation adopts an alternative approach� all contacts are modeled through

trains of collision impulses� Chapter described how to compute these impulses for the

case of rigid body collisions� this section handles the case of multibody collisions�

Problem � Multibody collision response� Given two colliding rigid bodies� each of

which may be part of a larger multibody� compute the impulse pair to be applied at the two

contact points� and the instantaneous changes in velocity of the colliding bodies� as well as

any changes in the velocity of other links of the multibodies�

The response of a multibody to an impulse is simpler than its response to an applied

force� Consider applying some force to a link of a multibody� The joint accelerations depend

on this force� but because of other forces due to gravity and possibly joint actuators� and

because of the velocity dependent Coriolis and centripetal forces� the joint accelerations

are not linear functions of the applied force� On the other hand� when a collision impulse

is applied to the multibody� it dominates all other �ordinary magnitude� forces that are

acting on the multibody� including the external forces and the velocity dependent forces�

Unlike impulses� ordinary magnitude forces have no e�ect on dynamics over an in�nitesimal

interval� As a result� all of the nonlinear terms drop out of the dynamic equations during

the interval of collision� resulting in a linear relationship between the applied impulse and

the instantaneous changes in joint velocities� This implies a linear relationship between

applied impulse and change in contact point velocity� and the linearity can be exploited

just as it was for rigid body collision response�

For physical realism� friction and non�elastic behavior at the contact point should

be included in the model� just as they were for rigid body collisions� Much of physical

derivation behind rigid body collision response can be applied directly to the multibody

�

case� The key is computing the multibody collision matrix� the counterpart to the rigid

body collision matrix�

�� Collision response a robotics perspective

The existence of the multibody collision matrix can be shown using the classical

robotics formulation of multibody dynamics� Consider a robot in contact with its envi�

ronment� as in Figure � � The equations of motion for the robot in this situation are

f

Figure � � A robot in contact with its environment�

�Lil��

�q�t� �H��q�t��hQ�t��C�q�t�� #q�t�� #q�t� �G�q�t�� JT �q�t�� f�t�

i� � ��

Here� q is the vector of joint positions� H is the joint�space inertia matrix� C is the Coriolis

matrix� and C #q contains all of Coriolis and centripetal acceleration terms� G describes the

e�ects of external forces like gravity� J is the ��n Jacobian matrix at the end e�ector� �f is

the external force and torque applied to the end e�ector �the caret is a reminder that this

is a �� spatial force�� and Q is a vector of the magnitudes of forces and torques appliedby the joint actuators� All quantities on the right�hand side are known or computable from

the dynamic state� so this equation can be directly integrated to determine the forward

dynamics of the robot� This equation also shows that the joint accelerations are not a

linear function of the force �f applied to the end e�ector�

Suppose in Figure � that the end e�ector is colliding with the block� The collision

produces a very large external force �f acting on the multibody for a short period of time tc�

��

For rigid bodies� the collision time is in�nitesimal� consider the limit as tc � �� The changein joint velocities during the collision is

� #q � limtc��

Z tc

��q�t� dt�

In the limit� the collision force grows arbitrarily large� and the terms Q�t��C�q�t�� #q�t��

andG�q�t�� become negligible� The same assumption is made in rigid body collisions� when

�nite magnitude forces like gravity are ignored over the duration of a collision� Substituting

� �� into the above integral� and dropping negligible terms�

� �q � limtc��

Z tc

�H��q�t�� JT �q�t�� f�t� dt�

In the limit� q�t� is a constant� just as the positions of the bodies are constant during

isolated rigid body collisions� Pulling constants outside the integral�

� �q � H��q� JT �q� limtc��

Z tc

�

�f�t� dt � H��q� JT �q� �p�

where �p is the �� spatial collision impulse�The change in the end e�ector velocity is related to the change in joint velocities

through the Jacobian� �� v

�� J�q� � �q � J�q� H��q� JT �q� �p�

The �� matrix J�q� H��q� JT �q� on the right�hand side is the inverse of the operational

space inertia matrix � developed by Khatib �Kha�� This matrix describes the apparent

mass of the end e�ector to an observer exerting forces and torques on it�

The robot is one of two bodies involved in the collision� on the robot� call the

velocity at the contact point u�� The change in u� is related to the change in the end

e�ector�s velocities by

�u� � �v�� r�

where r is the o�set vector of the collision point in the end e�ector frame� Letting � denote

the � identity matrix� this can be written as a matrix equation�

�u� � ��"r�� v

�� "r��J�q� H��q� JT �q�� z �

W

�p�

�

The last equation shows the linear relationship between the collision impulse �p and the

change in contact point velocity u�� they are related by a � � constant matrix W� Theimpulse �p is a spatial quantity� containing both force and torque components� Both com�

ponents might be useful� for example� when modeling torsional friction during impact� For

rigid�body collisions� a simpler model is typically used� only pure forces can be generated

at the contact point� For this case� �p � �p��T � where p is a pure force impulse� with only

three components� Denoting the left � sub�matrix ofW by K��

�u� � K� p�

The other body involved in the collision also experiences a collision impulse and

a corresponding change in contact point velocity� By Newton�s third law� the impulse

experienced by the second body must be �p� Calling the contact point velocity on thesecond body u�� one �nds

�u� � K� ��p��

If the second colliding body is a link of a multibody� K� can be derived by the process

described above for K�� if it is just a rigid body� the formulas from Chapter may be used�

Since the relative contact point velocity u between the two bodies is given by

u � u� � u��

The above three equations imply�

�u � �K� �K�� z �K

p� � ��

Compare this equation with its counterpart �� derived for simple rigid bodies�

they are identical�� In both cases� the change in relative contact point velocity is related

to the total impulse applied at the contact point through a constant matrix� Once this

connection is made� the collision impulse can be found by forming the collision ODE and

performing a collision integration� as described in Section �� The same algorithm for

computing collision impulses between rigid bodies can be applied to colliding multibodies�

The above derivation is valid for multibodies that collide at any of their links� not only the

�In the current derivation� the explicit dependence of p and u on a collision parameter � has beensuppressed for clarity�

��

at end e�ectors� The only change to the above analysis is that the Jacobian in � �� must

be referenced to the colliding link�

One nice feature of hybrid simulation is that some kinematic loops are handled

automatically� Consider again the manipulator of Figure � � When it contacts the environ�

ment� it forms a kinematic loop with the environment� With a constraint�based approach�

this situation requires special handling� the manipulator dynamics must be combined with

the kinematic constraint imposed at the end e�ector �Lil�� Under hybrid simulation� the

contact force between the end e�ector and the environment is modeled with collision im�

pulses that are propagated through the manipulator� Once the collision response algorithm

is developed� no changes are needed in the manipulator forward dynamics algorithms� This

same technique is used to model the dynamics of wheeled vehicles that form kinematic loops

with the ground� examples are described in Chapter ��

�� Articulated body collision dynamics

The previous section demonstrated the existence of the multibody collision matrix

K� an algorithm for computing it is now developed� Only half the problem is considered�

computing the � matrix Ki� which relates the impulse applied to one of the colliding

bodies to the change in contact point velocity on that body� This problem is solved once

for each body� and K� and K� are combined to form the �nal multibody collision matrix

K� as in � �� One way to compute Ki has been shown already� it is the left half of the

� � matrixW � ��"r��J�q� H��q� JT �q��

Computing this product explicitly� however� results in an O�n�� algorithm for an n�link

multibody� due to the inversion of H�q�� Since collisions occur so frequently in an impulse�

based simulation� computing Ki e�ciently is important� By studying the dynamics of

articulated bodies of the full multibody during a collision� an O�n� algorithm for computing

Ki may be derived� The algorithm will be derived for tree�like multibodies� these include

serial multibodies as a subclass�

Consider the dynamics of a tree�like articulated body during a collision� From

��

limtc��

Z tc

�

�fI

h dt � limtc��

Z tc

�

�IA

h�ah dt� limtc��

Z tc

�

�ZA

h dt�

��

In the limit as tc approaches zero� the collision force acting on the body is in�nite� which

gives rise to in�nite collision forces acting through joints of the linkage� Call the limit of

the integral on the left hand side �pIh� the spatial impulse exerted on link h through the

inboard joint� The articulated inertia �IA

h depends on link masses and joint positions� which

are constant as tc � �� thus

�pIh � �IA

h

Z tc

��ah dt� lim

tc��

Z tc

�

�ZA

h dt

� �IA

h��vh � limtc��

Z tc

�

�ZA

h dt�

The quantity ��vh is the total change in spatial velocity of link h during the collision� it is

the limit of in�nite accelerations integrated over an in�nitesimal interval� The integral of

z�a� forces contain some terms which vanish in the limit� and others which remain� From

�� the last term in the above equation is

limtc��

Z tc

�

��Zh �mXj��

h�Xij

��ZA

ij��I

A

ij�cij �

�IA

ij�sij

hQij ��s�ij

�ZA

ij��I

A

ij�cij

�i�s�ij�IA

ij�sij

�� dt�

Velocities remain �nite in magnitude during a collision� therefore so do the spatial Coriolis

forces cij � The joint actuators can not apply impulses� so the Qij are also �nite� The

quantities h �Xij ��IA

ij� and �sij are all constant as tc � �� Pulling these constants outside the

integrals� and dropping the �nite terms that vanish in the limit�

limtc��

Z tc

�

�ZA

h dt � limtc��

Z tc

�

�Zh dt�mXj��

h�Xij

�� IA

ij�sij�s

�ij

�s�ij�IA

ij�sij

�� limtc��

Z tc

�

�ZA

ijdt� � ��

where � is the �� identity matrix� De�ning the isolated and articulated zero acceleration�z�a�� impulses as

�Yh � limtc��

Z tc

�

�Zh dt � ��

�YA

h � limtc��

Z tc

�

�ZA

h dt� � � �

� �� becomes�

�YA

h � �Yh �mXj��

h�Xij

�� IA

ij�sij�s

�ij

�s�ij�IA

ij�sij

�� YA

ij� � ��

��

Lemma � Consider a tree linkage that experiences a collision impulse at link k� Let �pcoll

be the spatial vector comprising the impulsive collision force and torque� resolved at the

center of the body frame of link k� Then the isolated z�a� impulses of the links are given by

�Yi �

�� i �� k

��pcoll� i � k

Proof� By de�nition� �Zi is the force applied at the inboard joint that cancels out the Cori�

olis� gravitational� and external forces acting on link i� when link i is considered detached

from the rest of the linkage� �Yi is the integral of this force over time as tc � �� The forceneeded to oppose the �nite magnitude Coriolis and gravitational forces may be ignored�

since they vanish in the limit� If i �� k� than there are no other external forces acting on

the link� and so �Yi � �� If i � k� then �Zi must also cancel the spatial collision force� This

force becomes the impulse �pcoll in the limit� thus�Yi � ��pcoll� �

Lemma � Consider a tree linkage that experiences a collision impulse at link k� If link k

is not in the subtree rooted at link h� �YA

h � ��

Proof� The proof is by induction on the height of the subtree rooted at link h� If the

height is �� link h is a leaf link with no descendents� By � �� YA

h � �Yh� Since h �� k�

�Yh � � by Lemma �� Suppose now the height of link h is greater than �� Index link h�s

children i�� im� Each of the children is the root of a subtree of lesser height which also

does not contain link k� By the inductive hypothesis� �YA

ij� �� j � m� From � ��

�YA

h � �Yh � ��

Lemma � implies that articulated z�a� impulses need only be calculated along the

chain from the colliding link k up to the base link� the articulated z�a� impulse for all other

links is � �Figure �� Since �YA

ij� � for all children ij of the colliding link k� �Y

A

k � �Yk

by � �� By Lemma ��

�YA

k � ��pcoll� � ��

Now consider link h� a proper ancestor of link k� It has exactly one child link which

is a �not necessarily proper� ancestor of link k� any other children are roots of subtrees which

do not contain link k� For these other children ij � �YA

ij� � by Lemma �� Thus only one

��

8 9

10

11 125 6

7

1

2

3

4

collidinglink

propagation ofz.a. impulses

Figure �� The z�a� impulses need only be computed along the path from the colliding linkto the base of the linkage� The gray links are not the colliding link or ancestors of it forthese links� the articulated z�a� impulse is ��

term of the summation in � �� is non�vanishing� Since h �� k� �Yh � � by Lemma �� The

simpli�ed expression for the articulated z�a� impulse of link h� a proper ancestor of k� is�

�YA

h � h�Xi�

�� IA

i��si��s�i�

�s�i��IA

i��si�

�� YA

i� � � ��

where i� is the unique child of link h which is on the path from h to k�

Computing the articulated z�a� impulses is half of the work� it is also necessary to

compute the instantaneous changes of velocity due to a collision impulse� This is also done

by taking the limit of dynamic equations for articulated bodies� From ��

limtc��

Z tc

��qi dt � lim

tc��

Z tc

�

Qi ��s�i�IA

i i�Xh�ah ��s�i

�ZA

i ��IA

i �ci�

�s�i�IA

i �sidt�

The �nite magnitude Qi and �ci vanish in the limit� �IA

i � i �Xh� and �si are constants that can

��

be pulled outside of the integral� The resulting change in link velocity is

� #qi � � �s�i

�s�i�IA

i �si

h�IA

i i�Xh��vh � �Y

A

i

i� � ��

where ��vh is the limit of the in�nite acceleration �ah integrated over an in�nitesimal interval�

The change in link i�s spatial velocity is obtained by applying the same technique to ��

limtc��

Z tc

��ai dt � lim

tc��

Z tc

�

i�Xh�ah � �qi�si � �ci

�dt�

Again dropping terms that vanish as tc � �� and simplifying�

��vi � i�Xh��vh �� #qi�si� � ��

At this point� a procedure can be described for determining the instantaneous

change in a link�s velocity� when a spatial collision impulse �Pcoll is applied to it�

�� Compute the articulated inertias of all links of the multibody�

� Compute the articulated z�a� impulses of the chain beginning at the colliding link�

and ending at the base link �Equations � ��

� Propagate instantaneous changes in joint and link velocities� along the path beginning

at the root link and ending at the colliding link �Equations � ��

Although most processing occurs along the path from the base link to the colliding link� the

e�ects of the other links enter through the articulated inertias of the path links� An algo�

rithm for implementing the strategy is shown in Figure �� Step � is not included because

articulated inertias of all links are assumed available from the last dynamic integration�

�� Computing Ki

The impulseResponse algorithm of Figure �� relates a spatial collision impulse

to a spatial change in velocity� But the collision matrix Ki relates non�spatial quantities�

Figure �� depicts the body frame Fk of the colliding link� as well as the collision frame

Fcoll� Let R be the � rotation matrix taking vectors in Fcoll to vectors in Fk� and let

r be the vector from the origin of Fcoll to the origin of Fk� in Fk�s coordinates� In frame

Fcoll� there is an impulsive force p � R� but no impulsive torque� In frame Fk� there is an

impulsive force Rp and an impulsive torque �r�Rp� or in spatial notation�

�pcoll �

�� RT

"rRT RT

�� p

��

��

impulseResponse

�YA

k ��pcolli k

while link i has a parent

h index of parent of link i

�YA

h � h�Xi

�� IA

i �si�s�i

�s�i�IA

i �si

�� YA

i

i h

�a� �

h �repeat

i index of child of link h on path to link k

� #qi � � �s�i

�s�i�IA

i �si

h�IA


A

i

i��vi � i

�Xh��vh �� #qi�si

h i

until i � k

Figure �� impulseResponse� Compute ��vk� the instantaneous change in spatial velocityof link k� when it experiences a collision impulse �pcoll� This routine assumes that thearticulated inertias of all links are already computed�

The � � � matrix is just the spatial transformation k�Xcoll� This equation shows how the

non�spatial impulse p is mapped into a spatial impulse �pcoll� The spatial change in velocity

��vk � ��k��vk�T computed by impulseResponse must then be converted into a non�

spatial change in velocity at the contact point� Since �k and vk are expressed in frame Fk�

the change in velocity at the contact point is

�u � RT ��vk ��k � ��r�� RT�vk �R

T"r��k�

��

link k

xy

z

x

z

y

Fk

Fcoll

rp

Figure �� The body frame of a colliding link and the collision frame�

This is just the lower � � component of the spatial product�� RT

RT"r RT

�� k

�vk

��

�The upper � � component of the product is the change in angular velocity at the con�

tact point� which is not used in the collision model�� It is easily veri�ed that the spatial

transformation matrix in the above equation is coll �Xk� the spatial transformation from Fk

to Fcoll�The above observations� along with the algorithm impulseResponse provide a

way to compute �u � R� � the change in contact point velocity� given �p � R� � an impulse

applied at the contact point� But the matrix Ki relates changes in velocity to an arbitrary

collision impulse� Since velocity changes at the contact point are linear in the applied

collision impulse it is only necessary to compute the response to a set of basis impulses at

��

the collision point� Let � � R� � R� be the mapping from applied impulse to change in

contact point velocity� An arbitrary impulse p may be decomposed as

p � pxex � pyey � pxez�

where ex� ey and ez are the canonical basis vectors of the collision frame� Then

��p� � px��ex� � py��ey� � pz��ez��

By building the matrix Ki as

Ki �

�� ex� ��ey� ��ez�

��

it follows that

��p� � Ki p � ��

for any p � R� � Thus� the impulse response of the colliding body may be completely

characterized by the impulse response to three test impulses� each a unit vector in one of

the canonical basis directions�

Finally� the algorithm for computing Ki may be completely written� It is shown

in Figure �� After K� and K� are computed� the complete multibody collision matrix is

given by their sum� Once K is known� a collision integration can be performed to determine

the collision impulse� whether either of the colliding bodies are multibodies or simple rigid

bodies� all of the necessary dynamic information is encoded in K�

�� Propagating impulses through multibodies

After the collision impulse p is determined� it must be propagated through the

multibody to determine the instantaneous change in joint velocities� The machinery to do

this has already been developed� The algorithm propagateImpulse �Figure �� is similar

to the impulseResponse algorithm� except that the e�ects are propagated throughout the

entire tree linkage rather than just along the path to the colliding link� The iteration is

performed by enumerating over the joints in increasing order� as in the forward dynamics

algorithms of Chapter ��

Figure �� summarizes the entire process of resolving a collision that involves

a link of a multibody� When and n��link and an n��link two multibody collide� the

��

compMultibodyKi

compute the articulated inertias for all links of the multibody

compute k�Xcoll and coll

�Xk from R and r

for i � � to

�pcoll k�Xcoll

�� ei

��call impulseResponse to compute �vk

��i lower � � component of coll�Xk��vk

Ki

��

��

Figure �� compMultibodyKi� Compute Ki� half of the collision matrix corresponding toone of the bodies in the collision� Inputs are k� the index of the colliding link� and R andr� which specify the relative positions of the collision and body frame�

total cost of processing the collision is O�n� � n� � C�� where C is the cost of performing

a collision integration� A linear time multibody collision response algorithm allows the

e�cient simulation of large articulated structures in an impulse�based setting�

For �oating linkages� the same adjustment that was made for the forward dynam�

ics algorithm must be made in the collision response algorithms� No change is required

for articulated inertia and z�a� impulse propagation� When computing velocity changes�

however� the base link must be handled as a special case� Since there is no inboard impulse

applied to the base link�

��v� � ��IA

�

��YA

� �

For all joints and other links� velocity change propagation needs no adjustment�

With simple rigid bodies� the collision matrix is always positive de�nite� however�

the multibody collision matrix is only guaranteed to be positive semide�nite� Thus� the

invertibility of K becomes an issue� Figure �� shows an example whereK can be singular�

��

propagateImpulse

�YA

k ��pcolli k

while link i has a parent

h index of parent of link i

�YA

h � h�Xi

�� IA

i �si�s�i

�s�i�IA

i �si

�� YA

i

i h

for all links i not on path from base link to link k� �YA

i �

�a� �

for i � � to n


� #qi � � �s�i

�s�i�IA

i �si

h�IA


A

i

i��vi � i

�Xh��vh �� #qi�si

Figure �� propagateImpulse� Compute the instantaneous change in spatial velocity ofjoints of a tree linkage� when link k experiences a spatial collision impulse �pcoll� �pcoll iscomputed from p via Equation � �� This algorithm assumes that the articulated inertiasof all links are already computed�

The two�link multibody falls and collides with the �xed horizontal plate� Because there are

only two degrees of freedom in the multibody motion� no matter what impulse p is applied

at the contact point� the total change in relative contact point velocity �u can have no

component in the y direction� Since �u � Kp� the � matrix K only has rank two� Butthe collision integration algorithms requireK�� This is not a problem in practice� Let "K

��

be the canonical �inverse� ofK obtained by singular value decomposition �SVD� �PTVF��

Then "K��u gives the minimum norm p that causes the given �u� the p computed in this

manner has no component in the null space of K� This gives the expected results� in the

��

F

8 9

10

11 125 6

7

1

2

3

4

velocitychanges

C

8 9

10

11 125 6

7

1

2

3

4

velocitychanges

D

E

8 9

10

11 125 6

7

1

2

3

4

pcoll

z.a. impulses

8 9

10

11 125 6

7

1

2

3

4test impulse

Bz.a. impulses

8 9

10

11 125 6

7

1

2

3

4

Aarticulatedinertias

collision

integrationpK

Figure �� The entire procedure for resolving collisions involving a multibody here� link �is the colliding link� �A� Propagate articulated inertias� This step can be eliminated bysaving inertias from the forward dynamics algorithm� �B� Apply test impulse to the collidinglink� and propagate z�a� impulses up to the base link� �C� Propagate joint and link velocitychanges back to the colliding link� Steps B and C are performed three times� for test impulsesin the x� y� and z directions afterwards� K� is known� K� is found similarly� and these arecombined to give K� �D� Collision integration determines p� �E� Propagate z�a� impulsesfrom �pcoll up to base link� �F� Propagate velocity changes throughout the tree�

collision of Figure �� the collision impulse will lie in the x�z plane� The same approach

can be used when the collision matrix has rank one�

��

p

x

yz

Figure �� A situation in which the multibody collision matrix is rank de�cient�

�� Supporting control systems

To animate multibodies� it is usually necessary to apply control forces or torques

at the joints in order to achieve the desired physical motion� �For the rest of this section�

force is used in a generalized sense to mean force or torque� depending on whether a joint

is prismatic or revolute�� Using the collision check scheduler that is already present in an

impulse�based simulator� a �exible control architecture can be built on top of the dynamic

simulator�

�� Types of controllers

There are several types of controllers that can be used to control multibodies�

More information than appears here can be found in �Cra�� Many systems contain passive

elements such as springs and dampers attached at the joints of a multibody� These are not

controllers in a strict sense� however they in�uence the dynamics of the system in a similar

��

way� They may be undesirable but unavoidable characteristics of the system� such as friction

in joint bearings� or they may be elements intentionally added to the system by a designer�

such as the shock�absorbers on a car� Passive controllers are characterized by the force f

applied� given the position q and velocity #q of a joint� such as�

f � �� sgn� #q�f � �� #qf � �k �q � qn�

f � � c

q � qn

In order� these equations correspond to� constant friction� viscous damping� a linear spring�

and a hard spring which arises� for example� in a sealed� prismatic cylinder �this latter force

has been exploited for hopping robot control �Rai��

Passive controllers are most accurately modeled by placing the force computations

within the inner loop of the dynamic integration� For example� every time the integrator

calls treeFwdDyn to compute �q� passive controllers forces are recomputed� based on q and

#q� These control forces may be lumped in with actuator forces� Q� since they modify the

dynamics in the same way� The �control laws� for these passive elements are simple� so

even in the inner integration loop� the cost of recomputing the control forces is a negligible

part of computing the forward dynamics�

At the lowest level of a control system are elements which control external forces to

be applied at joints in order to maintain desired joint positions or velocities� Proportional�

derivative �PD� or proportional�integral�derivative �PID� controllers are two common types�

PD controllers are used� for example� in the human athlete models of Hodgins� et� al�

�HWBO� �� The basic operation of a low level controller is depicted in Figure ��

Suppose a particular joint is to be maintained at some desired position� qdes�

One approach would be to attach a spring with natural length qdes to the joint� The

sti�er the spring� the harder the joint would be forced to the desired value� To prevent

oscillations� a damper could also be added to the system� A positional PD controller

mimics the behavior of this spring�damper system� In control parlance� the spring constant

and damping constant are called kp and kv� respectively �p for position and v for velocity��

The actuator force calculation for a positional PD controller is given by

Q � �kp�q � qdes�� kv #q�

��

physical

system

low level

controller

actuator forces/torques

Q

q qq, ,

qdes

qdes

qdes

or

or

joint positions, velocities, & accelerations

Figure �� The coupling of a low level controller to a multibody system� The controllertakes as inputs the position� velocity� and possibly acceleration of a particular joint� andcomputes a torque to be applied at that joint� based on the control law and desired motion�

In some cases� a PD controller is insu�cient for maintaining qdes� Consider the

prismatic joint in Figure �� It is easily veri�ed that the external force results in an

equilibrium position of

qdes �mg

kp�

The static error is made arbitrarily small by choosing a very large kp� however this can lead

to instability in the system� and numerical sti�ness in the equations� Instead� a proportional�

derivative�integral �PID� controller can be used� This type of controller augments the spring

and damper components with an integral term� that is based on the static error accumulated

over time�

Q�t� � �kp�q�t�� qdes�� kv #q�t�� ki

Z t

�

�q�� qdes

�d� � ��

A PID controller provides eventual convergence to the qdes in cases such as the one in

Figure �� although they are more di�cult to analyze and introduce new stability concerns�

Another type of low level controller is a computed torque controller� which shields

a higher level controller from the internal dynamics of a system� The dynamics of a point

mass are trivial� To make the mass�s acceleration equal to a� a force f � ma is applied� For a

multibody� the dynamics are more complicated� and joint accelerations are not proportional

to joint forces� A computed torque controller hides this complexity� so that a higher level

controller can directly specify the desired accelerations of the system� If the desired joint

� �

mg

qdes

kp

Figure �� A PD controller with position constant kp causes the unweighted joint to attaina value of qdes� but it can not do so when the joint is loaded with a static weight�

accelerations of a multibody system are �qdes� the computed torque law is

Q � H�q��qdes �C�q� #q� #q �G�q�� JT �q� �f � � ��

Substituting into Equation � �� shows that with these joint forces� �q � �qdes� A computed

torque controller requires much more computation than PD and PID controllers� since a

dynamic model of the multibody must be analyzed to determine the joint forces�

Unlike passive elements like springs� controllers run at �nite rates� A controller is

updated with new values of q and #q at regular intervals� and it commands actuator forces

which will persist until the next update� even though q and #q change over the interval� The

update rate must be fast enough so that the controller has reasonably current information�

otherwise instability results� The required rate depends roughly on the mass of the system

and on the controller gains� Less massive systems and higher gains require higher update

rates�

� �

High level controllers are needed to give multibody systems intelligent behavior�

While a low level controllers might allow a human �gure to stand� a high level controller is

necessary to make it walk toward a goal� High level controllers are less concerned with the

dynamic stability of the system� and more concerned with the overall behavior� They run at

rates slower than the rates of the low level controllers� For example� a robotic controller that

examines machine vision input would have a fairly slow update rate� High level controllers

are more varied than low level controllers� and there are few canonical types� Finite state

machine controllers have enjoyed recent success as high level controllers for hopping robots

�Rai�� and models of human athletes �HWBO� �� These track the phase of the overall

system �such as �ight or loading�� and send commands to low level PD controllers based on

the phase� Use of state machines for higher level behavior control is described by Brooks in

the context of robot planning �Bro�� and by Ahmad et� al� for scenario control �ACH��

�� Controller scheduling

There may be many di�erent controllers for a system� running in parallel at di�er�

ent rates� In simulation� controllers are software routines that compute applied forces from

the current state� These routines must be called to recompute the control forces at a rate

matching that of the real controller they model� Scheduling control update events is simple

in an impulse�based simulator� As discussed in Chapter � the integration intervals are

determined by the collision heap� The system is integrated to the point in time indicated

by the TOI �eld of the top heap element� at which point a collision check is performed

between two bodies� The simulator guarantees that no heap events are missed�

Control update events can be added to the collision check events in the heap�

Suppose a PD controller is to update joint torques at a � kHz rate� A structure for this

control event is placed into the heap� with a time �eld set to the current time plus ��

seconds� Using the same scheduling algorithm� the simulation will be stopped at the appro�

priate time� and the PD update procedure can be called� The control event is placed back

into the heap� with the time �eld incremented by �� s� In this way� the simulator can

manage a variety controllers running independently and at di�erent rates� Adding control

update events to the heap does not signi�cantly a�ect the size of the integration steps�

collision check events still dominate the top of the heap� For simulations with a only few

bodies� collision checks can be made at rates on the order of �� kHz� This is far higher than

�

typical controller frequencies� The passive controller forces are computed within the inner

integration loop� so they are not scheduled in the heap�

�� Impulse�s control support architecture

Impulse shields the designer of a control system from the inner workings of the

control law scheduler and the dynamic integration loop through a layer of interface routines�

Standard types of low level controllers are provided� The designer writes high level control

routines as C functions that are compiled and linked with the rest of the simulator code�

When a high level controller is instantiated� it is registered with the scheduler so that it

may be called back at the desired rate� The high level control routine can instantiate low

level controllers and interact with them� Impulse�s control library contains�

�� Routines to create passive elements� and attach them to speci�ed joints of a multibody�

Parameters like spring constants and damping coe�cients are user speci�ed�

� Routines to create low level controllers� PD and PID controllers are available� in

both position� and velocity�control varieties� Computed torque controllers are also

provided� The rates and gains of these controllers are speci�ed when they are created�

Communication with these controllers is handled through routines that command

desired positions� velocities� or accelerations to an existing controller� and disable or

enable the controller� A high level controller typically calls these interface routines�

� Routines to execute general high level controllers at speci�ed rates through a call back

mechanism� The high level controller routines �currently C functions� are written by

the designer� When called� these high level controller routines are passed a pointer to

the object being controlled� so that the same control function can be used for many

clones of a particular object�

With these library routines� one can build up interesting control systems with a

few lines of C code� Figure �� is a system level view of the control support architecture�

The lowest level is the simulation engine� which contains modules to compute the dynamics

of rigid bodies and multibodies� and to perform collision detection and collision response

between bodies� On top of this layer is the control library described above� The designer

speci�es high level controllers on top of the library� A high level controller may do nothing

more than instantiate some low level controllers� and send them initial commanded values�

�

dynamics

engine

collision

response

P D P I Dtorque

computed

p, v

f

highlevelcontrol

controllibrary

simulationengine

scheduled

collision

detection

Figure �� The control support architecture of Impulse� Lower level controllers are pro�vided on top of the simulation engine� and the designer writes the high level controllers�The dotted line indicates controllers that are scheduled in the heap�

Or it can perform complex computations involving the state of the system and environment

to determine behavior� The designer is free to create new types of low level controllers� or a

control layer between the levels shown� Examples that use this control support architecture

are described in Chapter ��

� �

Chapter �

Computation of Mass Properties

for Polyhedral Bodies

For dynamic simulation involving rigid bodies� it is necessary to know several pa�

rameters describing the mass distribution of these bodies� A common formulation comprises

ten parameters� the total mass �a scalar�� the location of the center of mass �three param�

eters�� and the moments and products of inertia about the center of mass �six parameters��

One can always �nd a body frame� with origin at the body�s center of mass and axes aligned

with its principle axes of inertia� In this way� the entire mass distribution can be described

with a reduced set of four parameters� however the larger parameterization is still needed

as a starting point�

The problem of computing mass properties of solid objects has been studied ex�

tensively� Lee and Requicha note that the algorithms are closely tied to the underlying

representation of solid objects� and give an excellent survey of the various families of algo�

rithms in existence �LR�a�� Representations based upon primitive instancing and simple

sweeping have small domain of application� only simple �canned� objects may be described�

Algorithms based on decomposition fall into two categories� according to whether the de�

composition is exact or approximate� Exact decompositions are di�cult to obtain for general

objects and usually require considerable human labor �LK�� Approximate decompositions�

such as spatial enumerations or octree methods may be easier to generate but yield only

estimates of the desired integral properties �LR�b�� The resolution of these representations

determine the accuracy of the estimate� and errors in the representation tend to dominate

�

numerical errors �LR�a�� Algorithms that operate directly on CSG representations are

computationally expensive� indeed� the best algorithms for CSG operate by �rst converting

the CSG representations into approximate cell decompositions� Integrals over solid regions

can also be evaluated using Monte Carlo techniques� based on sampling points inside a

volume containing the solid� Accuracy increases with the number of sample points� but

convergence is slow� and long execution times are required for accurate results �LR�a��

Algorithms based on boundary representations �Breps� remain the method of

choice for exact� e�cient computation of integrals over solids� This is fortunate since most

graphical rendering algorithms require Breps� as do many e�cient collision detection algo�

rithms �Lin�� Van�� Thus� for applications such as dynamic simulation� Breps are likely

to be available� Lien and Kajiya give an algorithm for computing integrals over arbitrary

nonconvex polyhedra� based on a Brep �LK�� It is O�n� in the polyhedron complexity� and

works by projecting triangular faces to the origin� forming tetrahedra� and summing the

contributions of these tetrahedra to the composite integrals� Numerical errors increase in

near degenerate situations where the tetrahedra are long and thin� These errors are reduced

by shifting the projection origin to the barycenter of the object� however tetrahedra of high

aspect ratio can still occur� especially for highly tessellated objects� Also� the algorithm

essentially triangulates any non�triangular faces of the original polyhedron� and the manner

in which this is done may also generate long� thin tetrahedra� Nevertheless� the algorithm

proposed by Lien and Kajiya is fairly easy to code and generalizes to higher dimensions� It

may be easily adapted to compute any integral of the form xiyjzk over the solid region� in

addition to the integrals arising when computing mass properties�

This chapter presents an exact algorithm for computing mass properties of uniform

density polyhedra� based on a boundary representation of the solid� Like Lien and Kajiya�s

algorithm� it is exact� and linear in the number of vertices� edges� or faces of the polyhedron�

The polyhedron may be nonconvex� and the faces may have any number of sides� The

only assumptions made on the representation are that one can enumerate over all faces of

the boundary� and that given a face� one can enumerate over all edges in counterclockwise

order� In contrast to Lien and Kajiya�s algorithm� the algorithm is optimized for computing

the mass properties required for dynamic simulation� namely mass� center of mass� and

moments and products of inertia� These quantities are all computed in parallel during a

single traversal of the polyhedron� so that common subexpressions are exploited� it is very

fast� In addition� the algorithm presented here adaptively changes the projection direction�

� �

minimizing degeneracy and reducing numerical errors over those in Lien and Kajiya�s and

other algorithms�

�� Rigid�body mass parameters

See Appendix A� for an overview of rigid body dynamics� Key quantities in rigid�

body dynamics are a body�s mass m� center of mass r and inertia tensor Io� also called the

mass matrix� In order to formulate the equations of motion for the body� these quantities

must be determined� The initial problem may be expressed as follows�

Problem � Given� A rigid body comprising N parts� B�� BN � each a uniform density

polyhedron� There are no restrictions on the convexity or genus of the polyhedra� nor on

the shape of the bounding faces� For each polyhedron Bi� either its density �i or mass mi is

speci�ed� and the geometries of all of the polyhedra are speci�ed relative to a single reference

frame� Compute� The mass m� and the reference frame coordinates of the center of mass r

and inertia tensor Io for the entire rigid body�

The mass mi and density �i of polyhedral part Bi are related by mi � �iVi� where

Vi is the volume of the polyhedron� Assuming one can compute

Vi �

ZBi

dV�

the masses and densities of each polyhedron can be found� The total mass is m �PN

i��mi�

The coordinates of the center of mass r for the entire body are

r ��

m

NXi��

�ZBi

x dm�

ZBi

y dm�

ZBi

z dm

�T

�

The integrals above are the sums over all di�erential mass elements of the body� but for

uniform density polyhedra� these are easily converted to volume integrals� For body Bi�

dm � �i dV � Thus�

r ��

m

NXi��

�i

�ZBi

x dV�

ZBi

y dV�

ZBi

z dV

�T

�

The inertia tensor is a symmetric matrix� containing the moments and products

of inertia�

I� �

��Ixx �Ixy �Izx�Ixy Iyy �Iyz�Izx �Iyz Izz

��

� �

The moments of the inertia are given by

I �xx �NXi��

ZBi

�y� � z�� dm �NXi��

�i

ZBi

�y� � z�� dV

I �yy �NXi��

ZBi

�z� � x�� dm �NXi��

�i

ZBi

�z� � x�� dV

I �zz �NXi��

ZBi

�x� � y�� dm �NXi��

�i

ZBi

�x� � y�� dV�

Finally� the products of inertia are given by

I �xy �NXi��

ZBi

xy dm �NXi��

�i

ZBi

xy dV

I �yz �NXi��

ZBi

yz dm �NXi��

�i

ZBi

yz dV

I �zx �NXi��

ZBi

zx dm �NXi��

�i

ZBi

zx dV�

The primed inertia quantities above are not exactly the values appearing in the inertia

tensor� The primed values are computed relative to the given reference frame� but the

values in the inertia tensor must be computed relative to a frame with origin at the center

of mass� One way to accomplish this is to �rst compute the location of the center of mass

in the given frame� and then to apply a translation to the body which brings the center

of mass to the origin� After performing this transformation� the primed quantities can be

directly inserted into the inertia tensor�

A better solution is to use the transfer�of�axis relations for transferring a moment

or product of inertia about one axis to a corresponding one about a parallel axis� To transfer

the primed values computed above to a frame at the center of mass� one uses �MK��

Ixx � I �xx �m�r�y � r�z�

Iyy � I �yy �m�r�z � r�x�

Izz � I �zz �m�r�x � r�y�

Ixy � I �xy �mrxry

Iyz � I �yz �mryrz

Izx � I �zx �mrzrx�

The unprimed quantities are inserted into the inertia tensor� If the transfer�of�axis relations

are used� it is not necessary to explicitly transform the vertices of the polyhedron after

computing the center of mass� hence all of the integrals can be computed simultaneously�

� �

Although not explicitly allowed in the problem statement� point masses placed at

various locations on the rigid body are easily handled� Suppose the component body Bi is

a point mass rather than a polyhedron� located at position ri in the reference frame� All of

the required mass integrals can still be computed� since for a point massZBi

f dm � mif�ri�

where f is any scalar function of x� y� and z� This substitution may be used to compute all

integrals over Bi�

Rigid body dynamics can be computed more e�ciently with a reduced set of mass

properties� based on a body frame� This is a coordinate system attached to the body� with

origin at the body�s center of mass� and axes aligned with the body�s principle axes of

inertia� In this case� all of the products of inertia vanish and the inertia tensor is diagonal�

To compute the body frame� one must �rst compute r and Io as above� and then �nd and

a rotation matrix R such that

I � RT IoR

is diagonal� In this case� I is the diagonalized mass matrix� and R is the rotation matrix

transforming vectors in the body frame to vectors in the original reference frame� the

columns of R are the �i� �j� and �k unit vectors of the body frame� expressed in the reference

coordinates� The diagonal elements of I are the moments of inertia about the principle axes

of the body� Since these are positive� I and Io are positive de�nite� Diagonalizing a matrix�

that is �nding R� is a classical problem of linear algebra� The Jacobi method works quite

well for this application since Io is real� symmetric� and of moderate size� See �PTVF�� for

a complete discussion of this technique as well as an implementation�

�� Derivation of the algorithm

From the previous section� all required mass properties can be found from ten

integrals over volume� for each of the individual polyhedral components� To simplify no�

tation� the polyhedron index is henceforth dropped� and only a single polyhedral body is

considered� The domain of integration is written V as a reminder that it is a volume� Theremainder of this chapter describes an e�cient� exact algorithm for calculating these ten

� �

integrals�

T� �

ZV� dV

Tx �

ZVx dV

Ty �

ZVy dV

Tz �

ZVz dV

Tx� �

ZVx� dV

Ty� �

ZVy� dV

Tz� �

ZVz� dV

Txy �

ZVxy dV

Tyz �

ZVyz dV

Tzx �

ZVzx dV�

��

Note that each is an integral of a monomial in x� y� and z� The basic idea is to use

the divergence theorem to reduce each of the volume integrals to a sum of surface integrals

over the individual faces of the polyhedron� Each of these surface integrals are evaluated

in terms of integrals over a planar projection of the surface� For polygons in the plane�

Green�s theorem reduces the planar integration to a sum of line integrals around the edges

of the polygon� Finally� these line integrals are evaluated directly from the coordinates of

the projected vertices of the original polyhedron� Figure �� illustrates the approach� the

volume integral is ultimately reduced to a collection of line integrals in the plane� and the

values from these integrations are propagated back into the value of the desired volume

integration�

divergencetheorem projection Green’s

theorem

integral reduction

value propagation

Figure �� The strategy for evaluating volume integrals� Complicated integrals are decom�posed into successively simpler ones� and the values from evaluating the simplest integralsare combined and propagated back to evaluate the original ones�

��

�� Reduction to surface integrals

The �rst reduction is from an integral over the three�dimensional polyhedral vol�

ume to a sum of integrals over its two�dimensional planar faces� This reduction is easily

accomplished with the divergence theorem �Ste��

Theorem � Divergence� Let V be a region in space bounded by the surface �V� Let �ndenote the exterior normal of V along its boundary �V� ThenZ

Vr � F dV �

Z�VF � �n dA ��

for any vector �eld F de�ned on V�

For a polyhedral volume� the right�hand side of �� can be broken up into a

summation over faces of constant normal� so �n can be pulled outside the integral� The

integrals to be computed� for exampleRV x dV � do not immediately appear to be of the

form in the theorem� But one can �nd many vector �elds whose divergence is the function

x� a particularly simple choice is F�x� y� z� � ��x�� T � This choice has the added

advantage that two of its components are identically zero� so that the dot product on the

right�hand side of �� becomes a scalar multiplication� By making similar choices for the

other integrals and applying the divergence theorem� Equations �� become�

T� �XF��V

&nx

ZFx dA

Tx ��

XF��V

&nx

ZFx� dA

Ty ��

XF��V

&ny

ZFy� dA

Tz ��

XF��V

&nz

ZFz� dA

Tx� ��

XF��V

&nx

ZFx� dA

Ty� ��

XF��V

&ny

ZFy� dA

Tz� ��

XF��V

&nz

ZFz� dA

Txy ��

XF��V

&nx

ZFx�y dA

Tyz ��

XF��V

&ny

ZFy�z dA

Tzx ��

XF��V

&nz

ZFz�x dA�

��

�� Reduction to projection integrals

Green�s theorem reduces an integral over a planar region to an integral around its

one�dimensional boundary� however one must start with a region in the plane� Although

��

the planar faces of the polyhedron are in three�space� one can project them onto one of

the coordinate planes� The next theorem relates integrations over the original face to

integrations over the projection�

Theorem �� Let F be a polygonal region in �� space� with surface normal �n� and lying

in the plane

&n�� &n�� &n� � w � ��

Let ' be the projection of F into the �� plane� ThenZFf�� dA �

�

j&n� jZf�� h�� d� d��

where

h�� &n��&n�� &n�� w��

Proof� The points �� h�� lie in the plane of F � so F is the graph of h over '� From�EP�� Section �� Formula ��

ZFf�� dA �

Zf�� h��

s� �

��h

��

��

�

��h

��

��

d� d��

The square root in the integrand reduces to j&n� j�� and the theorem follows� �

This theorem provides the reduction of the integral of a polynomial in �� and

over the face F to the integral of a polynomial in � and � over the projected region '�From �� the constant w can be computed� w � ��n �p� where p is any point on the faceF � Numerical inaccuracy or �oating point errors can occur when the face normal �n haslittle or no component in the projection direction� in the extreme situation �&n� � �� the

face projects to a line segment� To reduce such errors� for a given face the �� coordinates

are always chosen as a right�handed� permutation of the of the x�y�z coordinates such that

j&n� j is maximized� This choice maximizes the area of the projected shadow in the �� plane�see Figure �� Note that a choice can always be found such that j&n� j �

p��

Recall the needed integrals �� over the region F � Independent of the threepossible correspondences between the x�y�z and �� coordinates� they all can be found

�This means ��

��

by computing the following � integrals�

ZF� dAZ

F� dAZ

F dA

ZF�� dAZ

F�� dAZ

F � dA

ZF�� dAZ

F�� dAZ

F � dA

ZF�� dAZ

F�� dAZ

F �� dA�

Using Theorem �� these � face integrals can all be reduced to integrals over the projection

region '� For instance�ZF�� dA � j&n� j��

Z��&n�� &n�� w

�&n� d� d�

� �j&n� j��&n��&n�

Z�� d� d� � &n�

Z�� d� d� � w

Z�� d� d�

�� j&n� j��&n�� &n�� &n�� w��

where

�f �

Zf dA� ��

��

The complete set of face integrals� reduced to projection integrals with Theorem ��

is shown below�

ZF� dA � j&n� j��Z

F� dA � j&n� j��Z

F dA � �j&n� j��&n�� &n�� &n�� w��Z

F�� dA � j&n� j��Z

F�� dA � j&n� j��Z

F � dA � j&n� j��&n�� &n�� &n�&n�� &n�� &n�w�� &n�w�� w��Z

F�� dA � j&n� j��Z

F�� dA � j&n� j��Z

F � dA � �j&n� j��&n�� &n�� &n��&n�� &n�&n�� &n�� &n��w��

� �&n�&n�w�� &n��w�� &n�w

�� &n�w�� w��Z

F�� dA � j&n� j��Z

F�� dA � �j&n� j��&n�� &n�� &n�� w��Z

F �� dA � j&n� j��&n�� &n�� &n�&n�� &n�� &n�w�� &n�w�� w��

��

�� Reduction to line integrals

The �nal step is to reduce an integral over a polygonal projection region in the ��

plane to a sum of line integrals over the edges bounding the region� The following notation

is used �see Figure �� The edges of ' are labeled E� through EK � Edge Ee is the directedline segment from ��e� �e� to ��e�� e�� e � �e�� e� and ��e � �e�� e �note

that ��K�� K�� Finally� edge Ee has length Le and exterior unit normal �me�

Green�s theorem� �Ste�� provides the �nal integration reduction�

�Sometimes more formally called Green�s theorem in the plane� Additionally� some texts call this Green�s

lemma� reserving Green�s theorem for a more general �D result �WB��

��

α

β

γn

F

Π

Figure �� The �� axes are a right�handed permutation of the x�y�z axes chosen tomaximize the size of the face�s projected shadow in the �� plane�

Theorem �� Green�s� Let ' be a region in the plane bounded by the single curve �'�

Let �m denote the exterior normal along the boundary� ThenZr �H dA �

I�H � �m ds ��

for any vector �eld H de�ned on '� where the line integral traverses the boundary counter�

clockwise�

This theorem is a two�dimensional version of the divergence theorem� and the polyhedral

application again provides simpli�cation� Since ' is polygonal� the right�hand side of ��

may be broken into a summation over edges of constant normal� and by always choosing H

so that one component is identically zero� the dot product becomes a scalar multiplication�

From �� and �� all integrals of the form

��p�q �

Z�p�q dA

��

α

β

Π

(α , β )1 1

(α , β )e e

(α , β )e+1 e+1

∆βe

∆αe

me

ε1

εe

(α , β )K KεK

Figure �� Notation for computing a projection integral as a sum of line integrals�

are needed for nonnegative integers p and q with p� q � � Consider �rst the case q � ��By choosing H � � �

p��p�� T � and applying Green�s theorem to the polygonal region '�

Z�p dA �

�

p� �

KXe��

&me�

ZEe��s�p�� ds�

In the right�hand integral� the integration variable s is arc length� and runs from � to Le�

the length of the edge� ��s� is the ��coordinate of the point on the edge that is a distance s

from the starting point� Letting the variable � be s�Le� one can change integration variables

�ds � Led�� to get Z�p dA �

�

p� �

KXe��

&me�Le

Z �

��Le��

p�� d��

Now &me�Le � ��e� where the plus or minus depends on whether the vertices of ' areindexed in counterclockwise or clockwise order� respectively� By convention� assume that the

vertices of the original face F are indexed in counterclockwise order� thus the vertices of 'will be indexed in counterclockwise order exactly when the sign of &n� is positive �Figure ��

helps in visualizing this situation�� Hence� &me�Le � sgn�&n��e� andZ�p dA �

sgn�&n��

p� �

KXe��

��e

Z �

��Le��

p�� d��

��

The case with q � � is similar� except one chooses H � � �p��

p�� T � Finally� one can

derive analogous equations for the cases when p � � or p � �� The results are�

Z�q dA � �sgn�&n��

q � �

KXe��

��e

Z �

��Le��

q�� d�

Z�p� dA �

sgn�&n��

p� �

KXe��

��e

Z �

��Le��

p��Le�� d�

Z��q dA � �sgn�&n��

q � �

KXe��

��e

Z �

��Le��Le��

q�� d��

��

�� Evaluation of integrals from vertex coordinates

The original volume integrals have been successively reduced to face integrals�

projection integrals� and �nally line integrals� The latter can be directly evaluated in terms

of vertex coordinates� with the help of the following theorem�

Theorem �� Let Le be the length of the directed line segment from ��e� �e� to ��e�� e��

Let ��s� and ��s� be the �� and ��coordinates of the point on this segment a distance s from

the starting point� Then for nonnegative integers p and q�

Z �

��Le��

p��Le��q d� �

�

p� q � �

pXi��

qXj��

�� p

i

�A�� q

j

�A�� p� q

i� j

�A �ie��p�ie �je��

q�je �

Proof� Denoting the integral on the left�hand side by I�

I �

Z �

�� e � ��e��

p �� e � ��e��q d�

�

Z �

�

pX

i��

Bpi ��

ie��

p�ie

! �� qXj��

Bqj ��

je��

q�je

�� d��

where the B�s are Bernstein polynomials�

Bnk ��

�� n

k

�A�k�� n�k�

��

Two important properties of these polynomials are �FR�� Far��

Bpi ��B

qj ��

�� p

i

�A�� q

j

�A�� p� q

i� j

�A Bp�qi�j ��

Z �

�Bnk �� d� �

�

n� ��

Expanding the product in �� and applying �� gives

I �pX

i��

qXj��

�� p

i

�A�� q

j

�A�� p� q

i� j

�A �ie��p�ie �je��

q�je

Z �

�Bp�qi�j �� d��

Evaluating the integrals using �� proves the theorem� �

All of the line integrals appearing in ��$�� are special cases of Theorem �

with either p or q set to � or �� Speci�cally�

Z �

��p�� d� �

�

p�

p��Xi��

�ie��p��ie

Z �

��q�� d� �

�

q �

q��Xj��

�je��q��je

Z �

��p�� d� �

�

�p� ��p� �

��e��

p��Xi��

�i� ��ie��p��ie � �e

p��Xi��

�p� � i��ie��p��ie

��Z �

��q�� d� �

�

�q � ��q � �

��e��

q��Xj��

�j � ��je��q��je � �e

q��Xj��

�q � � j��je��q��je

��

��

Making these substitutions into ��$�� give all of the needed projection integrals in

terms of the coordinates of the projection vertices�

��

Z� dA � �

sgn�&n��

KXe��

��e��e�� e�

��

Z� dA � �

sgn�&n��

�

KXe��

��e��e�� e��e � ��

e�

��

Z� dA � �sgn�&n��

�

KXe��

��e��e�� e��e � ��e �

��

Z�� dA � �

sgn�&n��

�

KXe��

��e��e��

e��ae � �e��e � ��

e�

��

Z�� dA � �

sgn�&n��

�

KXe��

��eh�e��

�e�� e��e � ��

e�

� �e��e�� e�e��

�e�i

��

Z�� dA � �sgn�&n��

�

KXe��

��e��e�� e��e � �e��

�e � ��e �

��

Z�� dA � �

sgn�&n��

�

KXe��

��e��e��

e��e � ��e��

�e � �e��

�e � ��

e�

��

Z�� dA � �

sgn�&n��

��

KXe��

��eh�e��

�e��

�e��e � �e��

�e � ��

e�

� �e��e��

�e��e � �e��

�e � ��

�e�i

��

Z�� dA � �sgn�&n��

��

KXe��

��eh�e��

�e��

�e��e � �e��

�e � ��e �

� �e��e��

�e��e � �e��

�e � ��

�e �i

��

Z�� dA � �sgn�&n��

�

KXe��

��e��e�� e��e � ��e��

�e � �e��

�e � ��e ��

��

��

�� Pseudocode and on�line C code

The derivations of the previous section specify a complete algorithm for computing

the ten desired volume integrals �� The algorithm comprises three routines�

�� CompVolumeIntegrals�V� �Figure �� computes the required volume integrals for apolyhedron by summing surface integrals over its faces� as detailed in Equations ��

� CompFaceIntegrals�F� �Figure �� computes the required surface integrals over apolyhedral face from the integrals over its projection� as detailed in Equations ��

� CompProjectionIntegrals�F� �Figure �� computes the required integrals over aface projection from the coordinates of the projections vertices� as detailed in Equa�

tions ��

This algorithm contains a minor simpli�cation over the presented derivation� When Equa�

tions �� are substituted into �� the computation of sgn�&n�� and j&n� j becomes un�necessary� since these factors always appear together in a product� giving simply &n� � Thus�

no signs or absolute values are computed in the routines CompFaceIntegrals and Comp�

ProjectionIntegrals�

ANSI C source code for the algorithm described in this paper� and detailed in

Figures ��$�� is publicly available from two web sites�

http ��www�acm�org�jgt�

http ��www�cs�berkeley�edu��mirtich�massProps�html

Also included is an interface to build up polyhedra� using a simple data structure� from

ASCII speci�cations� examples are provided� The code is public domain� and may be used

as is or in modi�ed form�

�� Test results

This section analyzes the speed and accuracy of the algorithm for various test

cases� These tests were run on an SGI Indigo II with a �� MHz R�� CPU� and double

precision �oating point numbers were used for the calculations�

The set of polyhedral objects that have volume integrals which are commonly

tabulated or easy to compute by hand is rather limited� The algorithm was run on two

��

such objects� an axes�aligned cube� � units on a side and centered at the origin� and a

tetrahedron de�ned by the convex hull of the origin� and the vectors �i� ��j� and �k� The

theoretical values for these objects are shown in Table �� For these two examples� all

object T� Tx Ty Tz Tx� Ty� Tz� Txy Tyz Tzxcube �� tetrahedron ��

Table �� Theoretical values of volume integrals for simple test polyhedra�

values computed by the algorithm were correct to at least � signi�cant �gures� The total

time required to compute all ten volume integrals was �� s for the tetrahedron� and ��

�s for the cube�

For a more interesting test� the algorithm was applied to several polyhedral ap�

proximations of a unit radius sphere� centered at the origin� In this case there are two

sources of error� numerical errors from the algorithm� and approximation errors inherent in

the geometric model� which is not a true sphere� These latter errors should not be attributed

to the algorithm itself� For a perfect unit sphere� the integrals Tx� Ty� Tz� Txy� Tyz� and Tzx

should vanish� while T� �� and Tx� � Ty� � Tz� �

�� The algorithm was applied

to a series of successive approximations to the sphere� beginning with an icosahedron� and

obtaining each �ner approximation by projecting the midpoint of each polyhedral edge onto

the unit sphere� and taking a convex hull� The computed values of a representative set of

volume integrals for each polyhedron are shown in Table ��

Without numerical error� the integrals Tx and Tyz would vanish for all six polyhe�

dral approximations of the sphere� due to symmetry� From Table �� the absolute values of

these computed values are all less than �� The theoretical values in the table correspond

to the sphere which circumscribes the polyhedra� For each polyhedron� the corresponding

values for the inscribed sphere were also determined� and the computed values for T� and

Tx� veri�ed to lie between the bounding values for these two spheres� For approximation

�� the di�erence in values for the inscribed and circumscribed sphere is �� for T�and �� for Tx� � These values are upper bounds on the numerical errors of the al�gorithm� Note that the deviations between theoretical and computed values for T� and Tx�

are reduced as the complexity of the polyhedron increases� while numerical error from the

��

algorithm should increase with complexity� In light of the very small errors incurred in the

computation of Tx and Tyz� the deviations between the computed and theoretical values of

T� and Tx� are almost certainly due mainly to the polyhedral approximation rather than to

numerical errors�

approx� verts edges faces T� Tx Tx� Tyz time

� � � � � � �� s � �� ms �� ms� �� ms � �� ms� �� ms

theoretical values for sphere �� $

Table �� Data for successive approximations of a unit sphere�

The execution times shown in Table �� are the total times for computing all ten

volume integrals for each polyhedron� The O�n� nature of the algorithm is evident� from

approximation on� the time ratios between successive re�nements very closely follow the

�� ratio in the number of faces� The algorithm is also very fast� all ten integrals are

computed for a polyhedron with over �� faces in only � ms�

Acknowledgments This chapter originally appeared as the paper �Mir�� We thank

Aristides Requicha for a valuable literature survey on this topic� and David Bara� for

useful comments on the initial draft of this paper� We especially thank John Hughes for his

detailed review and many suggestions for improving the paper�

��

CompVolumeIntegrals�V�

T�� Tx� Ty� Tz� Tx� � Ty� � Tz� � Txy� Tyz � Tzx �

for each face F on the boundary of V�� right�handed permutation of �x� y� z� that maximizes j&n� jcompFaceIntegrals�F�if �� x� T� T� � &n�F�

else if �� x� T� T� � &n�F�

else T� T� � &n�F�

T� T� � &n�F��

T� T� � &n�F��

T� T� � &n�F��

T�� T�� &n�F��

T�� T�� &n�F��

T�� T�� &n�F��

T�� T�� &n�F��

T�� T�� &n�F��

T�� T�� &n�F��

�Tx� Ty� Tz� �Tx� Ty� Tz� � �Tx� � Ty� � Tz�� Tx� � Ty� � Tz�� Txy� Tyz � Tzx� �Txy� Tyz � Tzx� �

Figure �� CompVolumeIntegrals�V�� Compute the required volume integrals for a poly�hedron� See Equations ��

��

CompFaceIntegrals�F�

computeProjectionIntegrals�F�w ��n � p for some point p on Fk� &n�� k� k� � k�� k� k� � k�� k� k� � k�

F� k��

F� k��

F� �k��&n�� &n�� w��

F�� k��

F�� k��

F�� k��&n�� &n�&n�� &n

�� &n�w�� &n�w�� w��

F�� k��

F�� k��

F�� k��&n�� &n��&n�� &n�&n�� &n��&n��w�� &n�&n�w�� &n

��w�� &n�w

�� &n�w�� w��

F�� k��

F�� k��&n�� &n�� w��

F�� k��&n�� &n�&n�� &n

�� &n�w�� &n�w�� w��

Figure �� CompFaceIntegrals�F�� Compute the required surface integrals over a poly�hedral face� See Equations ��

��

CompProjectionIntegrals�F�

�� b� ��

for each edge E in CCW order around F

�� coordinate of start point of E

�� coordinate of start point of E

�� coordinate of end point of E

�� coordinate of end point of E

��

��

��

��

��

��

��

��

��

��

��

��

��

��

��

��

C� � �� C� � ��C� � �� C�� C� � �� C�� C�� C� � �� C�� C� � �� C�� C��

C�� a�a� � a�� K�� C�� C�� K�� K�� C��

�� K��

��

�� C�

�� C�

�� C��

�� C��

�� C�

�� C��

�� C��

�� C�� K�� C�� K�� C�� K��

��

Figure �� CompProjectionIntegrals�F�� Compute the required integrals over a faceprojection� See Equations ��

��

Chapter �

Examples and Results

The ideas and theory presented in this thesis have been put into practice through

the prototype� Impulse� an experimental impulse�based dynamic simulator� Impulse is writ�

ten in ANSI C and runs on SGI and HP platforms� This chapter describes several examples

of simulations produced using Impulse� which fall into three categories� pure impulse�based

simulation� passive hybrid simulation� and controlled hybrid simulation� Snapshots of the

simulations are included� and most of the simulations described here can be viewed as

MPEG movies available from the Impulse web site�

http�%%www�cs�berkeley�edu%"mirtich%impulse�html

Many of these simulations were designed to test speci�c aspects of the impulse�based ap�

proach� Beyond these test cases� Impulse has been used as an engineering tool for real

applications in the parts feeding domain� These applications and some results are described

here� All of the simulations described here were run using the identical algorithms with

absolutely no tuning of parameters�

�� Pure impulse�based simulation

Pure impulse�based simulationmeans that all contact interaction is handled through

trains of collision impulses� there are no explicit constraints governing the relative motion of

di�erent bodies� All bodies in the simulation are either �xed in space� or execute piecewise

ballistic trajectories between collisions� This basic paradigm adeptly handles colliding and

transient contact� and can also model many types of continuous contact� including sliding�

��

Figure �� Snapshots from the dominos simulation�

rolling� and resting contact�

Dominos

The dominos simulation involves a row of seven dominos that fall over �Figure ��

While the dominos are falling� there are many simultaneous contacts between them� which

are modeled as individual collisions� The e�ects of the friction are evident in the simulation�

after all dominos have fallen� they come to rest on the supporting surface� Modeling friction

is critical to realism� as can be seen by comparing this simulation movie to a simulation

��

Figure �� Snapshots from the block drop simulation�

movie of frictionless dominos produced by the Simpact system �RK��

Block drop

The block drop simulation �Figure �� involves dropping two blocks onto a sup�

porting surface so that the blocks come to rest in a stack� The blocks visibly bounce several

times before settling into a resting state modeled with microcollisions� Performing this test

with more than two blocks proved troublesome� since collision impulses must be propagated

up and down the stack in order to keep blocks from penetrating� Stacks of objects subject to

��

Figure �� Snapshots from the block on ramp simulation�

gravitational forces are one type of system poorly modeled with impulse�based simulation�

the objects penetrate deeply into collision envelopes� and the collision detection and colli�

sion response computations become excessive� Impulse�based simulation can handle large

stacks which are in the process of settling� but once the system reaches a static state� it is

easier to model with constraints�

Block on ramp

The block on ramp simulation involves a block that slides down a ramp� under

conditions where the friction between the block and ramp is su�cient to stop the sliding

�Figure �� The ramp is at a �� angle with the horizontal� the block is launched with a

velocity along the ramp of �� cm%s� and the coe�cient of friction is �� This experiment

tests the accuracy of the friction model� since the block initially experiences sliding frictional

forces� and later experiences static friction forces when it comes to rest� Under Impulse� the

block reaches a resting state at very nearly the time and place predicted by theory� Graphs

of the block�s position and velocity are shown in Figure ��

��

Figure �� Snapshots from the chain of balls simulation�

��

Chain of balls

The chain of balls simulation illustrates the propagation of an impulse through a

chain of contacting bodies �Figure �� Four balls are initially placed a collision epsilon

apart� so that if any ball moves toward another� it will immediately penetrate the collision

envelope� After the striking ball hits the chain� the ball at the other end of the chain rolls

away� while the rest remain basically motionless� The moving ball rolls up a small ramp�

then back down to strike the chain� The second collision impulse is propagated less cleanly

because the balls have drifted slightly from their straight line con�guration� The drift is

primarily a result of the polyhedral models used for the spheres� which prevent the collisions

from being exactly central��

Under analytic approaches� there are two ways to propagate impulses through

chains of contacting bodies� One method computes the impulses one at a time� and the

other computes them simultaneously� The latter is computationally preferable in some situ�

ations� but can generate non�physical results in others� For instance� when the simultaneous

model is used for a ball chain simulation� the striking ball launches the entire chain into

motion� while it recoils backwards after impact �CS�� Bar�� How to choose between these

models is not clear� There is no ambiguity in the impulse�based model� which produces the

physically correct result�

Balls in dish

The balls in dish simulation �Figure �� involves dropping seven balls into a shal�

low dish to let them interact at will� After bouncing around for a few moments� the balls

come to rest with one ball at the center of the dish� surrounded symmetrically by the other

six� Although Impulse does not explicitly try to minimize the energy of the system� this is

the macroscopic result as collisions dissipate energy from the system�

In the �nal con�guration� there are � simultaneous contacts within the cluster of

balls� plus seven contacts between the balls and dish� which are all easily maintained with

microcollisions� These contacts are easier to model via collisions than those for the block drop

simulation �described above� for two reasons� First� there are not stacks of objects involved�

because of the shallow dish angle� the balls are not pushed into one another as strongly�

�A central collision is one in which the collision point is on the line segment joining the centers of massof the colliding bodies�

��

Figure �� Snapshots from the balls in dish simulation�

��

Second� collisions between spheres are central� and impart little angular acceleration to the

colliding bodies� As a result� the TOI bounds are much tighter� In contrast� when one

corner of a cube collides with a large �at surface� the angular acceleration of the cubes

cause the opposite corner to strike the surface soon afterward�

Coins

The coins simulation �Figure �� illustrates the wide variety of macroscopic con�

tact modes that are produced by Impulse�s collision model� In this simulation� four coins

are rolled toward the center of a plate� and four coins are simultaneously tossed into the

center of the plate� All of the coins collide in the middle� Through the course of the simu�

lation� many types of contact occur� including� colliding� sliding� prolonged rolling� resting

contact between di�erent coins� and resting contact between the coins and plate� Also

demonstrated is the familiar motion of a coin rolling along a path of increasing curvature�

eventually falling over and �rattling� to rest� The coin system is very representative of the

types of physical systems which are well suited to impulse�based simulation�

Pool break

Figure �� is a series of snapshots from the simulation of a pool break� Like the

chain of balls simulation� this simulation entails propagating collision impulses through

chains of contact� but with a more complex initial contact con�guration� there are initially

� contact points between the rack of � balls� The balls in the pool break simulation are

initially placed one collision epsilon apart� so that any motion on the part of one ball toward

another results in an immediate collision�

A high number of collision checks must be processed at the moment of the initial

break� Figure �� shows the number of collision checks per second of simulation time� After

the balls begin to separate� the collision check frequency quickly drops as higher TOI bounds

are achieved� The collision check rate stabilizes at approximately kHz after the break is

over� These collision checks are between the balls and the surface of the pool table� and are

needed to keep the balls from penetrating the table�

The collision model supports spin on the balls� By changing the initial angular

velocity of the cue ball� it can be made to draw back or eventually roll forward after the

initial impact with the rack� Other balls follow paths which deviate slightly from a straight

��

Figure �� Snapshots from the coins simulation�

��

Figure �� Snapshots from the pool break simulation�

��

time (s)

initial breakth

ousa

nds

of c

ollis

ions

per

sec

ond

Collision Rate

0.0 0.5 1.0 1.5 2.0

0

2

4

6

8

10

12

Figure �� The frequency of collision checks as a function of time during a pool break�Time refers to the time in the simulation� not the execution time required to compute thesimulation�

line� due to spin�

Bowling

A typical ten�pin bowling throw involves both prolonged and transient contact

modes� The ball is lofted onto the alley� and settles into continuous contact with the alley�

sliding at �rst� Eventually� contact with the alley causes the contact mode to shift from

sliding to rolling� as the relative tangential velocity at the contact between the ball and

alley is brought to zero� If the bowler imparts the proper initial translational and angular

velocities to the ball� the sliding phase will bring the ball toward the gutter� and the rolling

phase will cause it to hook back toward the center of the alley� Once the ball strikes the

pins� the contacts are predominantly transient� with many collision occurring� Snapshots

from such a bowling simulation are shown in Figure ��

��

Figure �� Snapshots from the bowling simulation�

��

A series of bowling experiments was performed to test Impulse�s ability to obtain

physically valid results� In the �rst batch of simulations� a straight ball was thrown down

the alley by launching the ball with zero angular velocity� and a center of mass velocity in

the �y direction �Figure �� trials were performed� keeping the initial ball velocity

0 +15−15 +30−30

position (cm)

pinline

x

y

Straight vs. Hooking Ball

felle

d pi

ns

pin line position (cm)

−15 −10 −5 0 +5 +10 +15

6.0

6.5

7.0

7.5

8.0

8.5

9.0

9.5

10.0straight

hooking

Figure �� Left� Bowling experiments were performed by recording the horizontal positionof the ball as it crossed the pin line� and the number of pins that were knocked down� Right�Results of the bowling experiments for straight and hooking balls�

constant� but varying the initial x�coordinate of the ball�s center of mass over a �� centimeter

window� In a second batch of � trials� the initial ball velocity conditions were altered

to produce a right�hander�s hooking ball� angular velocity of �� rad%s in the �y direction

and a linear velocity at an angle of �� from the y�axis� Accurate physical dimensions andmasses were used for all bodies� except for a slight approximation in the shape of the pins�

Figure �� shows the number of felled pins versus the ball position as it crossed

the pin line �ordinates are averaged over mm wide abscissa windows�� The hooking ball is

slightly better than the straight ball at most positions along the pin line� and is signi�cantly

��

better over a range between the head pin and rightmost second row pin �� to �� cm

on the pin line�� This agrees with the accepted wisdom that a right�handed bowler�s best

strategy is to throw a hooking ball between these two pins� The plots also illustrate the

dip in felled pins due to splits� when the ball hits the head pin dead on� Obtaining these

results analytically would be nearly impossible� but the chaotic bowling system is an ideal

application for impulse�based simulation� the evolution is collision intensive� with many

transient contacts between objects� and there is a gradual shift in contact mode between

the ball and the alley �bouncing to sliding to rolling��

Ball on spinning platter

Consider a ball rolling on a spinning platter� The nonholonomic model and the

Vakonomic model are two variational�based approaches to deriving the dynamics of this

system� which yield di�erent answers� Lewis and Murray computed simulations based on

each of these models� and then performed experiments to test their accuracy� using a large

steel ball on a spinning plexiglas surface �LM� �� They note that the general experimental

behavior was in good agreement with certain nonholonomic simulations� such as the one

pictured in Figure �� Also in the �gure is a snapshot from a similar experiment using

Impulse� Here� the nonholonomic contact between the ball and platter is modeled with

the usual collisions� The trajectories generated this way qualitatively match those of Lewis

and Murray� This example illustrates the versatility of the impulse�based method� with no

extra e�ort� collisions model the nonholonomic constraint between the ball and platter in a

physically accurate way�

Rattleback top

The rattleback top simulation demonstrates the ability of the impulse�basedmethod

to model nontrivial contact interaction� A rattleback top is an elongated solid with a convex

bottom� much like the hull of a ship� If spun in its preferred direction� the top will continue

to spin in that same direction until friction eventually stops it� If spun in the other direction�

however� instability soon occurs as the top begins to rock back and forth� and eventually

reverses its spinning direction� This anomalous behavior results from a slight asymmetry in

the top� either in the shape of the bottom or in the mass distribution� The key requirement

is that the principle axes of curvature at the contact point must be skewed in a particular

��

Figure �� Left� Trajectories of a ball on a spinning platter simulated by Lewis andMurray the physical experiments generally exhibited this behavior �LM�� reprinted withpermission�� Right� A snapshot of an analogous simulation using Impulse the spiral linesindicate the ball�s trajectory�

manner relative to the principle axes of inertia of the top�

A rattleback experiment was performed to test Impulse�s ability to predict this

phenomenon� First� two identical� balanced� ellipsoids were spun with the same initial

conditions to verify that they remained in phase when spun� The mass of each ellipsoid

was �� grams� Next� a � gram point�mass was attached to the surface of one of the

ellipsoids� in a position which skewed the principle axes of inertia in the prescribed way�

The experiment was redone� spinning the ellipsoids in a manner such that the rattleback

was spun in its non�preferred direction� After approximately two revolutions� the rattleback

top began rocking� and quickly reversed its spinning direction� as predicted� Snapshots from

the simulation are shown in Figure ��

Part feeder chute

The parter feeder chute simulation is one of a number of simulations related to part

feeding� Six parts are dropped into an inclined tray� funneled through a narrow opening

in the lower end� and land on the �at surface below� The parts are modeled after an

actual industrial part� a non�convex insulator cap� The parts slide along each other in the

chute� before spreading out as they fall onto the surface �Figure �� Experiments like

��

Figure �� Snapshots from the rattleback top simulation�

��

Figure �� Snapshots from the part feeder chute simulation�

��

Figure �� Snapshots from the exploding text simulation�

this can generate stochastic data related to a particular feeding operation� The data can

determine the feeder�s susceptibility to jamming� and its ability to singulate the parts� Such

an application� using Impulse� is described in �BC��

Exploding text

Physically based modeling is not only a valuable tool for simulation and virtual

prototyping� but also for generating animations� One example is the exploding text simula�

tion� The letters of the words �impulse�based simulation� are given random initial veloc�

��

ities� and set free to move through the space in an enclosed volume� with gravity turned

o�� Figure �� This example is a good test of the collision detection system� there are

� independently moving rigid bodies� many of them non�convex� It illustrates the use of

simulation to produce interesting visual e�ects� The animation is even more compelling

when played back in reverse� from random debris the words �impulse�based simulation�

materialize�

�� Passive hybrid simulation

As discussed in Chapter � pure impulse�based methods are not suitable for mod�

eling tightly constrained contact� such as that occurring at joints� Hybrid simulation allows

for certain contacts to be modeled as joints� greatly expanding the range of systems that

can be simulated� This section describes three examples that use passive hybrid simulation�

Passive means that there are no control systems acting� All external forces on multibodies

are due to gravity� collisions� or passive elements like springs and dampers�

See�saw

In the see�saw simulation� eight balls are placed in a shallow� funnel�like tray�

and roll toward a rectangular opening in the center of the tray� Blocking this opening

is a see�saw� a one link multibody� �xed to the tray through a revolute joint� The balls

eventually land on the see�saw� causing it to turn and rock as they pass through the opening

�Figure �� Contacts between di�erent balls� between the balls and the tray� and between

the balls and see�saw are all modeled with collisions�

Part sorter

The part sorter simulation is another application from the part feeding domain�

Six parts� of three di�erent types� slide down tracks arranged in a switchback fashion� Two

trap doors in the tracks allow parts with su�cient mass to fall through to the track below�

lighter parts slide across the trap doors� In this way� parts are sorted based on their mass

�Figure �� The trap doors are single link multibodies connected to the �xed tracks

through hinges� The hinges are equipped with springs to hold the doors closed� The spring

constants were chosen to let certain parts fall through� but let others pass over� Dampers

��

Figure �� Snapshots from the see�saw simulation�

��

Figure �� Snapshots from the part sorter simulation�

��

are attached at the joint to prevent excessive oscillation of the doors� Interaction between

the parts and the tracks and trap doors are modeled with collisions� the hinge contacts are

modeled with constraints�

The spring constants are adjusted so that individually one part type glides across

the �rst trap door� one part type falls through the �rst trap door but glides across the second�

and one part type falls through both trap doors� In practice� the simple sorter performs

poorly because its dimensions are too small� Parts can not be adequately separated� so the

trap doors do not have su�cient time to return to their resting state between parts� This

type of information would be very useful to the designer of a part sorter�

Triple pendulum

A �nal example of passive hybrid simulation is the triple pendulum simulation

�Figure �� Six balls are dropped into a shallow dish� Suspended above the dish is a

pendulum with three links and three revolute joints� The last link ends with a mallet� This

pendulum is positioned so that it can never hit the dish� but so that it can reach the balls in

the dish� It is not actuated� but dropped from a high potential energy state� so that gravity

drives it� As it swings� it collides with the balls� occasionally knocking one out of the dish�

The interactions between pairs of balls� between the balls and the dish� and between the

balls and the pendulum are modeled with collisions� Eventually� the pendulum comes to

rest in its minimum energy state� with the mallet resting between the three remaining balls�

The pendulum is not damped� its decrease in energy results entirely from collisions with

the balls�

�� Controlled hybrid simulation

Hybrid simulation is not restricted to passive systems� External controllers may

also be attached to objects� making the simulator a powerful tool for animation� as well as

simulation and testing� Three examples using Impulse are discussed below�

Furniture arrangement

The furniture arrangement simulation can be viewed as an animation application�

or as a design application with intelligent objects� This example only used rigid bodies a

��

Figure �� Snapshots from the triple pendulum simulation�

��

Figure �� Snapshots from the furniture arrangement simulation�

trivial case of multibodies and does not illustrate the full bene�ts of hybrid simulation�

But it is a good test of the control architecture� The moving objects are a table� six chairs�

and a wine bottle that move through two adjoining rooms �Figure �� The objects are

all controlled by applying external forces and torques via the control architecture� The table

controller steps through a sequence of eight user speci�ed con�gurations� applying forces

and torques to gravitate the table toward each con�guration� Damping forces and torques

are also applied� When a con�guration goal is attained� and the table velocity subsides� the

control system advances to the next goal�

��

Each chair is driven to gravitate toward a goal con�guration speci�ed relative to

the table� again using a PD controller� as the table moves� the chairs attempt to follow it�

Chair goal positions are also modi�ed by the presence of nearby walls� so that they �scoot

in� if the table moves close to a wall� Finally� the bottle has a simple control law which

repeatedly causes it to jump onto the table� if not there already� and gravitate toward

the center of the table� To a�ect a jump� the controller computes initial velocity values

which would arc the bottle in a parabolic trajectory onto the table� and writes these new

velocity values to the bottle�s state� as if it had experienced a collision� This illustrates the

�exibility available to the control designer� All object controllers also apply torques to keep

the objects upright�

A total of four controllers are required� one for the table� one for the bottle and

two for the chairs �one to control them to goal positions� and one to update these positions

in the presence of walls�� Each controller is a single C function� about one page long� The

controller rates vary from �$�� Hz�

Although real furniture is no more autonomous than a Luxo�lamp� this example

demonstrates the ease with which interesting motion can be produced from simple control

systems built on top of a dynamic simulator� The entertaining animation can be viewed

as the desired product of the simulation� From a more serious perspective� this example

illustrates the use of simulation as a design or layout tool� Consider the labor intensive

job of arranging furniture in a large� architectural virtual environment �as in �BS� �� The

task is made easier if the user only speci�es the positions of certain objects� like tables� and

other objects� like chairs� adjust themselves accordingly� The chairs are intelligent objects�

with useful behaviors�

Bicycle

The bicycle simulation involves an unmanned bicycle guided over an obstacle

course �Figure �� A controller designed to balance the bike at a speci�ed roll angle

was provided by Getz �Get�� In contrast to the bike controller employed by Hodgins�

et� al� �HWBO� �� Getz�s control law is nonlinear� employs a dynamic model of the bicycle�

accounts for the nonholonomic constraint between the wheels and ground� and does not

decouple the control of the pedals and handlebars� Hodgins� et� al� modeled a rider on the

bike� and built basic obstacle avoidance and clustering abilities into the control law� these

��

Figure �� Snapshots from the bicycle simulation�

��

are not present in Getz�s controller�

This example was chosen for two reasons� First� Getz�s control law relies on correct

modeling of both the bicycle dynamics and constraint forces with the road in order to

function properly� Second� the controller was developed independently� without regard

to any particular style of dynamic simulation� Prior to this experiment� the control law

had been implemented only as a MATLAB simulation of a bicycle on a �at surface� using

idealized motion constraints� and without collisions or obstacles�

The bicycle is a ��link multibody� a frame� a fork� and two wheels� geometry and

masses are based on a standard �� bike� The kinematic topology can be described as a

serial linkage� but the model was built as a tree linkage with the frame as the �oating base

link �Figure �� The controller takes as input the current steering angle� roll angle� rear

frame

fork

frontwheel

rearwheel

rearaxle

frontaxle

steeringcolumn

Figure �� The kinematic structure of the bicycle�

wheel velocity� and the derivatives of these quantities� Using a computed torque method�

it calculates desired accelerations of the pedals and handlebars to achieve the user de�ned

roll angle and forward velocity pro�les� The front wheel is not actuated and rolls freely�

Getz�s MATLAB code was translated in essentially unchanged form to a C function called

by Impulse�s control architecture� at a �� Hz rate� With this control law and the de�ned

pro�les� the bike is guided over an obstacle course which includes a pylon slalom� stair steps�

a wall� an ice patch� and a jump�

This example demonstrates how control strategies may be tested under more var�

ied conditions with an impulse�based simulator that supports general collision and contact

modeling� Hazards or other agents can be placed into the simulation without any modi�ca�

�

tion of the control law� a good controller causes the system to react in a natural way� The

graphs shown in Figures �� and �� show how well the controller worked in the presence

of the various disturbances�

Forward Velocity Profile

100 20 30 40

4

6

8

10

12

14

2

time (s)

forw

ard

velo

city

(m

/s)

desired

actual

pylon slalom

steps

wall crash

ice

jump

Figure �� The forward velocity pro�le as the bike traversed the obstacle course�

Creature war

The creature war simulation provides a detailed test of the multibody collision

response algorithms� and the control support architecture� In this test� a rover vehicle chases

crawling bugs� annihilating them by pushing them o� the edge of the world �Figure ��

Each bug has an ellipsoidal body and two elliptical legs �Figure �� The leg velocities

�

Roll Angle Profile

100 20 30 40

time (s)

−40

−30

−20

−10

0

+10

+20

+30

+40ro

ll an

gle

(deg

)

desired

actual

pylon slalom

steps

wall crashice

jump

Figure �� The roll angle pro�le as the bike traversed the obstacle course�

are regulated with individual PD controllers� Bugs control their headings by commanding

di�erent velocities to their legs� Bug behaviors are generated by a high level controller that

computes a desired heading using a potential �eld approach� They have a preference to

be in the center of the square world� and a stronger preference to avoid the rover when it

is nearby� Each bug�s high level controller uses the bug�s current position and that of the

rover to compute a new desired heading�

The rover is a ��link multibody �Figure �� The front wheels turn on axes

connected directly to the chassis� while two prismatic joints allow each rear wheel axis to

move vertically� relative to the chassis� These prismatic joints are equipped with shock

absorbers �springs and dampers� to smooth out bumps in the terrain� The rover uses four

PD controllers to regulate the velocity of its wheels� It changes its heading by commanding

��

Figure �� Snapshots from the creature war simulation�

�

rightleg

leftleg

body

Figure �� Left� Side view of a bug� Right� Kinematic tree for a bug� showing theindividual links and joints�

chassis

rightrearaxle

leftrearaxle

rightrearwheel

leftrearwheel

rightfrontwheel

leftfrontwheel

prismatic joint

revolute joint

Figure �� Left� Side view of the rover in normal and �jacked� con�gurations� Jackingallows the rover to push bugs rather than running over them� Right� Kinematic tree for therover� showing the individual links and joints�

di�erent desired velocities to the left and right side wheels� Two additional PD controllers

regulate the length of the rear prismatic pistons� By activating these controllers� the rover

can jack up its rear end and lower its front end in preparation to push bugs� Deactivating

these controllers makes the rover resume its normal driving posture� The rover�s behavior

is more complicated than that of the bugs� It repeatedly chooses one of the surviving bugs

as a victim� chases this bug down� assumes jacked position� and pushes the bug o� the

��

edge of world and into the abyss� It avoids driving itself over the edge� This behavior was

implemented via a state machine controller with �ve states� shown in Figure ��

spin forward

reverse

push

pick

prey ahead?

prey below?

always

prey killed?prey below?prey to side?

prey killed?

prey to side?prey near &dead ahead?

prey below?

roverbacking up?

Figure �� The high level state machine controller for the rover� There are �ve states�and the edges are labeled with the condition for the corresponding state transition�

The PD controller rates for this example vary from � to kHz� and the high

level control routines run at �� Hz� Two high level control routines are required for this

animation� one for the bugs and one for the rover� Both are C functions� about one page

long� The actual function for implementing the rover is shown in Figure �� The creature

war shows how interesting behaviors can emerge from physics plus simple control laws and

interactions between agents� The sequence of the �nal animation was quite unimagined

when the control laws were designed�

��

void cli rover�MB �mb�

fFILE �fp�

REAL Kp� Kd� Ki� period� desired�

REAL Kp�� Kd�� Ki�� period��

REAL Ks� Ls� damping�

fp � fopen��rover�ctrl�� r��

fscanf�fp� �lf lf lf lf lf�� Kp� Kd� Ki� period� roverVmax��

fscanf�fp� �lf lf lf lf�� Kp�� Kd�� Ki�� period��

fscanf�fp� �lf lf lf�� Ks� Ls� damping��

fclose�fp��

installSpring �mb��joint �� Ks� Ls��

installSpring �mb��joint �� Ks� Ls��

installDamper �mb��joint �� damping��

installDamper �mb��joint �� damping��

createPID �mb� �� period�� Kp�� Kd�� Ki��

createPID �mb� �� period�� Kp�� Kd�� Ki��

createPID �mb� �� period� Kp� Kd� Ki� ��




roverPrey � ��

roverState � ROVER FWD�

createGenCtrl �mb� �REAL� �� roverCtrl��

g

void roverCtrl�CTRL LAW �cl�

fMB �mb�

RB �chassis� �prey�

REAL vLeftDes� vRightDes� d� bearing� deltaV� k�

REAL deltaBear� spin� inc�

int dead� i�

VECT� goal� v�

JOINT �jnt�

SE� Tib�

static char �states� � � ffwd � spin� push� rev � pickg�

mb � cl�body�

chassis � mb�link��

prey � MBtable�CRAWLER � roverPrey��link��

se�inv��chassis�Tbi� �Tib��

if �roverPrey �� NUM CRAWLERS� fv�� v� � � �� v�� gelse vectCopy�prey�Tbi�d� v��

dead � �v��

se�xformPoint��Tib� v� goal��

d � sqrt�SQR�goal�� SQR�goal� ��

vectNormalize�goal� goal��

inc � �� acos�goal�� RAD TO DEG�

bearing � atan��goal� �� goal�� RAD TO DEG�

deltaBear � fabs�bearing��

se�xformVect��chassis�Tbi� chassis�w� v��

spin � v��

switch�roverState� f

case ROVER FWD�

deltaV � roverVmax � �� bearing �� spin��

vRightDes � roverVmax � deltaV�

vLeftDes � roverVmax deltaV�

if �dead� roverState � ROVER PICK�

else if �inc � �� roverState � ROVER REV�

else if �deltaBear � �� roverState � ROVER SPIN�

else if �d � �� deltaBear � �� fsetPID �mb� � ��

setPID �mb� ��

roverState � ROVER PUSH�

gbreak�

case ROVER SPIN�

vRightDes � roverVmax � �� bearing �� spin��

vLeftDes � vRightDes�

if �inc � �� roverState � ROVER REV�

else if �deltaBear � �� roverState � ROVER FWD�

break�

case ROVER PUSH�

if �roverPrey �� NUM CRAWLERS� froverState � ROVER REV� break�gdeltaV � roverVmax � �� bearing �� spin��

vRightDes � roverVmax � deltaV�

vLeftDes � roverVmax deltaV�

vectCopy�prey�v� v��

k � �SQR�v�� SQR�v� ��

displacePoint�prey�Tbi�d� v� k � vectNorm�v�� v��

if �v�� v�� v� � � �� v� � � ��

�� deltaBear � �� inc � �� roverState � ROVER REV�

break�

case ROVER REV�

vLeftDes � vRightDes � �� roverVmax�

se�xformVect��Tib� chassis�v� v��

if �v�� inc � �� roverState � ROVER PICK�

break�

case ROVER PICK�

vLeftDes � vRightDes � ��

if �dead� roverPrey��

disablePID �mb� ��

disablePID �mb� ��

roverState � ROVER FWD�

break�

g

vLeftDes �� ROVER WHEEL RAD�

vRightDes �� ROVER WHEEL RAD�

setPID �mb� �� vLeftDes��

setPID �mb� �� vLeftDes��

setPID �mb� �� vRightDes��

setPID �mb� �� vRightDes��

g

Figure �� The C functions that implement the rover�s high level control� Func�tion cli rover is called once to instantiate controllers and set initial values� FunctionroverCtrl implements the high level state machine depicted in Figure �� It is called bythe simulator scheduler every �� seconds� Calls to the control library are in underlinedand in bold font�

�� Execution times

Table �� summarizes the execution times needed to simulate the examples de�

scribed above� Simulation time is the length of time which passed in the simulation� com�

��

simulation computationtime time

example �sec� �sec % min�sec� slowdown

pure impulse�based simulation

dominos �� block drop �� block on ramp �� chain of balls � �� balls in dish �� coins �� pool break �� bowling �� ball on spinning platter �� rattleback top �� part feeder chute �� exploding text ��

passive hybrid simulation

see�saw �� part sorter � �� triple pendulum ��

controlled hybrid simulation

furniture arranging �� bicycle �� creature war ��

Table �� Simulation times for examples�

putation time is the actual time needed to compute the simulation� and slowdown is the

ratio of the latter to the former �a slowdown of �� corresponds to real time simulation��

Simulations were performed on an SGI Indigo II with �� MHz R�� processor� Execution

times shown are the average taken over several runs� with graphics turned o�� For the pure

impulse�based simulation examples� running times were generally less than �ve times slower

than real time� for a couple of the examples� real time simulation was achieved�

Impulse ran slower on the hybrid examples� generally one to two orders of mag�

nitude slower than real time� The slowdown in the part sorter and furniture arranging

examples was primarily due to collision detection involving several highly nonconvex bod�

ies� For the triple pendulum� bicycle� and creature war examples� the bulk of the processing

time was spent computing nontrivial multibody forward dynamics� The see�saw example�

which had only convex objects and a very simple multibody� ran relatively quickly�

��

Table �� details how the processing time was spent for the three controlled hybrid

simulation examples� In all cases� only a very small fraction of time was spent executing

forward collision collision controlexample arrangement dynamics detection response system

furniture �� bicycle �� creature war ��

Table �� How the simulation processing time is spent� All �gures are percentages of totalprocessing time�

control laws� Collision response� the time spent computing and propagating multibody col�

lision impulses� is also fairly small� As mentioned before� the nonconvex collision detection

uses most of the processing time in the furniture arranging example� and computing for�

ward dynamics uses most of the time in the other examples� Signi�cant speed up is possible

by using commercial packages that precompile the dynamics computation for greater e��

ciency �as in �HWBO� �� Also� under impulse�based simulation� the dynamics of separate

multibodies may be computed independently� so parallelization is trivial�

For rigid body and multibody forward dynamics integration� as well as for collision

integration� Impulse uses a standard fourth order Runge�Kutta integrator� with adaptive

step size �PTVF�� To take one step forward� the integrator calls the derivative evaluation

routine �� times� This can be quite expensive compared to� say� a non�adaptive Euler

integration step� which only requires one derivative evaluation per step� An impulse�based

simulator tends to call the dynamics integrator with fairly small intervals� since the time

between collisions is short� and with these small intervals the accuracy of a simpler and

faster integrator might be su�cient� Further study is needed�

�� Estimating pose statistics

The initial problem that sparked the development of Impulse was the simulation

of vibrational parts feeders� These machines� commonly used in automated assembly� use

high frequency� small amplitude vibratory motion to move and orient parts� which are

often subsequently handled by a manipulator� More detailed descriptions of these types of

feeders may be found in �BPM�� Vibrational parts feeders naturally led to the idea of

modeling contact with impulses� since the contact modes between the part and feeder are

��

highly transient� Many non�vibrational feeders are natural applications for impulse�based

simulation� because the parts still collide frequently with the feeder� and collisions have a

dominant e�ect on the dynamics of the parts�

Impulse was used to predict the probabilities of rigid parts landing in their various

stable poses� when dropped onto a �at surface� The problem has speci�c relevance to the

Adept Robotics �exible feeder� but would also be useful for other feeders� Results from the

simulator and from quasi�static methods were compared to data from physical experiments

for a number of real industrial parts� these results are included below� More details of the

methods used to estimate pose statistics are in �MZG��

�� Background

In contrast to �xed assembly lines� �exible assembly lines can be rapidly recon�

�gured to handle new parts� This can dramatically reduce the time needed to bring new

products to market and permit the cost of assembly lines to be amortized over multiple

products� Part feeders� which singulate and orient the parts prior to packing and insertion�

are critical components of the assembly line and one of the biggest obstacles to �exible

assembly� Currently� the design of part feeders is a black art that is responsible for up to

�� of the cost and �� of workcell failures �NW�� BPM��

Carlisle et� al� �RG�� proposed a �exible part feeding system that combines ma�

chine vision with a high�speed robot arm� In contrast to custom�designed hardware such

as the bowl feeder� only software is changed when a new part is to be fed� The idea is that

a collection of like parts are randomly scattered on a �at worktable by the force of gravity�

An overhead vision system determines the pose �position and orientation� of each part� The

robot arm then picks up each part and moves it into a desired �nal pose as illustrated in

Figure �� A method of estimating the throughput of this feeder is in �GC� �� but the

method requires knowledge about the distribution of poses� particularly orientation� when

the parts are scattered on the �at worktable� More formally� the problem to be solved is�

Problem � Estimating Pose Statistics EPS�� For a rigid polyhedral part X with

known center of mass and inertia tensor� denote the n faces of its convex hull H by F��

Fn� Assuming X is repeatedly dropped onto a at surface from some known distribution of

initial poses� compute the values p�� pn� where pi is the probability that X reaches a

�nal resting state with Fi down �against the surface��

��

Figure �� A exible part feeder using machine vision� a high�speed robot arm� and pivotinggripper� This illustration shows the system feeding car�stereo pushbuttons�

Additional assumptions are that the workspace is �at and much larger than X� and that X

does not collide with other parts�

Predicting the pose distribution of rigid parts dropped onto a �at surface is im�

portant in evaluating part designs for assembly� in estimating feeder throughput� and in

determining how many robots and assembly lines are required to meet speci�ed production

rates� This work in estimating pose statistics complements other ongoing work in auto�

mated assembly� Rao� Kriegman� and Goldberg have studied the use of a pivoting gripper

for Adept�s �exible feeder� they give an O�m�n logn� algorithm to generate pivot grasps

for a part with n faces and m stable con�gurations �RKG� �� Christiansen� Edwards� and

�

Coello Coello give a genetic algorithm for designing e�cient part feeders from component

gates �CEC�� Their algorithm takes pose statistics� such as the ones computable by Im�

pulse� as input�

�� Quasi�static algorithms

For completeness� Problem �� was also solved using two quasi�static algorithms�

The �rst algorithm was proposed by Wiegley et� al� �WRG�� and is an improvement on the

early work done by Boothroyd �BPM�� Motion of the part is assumed to be quasi�static�

inertia and velocity are neglected�

The idea of the algorithm is illustrated in Figure �� After computing the part�s

β1

β2

F

c

β0

dc0

v0

v1

v2

Fπ

2δ

dc1

d01

Hull H

Figure �� Estimating pose statistics using a sphere projection method�

convex hull H� the facets of H are projected onto a unit sphere centered at the center of

�

mass c� If F� is the projection of face F � then the ratio of the area of F� to the total surface

area of the sphere gives the probability that the part will land on face F under quasi�static

conditions� if its initial pose is uniformly distributed over SO��

Assuming triangular faces� the ratio in question is given by

A ��

��

where the �i are the interior angles of F�� the �i can be computed from standard spherical

trigonometry�

This procedure results in an initial estimate of each pi� To treat faces of H that

are statically instable� the center of mass is projected onto the plane of each face Fi� If the

projected point lies outside face Fi� gravity will cause the part to topple over to adjacent

face Fj � In this case pi is added to pj � and pi is set to zero� The algorithm to compute the

pose statistics is O�n�� in the number of polyhedral edges�

Since the quasi�static analysis does not model dynamic disturbances� it often over�

estimates the probability of landing on a facet that is stable but easily dislodged by small

vibration� Consider two facets of the part�s convex hull� Fi and Fj � and the bounding edge e

between them� Let g be the gravity vector from the part�s center of mass� If g intersects Fi

when in contact� the part will remain on facet Fi� under the quasi�static model� However�

dynamic energy may rotate across edge e when g points inside facet Fi but is close to edge

e �Figure �� A second quasi�static method� called the perturbed quasi�static method�

computes a more accurate likelihood that the part might rotate from facet Fi to facet Fj �

and propagates probabilities accordingly �see �MZG�� for details�� In all the experiments�

the cone half�angle was �� although the right way to choose this half�angle measure is not

completely clear�

�� Dynamic simulation

The EPS problem was attacked using full dynamic simulation of the parts being

dropped onto a surface� using Impulse� For dynamic simulation� many parameters not used

in the quasi�static and perturbed quasi�static algorithms are important� The coe�cients

of friction and restitution were both estimated to be �� The feeder con�guration is also

�Recently� Issa Nessnas of Adept has experimentally measured these coe�cients� He estimated thecoe�cient of friction to be �� and the coe�cient of restitution to be �� although there was alarge amount of noise in these results�

��

C

g

e

ε

Fi

Fj

Figure �� The perturbed quasi�static method uses a cone projected down from the centerof mass� to estimate the likelihood that the part will topple from face Fi to face Fj� Thislikelihood is used to propagate some of the probability from pi to pj�

important� Adept�s �exible feeder system dumps the part from an upper belt onto a lower

belt� where the parts are examined by the vision system� and then picked and placed by

one or more manipulators �Figure �� For dynamic simulation� the height of the drop

was estimated at �� cm� The horizontal velocity of the parts as they leave the upper belt

was estimated at �� cm%s�

The initial orientation of the parts poses a problem since the parts are in a stable

resting state on the upper belt� before being dropped onto the lower one� Thus� the initial

distribution of orientation is not uniform� but similar to the �nal �initially unknown� dis�

tribution� To circumvent this problem� the initial orientations for the �rst � drops were

chosen randomly� assuming a uniform distribution over SO�� This bootstraps the process

with a preliminary �nal pose distribution� For all remaining drops� the initial �upper belt�

poses are chosen from the current distribution of �nal �lower belt� poses� so that the results

�

Figure �� Top� CAD models of the four parts used in the experiments� From left to right�insulator cap� large white� rectangular black� and square black stereo buttons� Bottom�Photographs of the rectangular black stereo button in its seven stable states�

of the drop tests are continually fed back to determine initial conditions� A slight pertur�

bation �a rotation of up to �� degrees about a randomly chosen axis� is also applied to the

initial pose to introduce noise into the system due to belt vibration�

�� Experimental results and discussion

The three algorithms described were applied to four test parts� The test parts

were all small� plastic� rigid pieces� of the type typically used in automated assembly �Fig�

ure �� Part (� is an insulator cap purchased at a local hardware store� Parts (�

(� and (� are pushbuttons designed for a commercial car stereo system� Geometric mod�

els of each part were constructed by measuring the parts with a ruler� Centers of mass

and moments of inertia for the parts were computed automatically by Impulse� using the

techniques of Chapter �� As a control� physical experiments were performed by repeatedly

running several hundreds of samples of each test part through Adept�s �exible feeder sys�

tem �Figure �� the �nal resting poses were recorded by a human observer� Tables ��

through �� show the results� All quantities in the tables are percentages�

��

The error percentages included in the tables indicate the overall performance of

each algorithm for each sample part� They are computed as the average deviation of the

algorithm�s predictions from the physical test percentage� weighted by the frequency with

which that state actually occurs� Let p�� pn represent the probability of each of n states�

as measured in the physical test� Let a�� an represent the corresponding probabilities

computed by one of the algorithms� The error percentage for that algorithm is given by

e � ��nXi��

pi jai � pij� ��

Quasi� P�turbed Dynamic PhysicalPose Static Q$S Sim� Tests

� ��

error �� $

�� trials performed in physical experiments�

Table �� Orange insulator cap data�


� ��

error �� $

� trials performed in physical experiments�

Table �� White stereo button data�

The quasi�static and perturbed quasi�static algorithms are extremely fast� requir�

ing less than a second of computation time for parts with � facets� and the perturbed

quasi�static algorithm�s predictions are consistently more accurate than those of the quasi�

static one� Dynamic simulation is slower� For each part� �� drops were simulated� taking

approximately � minutes per part� however greater accuracy is also achieved� The dy�

namic simulation algorithm is the most accurate for all sample parts� except the insulator

��


� ��

error �� $


Table �� Rectangular black stereo button data�


� ��

error �� $


Table �� Square black stereo button data�

cap �Table �� for which the perturbed quasi�static algorithm slightly outperforms it�

The dynamic simulation algorithm�s prediction accuracy is also the most consistent� the

composite error is less than � in all cases�

In an interactive setting� where a designer is perhaps editing the CAD model of a

part in order to improve feeder throughput� the perturbed quasi�static algorithm is clearly

the best choice� The designer need only wait seconds to see how changing a part�s CAD

model alters the pose distribution and feeder throughput� The dynamic simulation algo�

��

rithm is useful for obtaining a more accurate estimate once the design has been determined�

or for analyzing the e�ects of more subtle design changes� It models several factors� that

aren�t considered by the standard and perturbed quasi�static algorithms� including� friction�

collisions with energy loss� mass moments of inertia� height of drop� and initial conditions of

the part prior to drop� To study the e�ects of varying these parameters� dynamic simulation

is appropriate�

Our simulation experiments involved �� drops tests� Often� fewer trials may be

su�cient� reducing the computational cost of this method� Suppose the true �unknown�

probability that a part lands in a particular pose is p� The number of times the part lands

in this pose over n trials is a binomial random variable� which may often be approximated

by a normal distribution�� A con�dence interval statement is of the form� �p lies within the

range �� with �� certainty�� Here� � is the probability estimate obtainedfrom the n trials� � is the allowable error tolerance� and � is the level of the statistical test�

Given � and �� one can bound the number of trials necessary by

n �)��

� �

��

where )�x� is the cumulative normal distribution function� For example� to pinpoint the

probability of a particular �nal pose to within �� with �� certainty� � � �� and

� � �� From �� trials are su�cient� See �Dev�� for more information�

�� Other part feeding experiments

Impulse is currently being used for several other studies in part feeding� Impulse�

based simulation is a good match to this domain� because the interactions between feeder

and parts are often collision intensive� or of a vibratory nature� An accurate friction model

is also required for accurately predicting the behavior of many feeders�

Berkowitz and Canny have developed a tool for designing industrial part feeders

based on stochastic data from dynamic simulation �BC�� In many feeders� parts are pro�

pelled along a feeder track� through vibration� conveyor belt� or gravity� Various protrusions

and cut�outs �also called gates� along the path reorient the passing parts� or reject them�

parts passing successfully through all of the gates are correctly oriented� Feeder designs

can be parameterized by the positions� angles� and dimensions of the gates� Berkowitz�s

�A common rule of thumb is that the normal approximation is valid if the numbers of successes andfailures during the trial series both exceed �ve�

��

and Canny�s tool allows a designer to search through the space of possible feeder designs�

searching for the optimal one with respect to a metric the designer speci�es� Impulse is used

to compute the trajectories of the parts through the feeders� the impulse�based approach is

very e�cient for this type of simulation�

Another way to apply this idea would be for the algorithm of Christiansen et� al�

They describe an approach to near�optimal design of vibratory bowl feeders� using a genetic

algorithm to search through the space of all gate sequences �CEC�� For each possible gate�

their algorithm requires as input a stochastic matrix that characterizes the gate operation�

element �i� j� of the matrix is the probability that a part entering a gate in state i will exit

in state j� Impulse�based simulation can easily gather this type of information� A similar

application is the algorithm of Wiegley et� al� that generates a sequence of passive fences

to align parts moving on a conveyor belt� In �WGPB�� they analyze their feeders using a

frictionless� quasi�static model�

A completely di�erent type of part feeding involves micro�actuated motion arrays�

a type of micro electro�mechanical �MEM� device� These devices also have applications in

video displays� B�ohringer et� al� have fabricated a motion array comprising several thou�

sand torsional resonators distributed over a surface of a few square centimeters �BDMM��

however di�culties in producing and testing these motion arrays have hindered experimen�

tal results� Reznik and Brown are using Impulse to analyze motion arrays based on the

model of B�ohringer et� al�� Their study focuses on determining feed rates for the arrays� and

how the performance is a�ected by variations in certain design parameters and operating

conditions �RB�� Results such as these might increase understanding of these devices and

suggest future improvements� Figure �� depicts one of Reznik�s and Brown�s experiments�

Electrostatic stepper motors are another type of MEM device that could be improved with

results from simulation� Since their operation depends on transient contact between com�

ponents� analytic results are di�cult to derive� Zhuang is using Impulse to analyze MEM

stepper motors and the e�ects of varying design parameters �Zhu��

Acknowledgments Thanks to Neil Getz for providing his bicycle controller and explain�

ing its operation� and to Je� Wendlandt for many enlightening discussions� Section ��

was largely extracted from �MZG�� thanks to Randy Brost and Bruce Shimano for use�

ful feedback on the initial draft of that paper� Versions of the quasi�static algorithm of

�

Figure �� A snapshot taken from Impulse during one of the MEM motion array exper�iments described in �RB�� Here� two di�erent resonator tilings are compared to determinepossible di�erences in feed rates �reprinted with permission��

Section �� were implemented at USC� Wiegley in �� Zheng Yeh in �� and Yan

Zhuang in �� The convex hull routine is due to Ioannis Emiris and John Canny� Special

thanks to Stephen Chenney for the renderings of the controlled hybrid simulations� shown

in Figures �� and ��

�

Chapter �

Future Work

This thesis has introduced impulse�based simulation� a paradigm for dynamic sim�

ulation of rigid body and articulated body systems� The approach� pioneered by Hahn� has

been extended� re�ned� and implemented with the prototype simulator Impulse� Tests with

Impulse show that the method is capable of interactive simulation speeds for moderately

complex physical systems� and that the physical accuracy of the method is suitable for many

applications� This �nal chapter mentions some challenges that remain and some avenues

for future research�

�� Paradigm switching

Hybrid simulation� as presented in this thesis� is a somewhat limited blending of

constraint� and impulse�based simulation techniques� Constraints hold together the joints

of a multibody� while an impulse�based approach is used for the contacts between di�erent

multibodies� This expands the domain of what can be done using impulse�based approaches�

but fails to address some basic de�ciencies in the method� What can be done in the case

of a stack of books on a table� Here� a constrained model for contact between bodies is

appropriate� but the presented hybrid methods do not employ one� One way to solve this

problem would be to detect the onset of static contact between bodies� This might be done

by observing that a high number of low velocity impacts are occurring between adjacent

books in the stack� At this point� a paradigm shift could be made to model the contact

interactions between books with constraint forces�

The transition need not be a sharp one� A contact force f � that is less than the

true contact force f might initially be applied between the books� the slack would be picked

up by collisions� Even the force f � would reduce the number of collisions and increase

the simulation speed� As f � approaches f � the number of collisions decreases to zero�

and pure constraint�based modeling is achieved� A gradual paradigm shift like this seems

less susceptible to anomalous behavior that might result from discontinuities in simulation

strategy� This impulse� to constraint�based shift could naturally and e�ciently handle a

very common case� objects coming into initial contact through collision� followed by some

period of low impact collisions as the objects settle into continuous contact�

Constraint� to impulse�based paradigm shifts are also appropriate� Consider� an

arrangement of bowling pins in resting contact with the ground until struck by the inevitable

ball� a stack of blocks at rest until the bottom one is suddenly removed� or a skateboard

in steady rolling contact with the ground until a speed bump sends it airborne� In all

of these situations� a switch from a constraint�based to an impulse�based approach seems

appropriate in preparation for periods of transient� collision intensive contact� An abrupt

change in the normal force applied between a pair of bodies might be the triggering event

for this shift from constraint�based to impulse�based modeling of the contact�

We believe that a fuller blending of the impulse� and constraint�based approaches

through paradigm shifts is a very fruitful area for future research� By applying each tech�

nique in the cases to which it is well suited� the range of physical systems that can be

e�ciently simulated can be signi�cantly increased�

�� Collision detection issues

Of major in�uence in the design of Impulse�s narrow�phase collision detection sys�

tem was the fact that the Lin�Canny algorithm does not operate correctly on intersecting

polyhedra� This motivated the one�sided approach to �nding the moment of collision� In

the more common two�sided approach� the simulation regularly proceeds past the moment

of collision� at which point the collision detection system detects penetration and the sim�

ulation is backed up� Binary search or similar techniques can pinpoint the moment of

collision to within a prescribed spatial or temporal tolerance� A disadvantage to the two�

sided approach is that it is susceptible to missing collisions� But the two�sided approach is

likely to converge to the collision time faster than the one�sided approach� which requires

conservative lower bounds on the time of impact� The two�sided approach is certainly easier

to implement when the objects have complex dynamics� making conservative bounds hard

to obtain� If a way to avoid missing collisions was devised� a two�sided approach might be

more e�cient for impulse�based simulation�

Parallelization is an important advantage of the impulse�based method� but it

extends only to the dynamic integration phase� Parallel collision detection algorithms could

greatly improve performance on parallel machines� Using the single collision heap scheme

described in this thesis� whenever a collision check occurs between any pair of bodies� the

dynamic integration of every body stops� This is clearly ine�cient� If the bodies were

partitioned into separate heaps� based on spatial locality� the dynamic integration of a

body would not be halted by irrelevant collision check events� the individual heaps could

be parceled among the di�erent processors� This partitioning is advantageous in single

processor settings as well� since certain bodies could be evolved further in time for each call

to the integrator� resulting in larger integration steps and faster simulation� The situation

would be as depicted in Figure �� All body integrations need not stop for every collision

body n

body 2

time (s)0 1/30 2/30 3/30

body i

tc

body 1

Figure �� If the dynamic integrations of every body are not required to stop for everycollision check� then the evolution of the states does not have to proceed in lockstep�

check� so the evolution of the bodies does not proceed in lockstep� The bars in the �gure

indicate the time to which each body�s state has been evolved� for example� body � has been

evolved to almost �� seconds� Whenever all bodies have been evolved to the next frame

refresh time� the screen is redrawn with their positions at that time� Some consistency

�

problems must be addressed� Referring to the �gure� if when body i is integrated to time

tc a possible collision with body � is detected� then the states of both bodies must be reset

to the last point at which they were veri�ed as separate� The work performed to integrate

body � to its current state would be wasted in this case� Still� this scheme would likely

result in a signi�cant performance improvement on average�

�� Interpreted control and object encapsulation

One powerful application of simulation is the design and testing of control systems�

Experience with Impulse has demonstrated that many potential problems with a control

scheme can be detected and corrected through simulation alone� A disadvantage of Impulse�s

current architecture is that control laws must be speci�ed in C� compiled� and linked with

the rest of the simulator code before they can be tested� This delay in the design�test

loop quickly becomes a burden� Even more serious is the portability problem created when

di�erent versions of Impulse have di�erent compiled control laws� A better approach would

be to specify high level control laws in an interpreted language� such as Scheme or Java

�GM� �� At a control update event� the necessary dynamic states would be passed to the

interpreter� which would execute the control law� and pass back control forces and torques�

More general state information� such as that arising during planning� could be kept in static

data structures on the interpreted side� For e�ciency� the low level controllers� and springs

and dampers� could still be implemented as compiled C routines called by the high level

controller� The interpreter provides a completely general language for specifying control

laws� One concern is the impact on execution time� although from Table �� the outlook

appears good� The table indicates that a very small fraction of the time is spent executing

control laws� Of this fraction� most of the time is devoted to executing low level control

functions� such as the PD update routine� For these examples� a reasonable estimate is that

less than �� of the time is spent executing user�de�ned control routines� If the interpreted

versions of these routines ran ten times slower than the compiled versions� execution time

would only increase ��

Once the control laws are being written in an interpreted language� it makes sense

to combine the entire object speci�cation in such a language� Rather than have the geom�

etry of an object speci�ed in some �xed format data �le� it could be speci�ed by a Java

program that executed calls to compiled library routines� like createVertex�x�y�z� or

createEdge�v��v�� Such calls could be collected in a list� or put inside loops so that an

interpreted program could algorithmically build up the geometry� The line between program

and data becomes blurred� there is simply a paragraph of code that generates the geometry

somehow� Other important properties for dynamic simulation could also be speci�ed by the

same program� such as masses or densities of objects� coe�cients of friction� and so on� Even

the rendering model could be speci�ed this way� although there may be more reason to keep

that model in a �xed� standardized format for e�ciency purposes� Leveraging o� the lan�

guage interpreter� this object speci�cation format is completely general and portable� Users

at di�erent sites could each create full descriptions of creatures� including their geometries�

physical properties� rendering models� and behaviors implemented through control systems�

Scripts describing these creatures could be submitted to a common simulation engine� the

creatures would inhabit a common world and be free to interact with each other� Graphical

results could be sent back to the remote sites� where the creature designs could be evaluated

and re�ned �Figure ��

Java

attachSpring("knee");createPID(...);

case LIFTOFF: if (dist("foot", "ground") > eps) state = FLIGHT;

Behavior

createVertex("v1");createVertex("v2");

createEdge("v1", "v2");

Geometry

Physics

Graphics

simulator

Java

setDensity(5.0);

attachPointMass(v5, 100);

setMaterial(STEEL); pointer to VRML/Inventor

Java objectdescription

graphicaloutput

Figure �� Interpreted object speci�cation allows generality and portability�

Taking the logical next step� why make the designer program at all� Consider

building a creature to inhabit a virtual world� A number of systems have been proposed

that allow a user to interactively snap together kinematic structures subject to dynamic

�

constraints �BB�� Gle�� IC�� WGW�� One could envision such a system� augmented

with widgets representing various types of low level controllers that could be attached to

the creature� Various types of sensors� for example� compasses or joint encoders� could

be attached as well� The sensors could be very simple� like encoders that return a joint

position� or quite complex� like a vision sensor that returns a binary image from a camera

mounted on the front of the creature� After the geometry and sensing and control systems

were constructed� a high level control system could be speci�ed� again using a graphical

user interface� One paradigm would be interactive design of state machine control systems�

where the actions to be taken at each state and the conditions for state transitions are built

up from primitives based on the control and sensing systems that are on board �Figure ��

Finally� at the push of a button� a complete description of the creature� in the form of a Java

chase

spinleft

push

reverse

activaterear pistons

right wheelsfull fwd

left wheelsfull fwd

push

gyro compass

N

bearingrangefinder vision

P D

computedtorque

P I D

spring damper pistonlinkrevolute

joint jointprismatic spherical

joint motor

encoder

Figure �� A conception of a creature design environment� Through a graphical user inter�face� a designer completely describes the geometry� physics� control and sensing� renderingmodel� and behavior for a creature� A script describing the creature is automatically gener�ated and ready for use in a physical simulation�

program� is automatically generated� and ready for deployment into a virtual world� Many

details need to be worked out� but such a system is entirely possible and would greatly

�

accelerate the creation of interesting virtual worlds populated by intelligent creatures�

�� Physical simulation for animation and VR

In recent years� the computer graphics community has increasingly turned to phys�

ical models for generating realistic animations� Several systems have been developed which

model a particular type of creature� often with intelligence and goals� interacting within

its environment� just a few examples are in �Rey�� MZ�� RH�� Mil�� NM�� TT��

Sim�� HWBO� �� The animations these systems produce are extremely compelling� and

one can imagine the richness of a virtual world populated by these sorts of autonomous

creatures� operating under independent control systems and interacting in complex ways�

This has yet to happen� however� largely because physically based animation systems have

each been specialized for particular types of creatures and environments� Miller�s system

is for animating snakes� Tu�s and Terzopoulos�s system is for �sh� Raibert�s and Hodgins�s

system only models contact with a ground plane� and so on�

This is in no way a criticism of these systems� since traditionally such restrictions

have been necessary to obtain reasonable speeds� This condition is changing however� An

impulse�based simulator can e�ciently and accurately simulate three�dimensional physical

systems comprising rigid and articulated bodies� a fairly general class� Fast and robust

general collision detection and physically accurate collision response facilitate realistic in�

teractions among di�erent components or creatures� no matter how they come together�

The designer of a creature does not have to worry about the variety of contact interactions

it might encounter in a virtual world� correct contact forces will be computed� The de�

signer can then consider the behavior system at a higher level� as one would if designing the

controller for a robot in the real world� Many questions and challenges remain� however�

Impulse has shed some light on the possibilities that a general purpose dynamic simulator

could create� Such a simulator is needed before the work of so many can be combined in a

virtual environment inhabited by diverse� autonomous agents�

�

Appendix A

Mathematical Preliminaries

A�� Vectors� matrices� and frames

In this text� a frame designates a right�handed� three�dimensional coordinate sys�

tem� Frames are usually designated by scripted uppercase letters� such as F � The right�handed requirement means that the canonical basis vectors satisfy

i� j � k�

The components of a vector are always expressed relative to the basis vectors of some such

frame� which may be implied or stated explicitly�

It is often necessary to convert the coordinates of a vector expressed in one frame to

coordinates expressed in another frame� Let vF and vG be �� column vectors comprisingthe coordinates of vector v relative to frames F and G� respectively� Let rx� ry� and rz bethe � � column vector comprising the coordinates of frame F �s i� j� and k basis vectors

relative to frame G� respectively� Then�

vG �

�� rx ry rz

�� z �

R

vF �

The � matrix R is called a change�of�basis matrix� Under the assumption that the twobases are orthonormal� right�handed bases� the change�of�basis is always a rotation matrix�

This is an orthogonal matrix� meaning that its columns are all of unit length and mutually

�

perpendicular� The same can be said of its rows� Furthermore� its determinant is �� and

it possesses the extremely useful property that

R�� RT �

Matrices must also be transformed to operate on vectors speci�ed in di�erent

bases� For example� Let � and L be the coordinates of the rigid body�s angular velocity

and angular momentum� relative to a frame attached to the body� These coordinate vectors

are related to each other through the � mass matrix� I�

L � I��

Suppose now that the body�s angular velocity and angular momentum coordinate vectors

are expressed relative to a di�erent frame� �xed in space� Call these �xed frame vectors ��

and L�� These two vectors can still be related through a matrix I�� given by

I� � RIRT �

where R is the rotation matrix taking coordinate vectors in the body frame to coordinate

vectors in the �xed frame� The matrix I� represents the same linear mapping as I does� it

is just coordinatized relative to a di�erent basis� Justi�cation for the transformation above�

can be seen from

L� � I��

�RIRT

��R��

� RI�

� RL�

Thus� the angular momentum vector computed in the �xed frame using I� and �� is the

same as that computed in the body frame using I and �� but with the coordinates expressed

in the new frame�

Matrices can also represent the coordinates of second�order tensors that map two

vectors into a scalar� These coordinate matrices are transformed to a new frame in the

same way� See �Cul�� for details�

�

A�� Representing cross products as matrices

In the vector space R� � the cross product �� is an operator taking two vectors toa third vector� In coordinates� the operation can be described by

a� b �

��aybz � azby

azbx � axbz

axby � aybx

�� One can equivalently write

a� b � "ab�

where "a is the � skew�symmetric matrix

"a �

�� az ay

az � �ax�ay ax �

�� In this way� "a can be thought of as a transformation from R

� to itself� taking each vector

v to the cross product a � v� where a is a �xed vector� Representing the cross product

operation as a matrix is often useful in studying equations arising in dynamics� this thesis

uses this representation frequently� denoting cross product matrices with the standard tilde

notation� The matrix "a is skew�symmetric�

"aT � �"a�

Let the matrix "aF correspond to the �cross product by a� operation� in frame F �This same operation may be expressed in a new frame G using the matrix transformationrule described above� If R is the rotation matrix transforming coordinate vectors in F tocoordinate vectors in G� then the operation is described in the new frame by

"aG � R"aFRT �

The matrix "aG represents the same operation as "aF � except it operates on coordinate vectors

resolved in frame G rather than frame F � Since

aG � RaF �

the transformation rule for cross product matrices is often written as

g�Ra� � R"aRT �

�

Cross product matrices provide a convenient way of writing the derivative of a

time varying rotation matrix�

Theorem �� Derivative of a rotation matrix� Let B be a frame rotating relative to

an inertial frame O� with instantaneous angular velocity ��t�� Let R�t� be the rotation

matrix that transforms vector coordinates in B to vector coordinates in O� If �o�t� is the

representation of ��t� in frame O coordinates� and �b�t� is the representation of ��t� in

frame B coordinates� then

#R � "�oR�

#R � R"�b�

Proof� Let r be an arbitrary vector that is �xed in� and therefore rotates with� frame B�Let ro and rb be the coordinate representations of r in O and B� One can compute thederivative #ro in several ways�

#ro �d

dt�Rrb� � #Rrb �R #rb � #Rrb

#ro � �o � ro � �o � �Rrb� � "�oRrb

#ro � R��b � rb� � R"�brb

The �rst equation follows from straight di�erentiation� the second and third follow from

the formula for the velocity of a point on a rotating body� performing the cross product in

frames O and B� respectively� Equating the right hand sides of these three equations�

#Rrb � "�oRrb � R"�brb�

Since rb is arbitrary� the result follows� �

A�� Rigid body dynamics

All of the external in�uences on a rigid body may be summarized as a single force

f�t� acting at the center of mass of the body� and a single moment � �t� applied to the body�

Let r�t� be the position of the center of mass of the body� and m the mass of the body� By

Newton�s second law�

f�t� �d

dt�m #r�t�� m�r�t�� A��

This relation can be rephrased as a �rst order ordinary di�erential equation by letting v�t�

denote the velocity of the center of mass of the body� Then�

#r�t� � v�t�

#v�t� ��

mf�t��

For any f�t�� this �rst order system can always be numerically integrated to de�

termine the motion of the center of mass over time� In many cases� particularly those

described in this thesis� r�t� can be solved in closed form� Consider the case of a ballistic

body� where gravity is the only external in�uence� The gravitational force acts downward

through the center of mass� with constant magnitude mg� where g is the gravitational

constant� Equation �A�� becomes

�mgk � m�r�t��

which upon integrating yields�

r�t� �

��rx�� vx��t

ry�� vy��t

rz�� vz��t� ��gt

�

�� The vectors r�� and v�� are initial data needed to completely solve the ODE�

The other component of rigid body motion is rotation� Let �o�t� be the angular

velocity of the body� and � � the total moment applied to the body� The rotational analog

of �A�� is

� o�t� �d

dt�Io�t��o�t�� A��

Io�t� is often called the mass matrix or inertia tensor� It describes how mass is distributed

in the body� The equation above is only valid when the vectors and matrices are expressed

relative to a frame that it not rotating� like a frame O with origin at the center of mass ofthe body� but with axes that point in �xed directions� The o subscripts denote quantities

expressed in this frame� It can be shown that

Io � R�t�IRT �t��

where I is the body frame mass matrix� This matrix describes how mass is distributed in

the body� relative to a frame B with origin at the center of mass� and that rotates with thebody� I is a constant diagonal matrix� and R�t� is the rotation matrix that maps vectors

in B to vectors in O� The above equation is just an example of a matrix change�of�basistransformation� If ��t� and � �t� are the angular velocity and applied moment vectors

expressed in frame B� �A�� can be rewritten�

R� �t� �d

dt

hR�t�IRT �t�

��R�t��t��

i� #R�t�I��t� �R�t�I #��t�

� R�t�"��t�I��t� �R�t�I #��t��

Multiplying both sides by RT �t� and solving for #��t��

#��t� � I�� "��t�I��t� � � �t�� A��

Writing the diagonal elements of I as Ix� Iy� and Iz��#x�t�

#y�t�

#z�t�

��

Iy�IzIx

y�t�z�t�

Iz�IxIy

z�t�x�t�Ix�IyIz

x�t�y�t�

��

�x�t�Ix

�y�t�Iy

�z�t�Iz

�� These are the Euler equations� they describe how body angular velocity evolves over time�

Often the equations of translation and rotation are grouped together and written�Xf�t� � ma�t� �A��X� �t� � I��t� � ��t�� I��t� �A� �

Equation �A�� is just �A�� with a�t� denoting linear acceleration� the second time deriva�

tive of position� Equation �A� � is a restatement of �A�� with ��t� denoting angular

acceleration� the time derivative of angular velocity� The summation signs are reminders

that the total force and moment acting on the body should be used� Together� these are

called the Newton�Euler equations� they completely characterize the dynamics of a rigid

body� More detailed treatments of these topics are in �Gre�� MK��

A�� Quaternions and integration of orientation

For dynamic simulation� unit quaternions are usually the best representation for

the orientation of a rigid body� Unlike Euler angles and related parameterizations� quater�

�

nions provide a parameterization with no singularities in the mapping to SO�� Unlike

rotation matrices� quaternions are a minimum dimension nonsingular parameterization for

SO�� and when the con�guration point drifts o� the manifold� it can easily be projected

back onto it by normalizing the quaternion� This section derives the ODE that needs to

be integrated for the orientation of a rigid body as a function of time� A more detailed

presentation of quaternions is in �FP��

Every element of SO�� can be identi�ed with a rotation about some unit vector

u by an angle �� The matrix corresponding to this rotation is

R �

��u�xvers � � cos � uyuxvers � � uz sin � uzuxvers � � ky sin �

uxuyvers � � kz sin � u�yvers � � cos � uzuyvers � � ux sin �

uzuzvers � � uy sin � uyuzvers � � kx sin � u�zvers � � cos �

�� A��

where vers � � �� cos �� This orientation can also be represented by the unit quaternion

q � �qs� qx� qy� qz� � �cos�

�ux sin

�

� uy sin

�

� uz sin

�

��

This relation can be inverted to express u and trigonometric functions of � in terms of the

quaternion components�

u ��p�� q�s

��qx

qy

qz

��cos � � q�s � �sin � � qs

q�� q�s

vers � � �� q�s��

Substituting these equivalences into �A�� gives R in terms of quaternions�

R �

��q�s � q�x � �

� qyqx � qzqs qzqx � qyqs

qxqy � qzqs q�s � q�y � �� qzqy � qxqs

qxqz � qyqs qyqz � qxqs q�s � q�z � ��

�� A��

If the quaternion components vary with time�

#R �

��qs #qs � qx #qx� #qyqx � qy #qx � #qzqs � qz #qs #qzqx � qz #qx � #qyqs � qy #qs

#qxqy � qx #qy � #qzqs � qz #qs �qs #qs � qy #qy� #qzqy � qz #qy � #qxqs � qx #qs

#qxqz � qx #qz � #qyqs � qy #qs #qyqz � qy #qz � #qxqs � qx #qs �qs #qs � qz #qz�

��A��

Suppose R�t� is the rotation matrix mapping vectors in the body frame to vectors

in a �xed frame� If � is the angular velocity expressed in the body frame� then from

Theorem ��

"� �

�� wz wy

wz � �wx

�wy wx �

�� RT #R�

Substituting �A�� and �A�� into the above equation produces a complex expression� but

it simpli�es greatly after applying the unit quaternion restriction and its derivative�

q�s � q�x � q�y � q�z � �

qs #qs � qx #qx � qy #qy � qz #qz � ��

With these substitutions� the components of � are easily deduced��x

y

z

�

�� qx �qs �qz �qy�qy �qz �qs �qx

�qz �qy �qx �qs

�qs �qx �qy �qz

��

��#qs

#qx

#qy

#qz

��

The last row of the above equation is just the derivative of the unit quaternion

constraint� it is added to make the matrix square� This matrix is always invertible �the

determinant is always �� Inverting the equation gives��#qs

#qx

#qy

#qz

��

��x �y �z �s

�s �z �y �x

�z �s �x �y

�y �x �s �z

��

��wx

wy

wz

�

��

��x �y �z�s �z �y

�z �s �x�y �x �s

��

This �nal equation expresses the derivatives of the quaternion components in terms of

the angular velocities� It is used by the integrator to evolve the orientation� which is

parameterized by a quaternion�

�

Bibliography

�ACH�� O� Ahmad� J� Cremer� S� Hansen� J� Kearney� and P� Willemson� Hierarchi�

cal� concurrent state machines for behavior modeling and scenario control� In

Proceedings of �� Conference on AI� Simulation� and Planning in High Au�

tonomy Systems� December ��

�Bar�� David Bara�� Analytical methods for dynamic simulation of non�penetrating

rigid bodies� Computer Graphics� ��$� July ��

�Bar�� David Bara�� Curved surfaces and coherence for non�penetrating rigid body

simulation� Computer Graphics� ��$�� August ��

�Bar�� David Bara�� Coping with friction for non�penetrating rigid body simulation�

Computer Graphics� ��$�� August ��

�Bar�� David Bara�� Dynamic Simulation of Non�Penetrating Rigid Bodies� PhD

thesis� Department of Computer Science� Cornell University� March ��

�Bar�� David Bara�� Fast contact force computation for nonpenetrating rigid bodies�

In SIGGRAPH Conference Proceedings� ACM Press� ��

�Bar�� David Bara�� Linear�time dynamics using lagrange multipliers� In SIGGRAPH

Conference Proceedings� ACM Press� ��

�BB�� Ronen Barzel and Alan H� Barr� A modeling system based on dynamic con�

straints� Computer Graphics� ��$�� August ��

�BC�� Dina R� Berkowitz and John Canny� Designing parts feeders using dynamic

simulation� In International Conference on Robotics and Automation� pages

��$�� IEEE� April ��

�

�BD�� William E� Boyce and Richard C� DiPrima� Elementary Di�erential Equations

and Boundary Value Problems� John Wiley * Sons� Inc�� New York� fourth

edition� ��

�BDMM�� K� B�ohringer� B� Donald� R� Mihailovich� and N� MacDonald� Sensorless ma�

nipulation using massively parallel microfabricated actuator arrays� In Inter�

national Conference on Robotics and Automation� IEEE� May ��

�BJO�� H� Brandl� R� Johanni� and M� Otter� A very e�cient algorithm for the

simulation of robots and similar multibody systems� In Proceedings of

IFAC�IFIP�IMACS International Symposium on the Theory of Robots� De�

cember ��

�BK�� Vivek Bhatt and Je� Koechling� Classifying dynamic behavior during three di�

mensional frictional rigid body impact� In International Conference on Robotics

and Automation� IEEE� May ��

�BK��a� Vivek Bhatt and Je� Koechling� Partitioning the parameter space according

to di�erent behavior during d impacts� Transactions of ASME � Journal of

Applied Mechanics� �� To appear��

�BK��b� Vivek Bhatt and Je� Koechling� Three dimensional frictional rigid body impact�

Transactions of ASME � Journal of Applied Mechanics� �� To appear��

�BPM�� Geo�rey Boothroyd� Corrado Poli� and Laurence E� Murch� Automatic Assem�

bly� Marcel Dekker� Inc��

�Bra�� Raymond M� Brach� Mechanical Impact Dynamics Rigid Body Collisions� John

Wiley * Sons� Inc��

�Bro�� Rodney Brooks� A robust layered intelligent control system for a mobile robot�

IEEE Journal of Robotics and Automation� RA��$� March ��

�BS� � Richard W� Bukowski and Carlo H� S+equin� Object associations� A simple and

practical approach to virtual d manipulation� In Symposium on Interactive

�D Graphics� pages ��$�� New York� �� ACM Press�

�Can�� John Canny� Collision detection for moving polyhedra� Technical Report MIT

A�I� Lab Memo �� Massachusetts Institute of Technology� October ��

�

�CEC�� Alan D� Christiansen� Andrea Dunham Edwards� and Carlos A� Coello Coello�

Automated design of part feeders using a genetic algorithm� In International

Conference on Robotics and Automation� IEEE� ��

�CLMP� � Jonathan D� Cohen� Ming C� Lin� Dinesh Manocha� and Madhav K� Ponamgi�

I�collide� An interactive and exact collision detection system for large�scaled

environments� In Symposium on Interactive �D Graphics� pages ��$�� ACM

Siggraph� ACM Siggraph� April ��

�CPS�� Richard K� Cottle� Jong�Shi Pang� and Richard E� Stone� The linear comple�

mentarity problem� Academic Press� ��

�Cra�� John J� Craig� Introduction to Robotics� Mechanics and Control� Addison Wes�

ley Series in Electrical and Computer Engineering� Addison�Wesley� Reading�

MA� second edition� ��

�CS�� James F� Cremer and A� James Stewart� The architecture of newton� a general�

purpose dynamics simulator� In International Conference on Robotics and Au�

tomation� pages ��$�� IEEE� May ��

�Cul�� Charles G� Cullen� Matrices and Linear Transformations� Dover Publications�

Inc�� New York� second edition� ��

�Dev�� Jay L� Devore� Probability � Statistics for Engineering and the Sciences�

Brooks%Cole Publishing Company� Monterey� California� ��

�dJB�� Javier Garc,ia de Jal,on and Eduardo Bayo� Kinematic and Dynamic Simulation

of Multibody Systems� The Real�Time Challenge� Springer�Verlag� ��

�Ede�� H� Edelsbrunner� A new approach to rectangle intersections� part i� Interna�

tional Journal of Computational Mathematics� ��$��

�EP�� C� H� Edwards� Jr� and David E� Penney� Calculus and Analytic Geometry�

Prentice�Hall� Inc�� Englewood Cli�s� second edition� ��

�Far�� Gerald Farin� Curves and Surfaces for Computer Aided Geometric Design�

Academic Press� Inc�� San Diego� second edition� ��

�

�Fea�� R� Featherstone� The calculation of robot dynamics using articulated�body

inertias� International Journal of Robotics Research� ��$��

�FP�� Janez Funda and Richard P� Paul� A comparison of transforms and quaternions

in robotics� In International Conference on Robotics and Automation� pages

��$�� IEEE� May ��

�FR�� R�T� Farouki and V�T� Rajan� Algorithms for polynomials in Bernstein form�

Computer Aided Geometric Design� ��$�� June ��

�GC� � Ken Goldberg and John Craig� Estimating throughput for a �exible part

feeder� Simulation and experiments� In International Symposium of Experi�

mental Robotics� Stanford University� June ��

�Get�� N� H� Getz� Control of balance for a nonlinear nonholonomic nonminimum�

phase model of a bicycle� In American Control Conference� Baltimore� June

�� American Automatic Control Council�

�GJK�� Elmer G� Gilbert� Daniel W� Johnson� and S� Sathiya Keerthi� A fast procedure

for computing the distance between complex objects in three�dimensional space�

IEEE Journal of Robotics and Automation� ��$�� April ��

�Gle�� Michael L� Gleicher� A Di�erential Approach to Graphical Interaction� PhD the�

sis� School of Computer Science� Carnegie Mellon University� November ��

�GLM�� S� Gottschalk� M� C� Lin� and D� Manocha� Obb�tree� A hierarchical structure

for rapid interference detection� In SIGGRAPH Conference Proceedings� ACM

Press� ��

�GM� � James Gosling and Henry McGilton� The java language environment� A white

paper� Technical report� Sun Microsystem� ��

�Gre�� Donald T� Greenwood� Principles of Dynamics� Prentice�Hall� Inc�� Englewood

Cli�s� second edition� ��

�Hah�� James K� Hahn� Realistic animation of rigid bodies� Computer Graphics�

��$�� August ��

��

�HBZ�� Brian Von Herzen� Alan H� Barr� and Harold R� Zatz� Geometric collisions for

time�dependent parametric surfaces� In SIGGRAPH Conference Proceedings�

pages �$�� ACM Press� ��

�HJ�� Roger A� Horn and Charles R� Johnson� Matrix Analysis� Cambridge University

Press� Cambridge� ��

�Hub�� Philip M� Hubbard� Collision Detection for Interactive Graphics Applications�

PhD thesis� Department of Computer Science� Brown University� October ��

�Hub�� Philip M� Hubbard� Approximating polyhedra with spheres for time�critical

collision detection� ACM Transactions on Graphics� � �� July ��

�HWBO� � Jessica K� Hodgins� Wayne L� Wooten� David C� Brogan� and James F� O�Brien�

Animating human athletics� Computer Graphics� pages ��$�� Siggraph

Conference Proceedings�

�IC�� Paul M� Isaacs and Michael F� Cohen� Controlling dynamic simulation with

kinematic constraints� behavior functions and inverse dynamics� In SIGGRAPH

Conference Proceedings� pages � $�� ACM Press� ��

�JR� A� Jain and G� Rodriguez� Lecture notes on the spatial operator algebra for

multibody dynamics�

�Kel�� J� B� Keller� Impact with friction� Journal of Applied Mechanics� � March

��

�Kha�� Oussama Khatib� A uni�ed approach for the motion and force control of robot

manipulators� The operational space formulation� IEEE Journal of Robotics

and Automation� RA��$ � February ��

�LC�� Ming C� Lin and John F� Canny� A fast algorithm for incremental distance

calculation� In International Conference on Robotics and Automation� pages

��$�� IEEE� May ��

�Lil�� Kathryn W� Lilly� E�cient Dynamic Simulation of Robotic Mechanisms�

Kluwer Academic Publishers� Norwell� ��

��

�Lin�� Ming C� Lin� E�cient Collision Detection for Animation and Robotics� PhD

thesis� University of California� Berkeley� December ��

�LK�� Sheue�ling Lien and James T� Kajiya� A symbolic method for calculating the

integral properties of arbitrary nonconvex polyhedra� IEEE Computer Graphics

and Applications� �� $�� October ��

�LM�� M�C� Lin and Dinesh Manocha� Interference detection between curved objects

for computer animation� In Models and Techniques in Computer Animation�

pages ��$ �� Springer�Verlag� ��

�LM� � Andrew D� Lewis and Richard M� Murray� Variational principles in constrained

systems� Theory and experiments� International Journal of Nonlinear Mechan�

ics� ��

�LNPE�� C� Lubich� U� Nowak� U� Pohle� and Ch� Engstler� Mexx � numerical software

for the integration of constrained mechanical multibody systems� Technical Re�

port Technical report SC�� Konrad�Zuse�Zentrum fur Informationstechnik�

Berlin� ��

�L�ot�� Per L�otstedt� Coulomb friction in two�dimensional rigid body systems�

Zeitschrift fur Angewandte Mathematik und Mechanik� �� $� � ��

�L�ot�� Per L�otstedt� Numerical simulation of time�dependent contact and friction

problems in rigid body mechanics� SIAM Journal of Scienti�c Statistical Com�

puting� ��$�� June ��

�LR�a� Yong Tsui Lee and Aristides A� G� Requicha� Algorithms for computing the

volume and other integral properties of solids� I� Known methods and open

issues� Communications of the ACM� �� $�� September ��

�LR�b� Yong Tsui Lee and Aristides A� G� Requicha� Algorithms for computing the

volume and other integral properties of solids� II� A family of algorithms based

on representation conversion and cellular approximation� Communications of

the ACM� ��$ �� September ��

�

�LRK�� Paul U� Lee� Diego C� Ruspini� and Oussama Khatib� Dynamic simulation of

interactive robotic environment� In International Conference on Robotics and

Automation� pages ��$�� IEEE� May ��

�MC� a� Brian Mirtich and John Canny� Impulse�based dynamic simulation� In K� Gold�

berg� D� Halperin� J�C� Latombe� and R� Wilson� editors� The Algorithmic

Foundations of Robotics� A� K� Peters� Boston� MA� �� Proceedings from

the workshop held in February� ��

�MC� b� Brian Mirtich and John Canny� Impulse�based simulation of rigid bodies� In

Symposium on Interactive �D Graphics� pages ��$�� New York� �� ACM

Press�

�Mil�� Gavin Miller� Goal�directed animation of tubular articulated �gures or how

snakes play golf� In Norman A� Badler� Brian A� Barsky� and David Zeltzer� ed�

itors� Making Them Move� Mechanics� Control� and Animation of Articulated

Figures� pages $�� San Mateo� CA� �� Morgan Kaufmann Publishers�

Inc� From the workshop held April �$��

�Mir�� Brian Mirtich� Fast and accurate computation of polyhedral mass properties�

Journal of Graphics Tools� ��

�MK�� J�L� Meriam and L�G� Kraige� Engineering Mechanics Volume �� Dynamics�

John Wiley * Sons� Inc�� New York� ��

�MR�� J� Marsden and T� Ratiu� Introduction to Mechanics and Symmetry� a basic

exposition of classical mechanical systems� Springer�Verlag� New York� ��

�MW�� Matthew Moore and Jane Wilhems� Collision detection and response for com�

puter animation� Computer Graphics� ��$�� August ��

�MZ�� Michael McKenna and David Zeltzer� Dynamic simulation of autonomous

legged locomotion� Computer Graphics� pages �$�� Siggraph Con�

ference Proceedings�

�MZG�� Brian Mirtich� Yan Zhuang� Ken Goldberg� John Craig� Rob Zanutta� Brian

Carlisle� and John Canny� Estimating pose statistics for robotic part feeders� In

�

International Conference on Robotics and Automation� pages ��$�� IEEE�

April ��

�NM�� J� Thomas Ngo and Joe Marks� Spacetime constraints revisited� Computer

Graphics� pages �$ �� Siggraph Conference Proceedings�

�NW�� James L� Nevins and Daniel E� Whitney� Computer�controlled assembly� Sci�

enti�c American� ��

�Ove�� M� Overmars� Point location in fat subdivisions� Information Processing Let�

ters� ��$� � ��

�Pau� � Eric Paulos� Parallel impulse based dynamic simulation� Available from WWW

site http ��robotics�eecs�berkeley�edu� paulos�IMP�� May ��

�PML� � Madhav K� Ponamgi� Dinesh Manocha� and Ming C� Lin� Incremental algo�

rithms for collision detection between solid models� In Proceedings of Third

ACM Symposium on Solid Modeling and Applications� pages �$�� New

York� May �� ACM Press�

�PS�� Michael A� Peshkin and Arthur C� Sanderson� The motion of a pushed� sliding

workpiece� IEEE Journal of Robotics and Automation� �� $ �� December

��

�PS�� Michael A� Peshkin and Arthur C� Sanderson� Minimization of energy in quasi�

static manipulation� IEEE Transactions on Robotics and Automation� �� $

�� February ��

�PT�� J�S� Pang and J�C� Trinkle� Complementarity formulations and existence of

solutions of dynamic multi�rigid�body contact problems with coulomb friction�

Mathematical Programming� �� To appear��

�PTVF�� William H� Press� Saul A� Teukolsky� William T� Vetterling� and Brian R�

Flannery� Numerical Recipes in C� The Art of Scienti�c Computing� Cambridge

University Press� Cambridge� second edition� ��

�Rai�� Marc H� Raibert� Legged Robots that Balance� The MIT Press series in arti�cial

intelligence� MIT Press� Cambridge� ��

��

�RB� Dan S� Reznik and Stan W� Brown� Dynamic simulation of micro�actuated

motion arrays with application to design optimization� Submitted to ��

IEEE International Conference on Robotics and Automation�

�Rey�� Craig W� Reynolds� Flocks� herds� and schools� A distributed behavioral model�

Computer Graphics� pages $�� Siggraph Conference Proceedings�

�RG�� Anil Rao and Ken Goldberg� Planning grasps for a pivoting gripper� Technical

report� USC Institute for Robotics and Intelligent Machines� October ��

USC Techreport IRIS��

�RH�� Marc H� Raibert and Jessica K� Hodgins� Animation of dynamic legged lo�

comotion� Computer Graphics� pages ��$ �� Siggraph Conference

Proceedings�

�RK� Diego C� Ruspini and Oussama Khatib� Simpact� Available from WWW site

http ��flamingo�stanford�edu�users�ruspini�simpact�html�

�RKG� � A� Rao� D� Kriegman� and K� Goldberg� Complete algorithms for reorient�

ing polyhedral parts using a pivoting gripper� In International Conference on

Robotics and Automation� IEEE� May ��

�Rou� � Edward J� Routh� Elementary Rigid Dynamics� Macmillan� London� ��

�Sim�� Karl Sims� Evolving virtual creatures� Computer Graphics� pages � $� ��

Siggraph Conference Proceedings�

�SSCK�� K� Sridharan� H�E� Stephanou� K�C� Craig� and S�S� Keerthi� Distance measures

on intersecting objects and their applications� Information Processing Letters�

��$�� August ��

�Ste�� Sherman K� Stein� Calculus and Analytic Geometry� McGraw�Hill� Inc�� New

York� third edition� ��

�Str�� W� J� Stronge� Unraveling paradoxical theories for rigid body collisions� Journal

of Applied Mechanics� ��$�� December ��

�SWF�� John M� Snyder� Adam R� Woodbury� Kurt Fleischer� Bena Currin� and Alan H�

Barr� Interval methods for multi�point collisions between time�dependent

�

curved surfaces� In SIGGRAPH Conference Proceedings� pages �$� ACM

Press� ��

�TPSL�� J�C� Trinkle� J�S� Pang� S� Sudarsky� and G� Lo� On dynamic multi�rigid�body

contact problems with coulomb friction� Zeitschrift fur Angewandte Mathematik

und Mechanik� �� To appear��

�TT�� Xiaoyuan Tu and Demetri Terzopoulos� Arti�cial �shes� Physics� locomotion�

perception� behavior� Computer Graphics� pages �$ �� Siggraph Con�

ference Proceedings�

�Van�� George Vanecek� Jr� Brep�index� A multi�dimensional spatial partitioning tree�

International Journal of Computational Geometry and Applications� ��$

�� September ��

�WB�� C� Ray Wylie and Louis C� Barrett� Advanced Engineering Mathematics�

McGraw�Hill� Inc�� New York� �fth edition� ��

�WGPB�� Je� Wiegley� Ken Goldberg� Mike Peshkin� and Mike Brokowski� A complete

algorithm for designing passive fences to orient parts� In International Confer�

ence on Robotics and Automation� pages ��$�� IEEE� April ��

�WGW�� Andrew Witkin� Michael Gleicher� and William Welch� Interactive dynamics�

Computer Graphics� ��$� March ��

�WM�� Yu Wang and Matthew T� Mason� Modeling impact dynamics for robotic

operations� In International Conference on Robotics and Automation� pages

��$�� IEEE� May ��

�WO�� M�W� Walker and D�E� Orin� E�cient dynamic computer simulation of

robotic mechanisms� Journal of Dynamic Systems� Measurement� and Con�

trol� �� $�� September ��

�WRG�� Je� Wiegley� Anil Rao� and Ken Goldberg� Computing a statistical distribu�

tion of stable poses for a polyhedron� In ��th Annual Allerton Conference on

Communications� Control� and Computing� ��

��

�Zhu�� Yan Zhuang� Simulation of linear electrostatic stepper motor �a mems de�

vice� using impulse� Available from WWW site

http ��www�cs�berkeley�edu� yzhuang�projects�cs��report�html�

May ��

of Briankuffner.org/james/software/dynamics/mirtich/mirtich...king p erformed to detect aggressiv e...

Documents

Transcript of of Briankuffner.org/james/software/dynamics/mirtich/mirtich...king p erformed to detect aggressiv e...