**Penetration Depth Computation**

In this section, we give a brief overview of penetration depth (PD) computation algorithms between convex polytopes and general polyhedral models. The PD of two inter-penetrating objects A and B is deﬁned as the minimum translation distance that one object undergoes to make the interiors of A and B disjoint. It can be also deﬁned in terms of the TCSO. When two objects are overlapping, the origin of the Minkowski sum of A and *−*B is contained inside the TCSO. The penetration depth corresponds to the minimum distance from the origin to the surface of TCSO [18]. PD computation is often used in motion planning [37], contact resolution for dynamic simulation [38, 39] and force computation in haptic rendering [40]. For example, computation of dynamic response in penalty-based methods often needs to perform PD queries for imposing the non-penetration constraint for rigid body simulation. In addition, many applications, such as motion planning and dynamic simulation, require a continuous distance measure when two (non-convex) objects collide, in order to have a well-posed computation.

Some of the algorithms for PD computation involve computing the Minkowski sums and computing the closest point on its surface from the origin. The worst case complexity of the overall PD algorithm is governed by the complexity of computing Minkowski sums, which can be O(n2) for convex polytopes and O(n6) for general (or non-convex) polyhedral models [16]. Given the complexity of Minkowski sums, many approximation algorithms have been proposed in the literature for fast PD estimation.

**Convex Polytopes**

Dobkin et al. [16] have proposed a hierarchical algorithm to compute the directional PD using Dobkin and Kirkpatrick polyhedral hierarchy. For any direction d, it computes the directional penetration depth in O(log n log m) time for polytopes with m and n vertices.

Agarwal et al. [41] have presented a randomized approach to compute the PD values [41].

It runs in O(m 4 +*E*n 4 +*E *+ m1+*E *+ n1+*E*) expected time for any positive constant E. Cameron [18] has presented an extension to the GJK algorithm [17] to compute upper and lower bounds on the PD between convex polytopes. Bergen has further elaborated this idea in an expanding polytope algorithm [42]. The algorithm iteratively improves the result of the PD computation by expanding a polyhedral approximation of the Minkowski sums of two polytopes.

**Incremental Penetration Depth Computation**

Kim et al. [43] have presented an incremental penetration depth (PD) algorithm that marches towards a “locally optimal” solution by walking on the surface of the Minkowski sum. The surface of the TCSO is implicitly computed by constructing a local Gauss map and performing a local walk on the polytopes.

This algorithm uses the concept of width computation from computational geometry. Given a set of points P = *{*p1, p2, … , p*n**} *in 3D, the *width *of P , W(P ), is deﬁned as the minimum distance between parallel planes supporting P . The width W(P ) of convex polytopes A and B is closely related to the penetration depth PD(A, B), since it is easy to show that W(P ) = PD(P , P ). It can be shown that width and penetration depth computation can be reduced to searching only the VF and EE antipodal pairs (where V, E and F denote a vertex, edge and face, respectively, of the polytopes). This is accomplished by using the standard dual mapping on the Gauss map (or normal diagram). The mapping is deﬁned from object space to the surface of a unit sphere S2 as: a vertex is mapped to a region, a face to a point, and an edge to a great arc. The algorithm ﬁnds the antipodal pairs by overlaying the upper hemisphere of the Gauss map on the lower hemisphere and computing the intersections between them.

**L****o****ca****l Walk**

The incremental PD computation algorithm does not compute the entire Gauss map for each polytope or the entire boundary of the Minkowski sum. Rather it computes them in a lazy manner based on local walking and optimization. Starting from some feature on the surface of the Minkowski sum, the algorithm computes the direction in which it can decrease the PD value and proceeds towards that direction by extending the surface of the Minkowski sum.

At each iteration of the algorithm, a vertex is chosen from each polytope to form a pair. It is called a *vertex hub pair *and the algorithm uses it as a hub for the expansion of the local Minkowski sum. The vertex hub pair is chosen in such a way that there exists a plane supporting each polytope, and is incident on each vertex. It turns out that the vertex hub pair corresponds to two intersected convex regions on a Gauss map, which later become intersecting convex polygons on the plane after *central projection*. The intersection of convex polygons corresponds to the VF or EE antipodal pairs that are used to reconstruct the local surface of the Minkowski sum around the vertex hub pair. Given these pairs, the algorithm chooses the one that corresponds to the shortest distance from the origin of the Minkowski sum to their surface. If this pair decreases the estimated PD value, the algorithm updates the current vertex hub pair to the new adjacent pair. This procedure is repeated until the algorithm can not decrease the current PD value and converges to a local minima.

**Initializatio****n and Reﬁnement**

The algorithm starts with an initial guess on the vertex hub pair. A good estimate to the penetration direction can be obtained by taking the centroid diﬀerence between the objects, and computing an extremal vertex pair for the diﬀerence direction. In other cases, the pen- etrating features (for overlapping polytopes) or the closest features (from non-overlapping polytopes) from the previous instance can also suggest a good initial guess.

FIGURE 56.4: Iterative optimization for incremental PD computation: (a) The current and a shaded region represents edges and faces incident to *v***1***v*** l **. (b) shows local Gauss maps and their overlay for

*v***1**

*v***. (c) shows the result of the overlay after central projection onto a plane. Here, f1, e1, f2 and e2 comprise vertices (candidate PD features) of the overlay. (d) illustrates how to compute the PD for the candidate PD features in object space. (e) f2 is chosen as the next PD feature, thus**

*l*

*v***2**

*v***is determined as the next vertex hub pair.**

*l*After the algorithm obtains a initial guess for a **V****V **pair, it iteratively seeks to improve the PD estimate by jumping from one **V****V **pair to an adjacent **V****V **pair. This is accomplished by looking around the neighborhood of the current **V****V **pair and walking to a pair which provides the greatest improvement in the PD value. Let the current vertex hub pair be *v***1***v*** l **. The next vertex hub pair

*v***2**

*v***is computed as follows:**

*l*1.Construct a local Gauss map each for v1 and v

t,2. Project the Gauss maps onto z = 1 plane, and label them as G and G

t, respectively. G and Gtcorrespond to convex polygons in 2D.3. Compute the intersection between G and G

tusing a linear time algorithm such as [44]. The result is a convex polygon and let uibe a vertex of the intersection set. If uiis an original vertex of G or Gt, it corresponds to the VF antipodal pairin object space. Otherwise, it corresponds to an EE antipodal pair.

4. In object space, determine which u

icorresponds to the best local improvement in PD, and set an adjacent vertex pair (adjacent to ui) tov2v.l

This iteration is repeated until either there is no more improvement in the PD value or number of iterations reach some maximum value. At step 4 of the iteration, the next vertex hub pair is selected in the following manner. If u*i *corresponds to VF, then the algorithm chooses one of the two vertices adjacent to F assuming that the model is triangulated. The same reasoning is also applied to when u*i *corresponds to EE. As a result, the algorithm needs to perform one more iteration in order to actually decide which vertex hub pair should be selected.

A snapshot of a typical step during the iteration is illustrated in Figure 56.4.

Eventually the algorithm computes a local minima.

**I****m****pl****em****entation and Application**

The incremental algorithm is implemented as part of DEEP [12]. It works quite well in practice and is also able to compute the global penetration depth in most cases. It has been used for 6-DOF haptic rendering (force and torque display), dynamic simulation and virtual prototyping.

Non-Convex Models Algorithms for penetration depth estimation between general polygonal models are based on discretization of the object space containing the objects or use of digital geometric algorithms that perform computations on a ﬁnite resolution grid. Fisher and Lin [45] have presented a PD estimation algorithm based on the distance ﬁeld computation using the fast marching level-set method. It is applicable to all polyhedral objects as well as deformable models, and it can also check for self-penetration. Hoﬀ et al. [46, 47] have proposed an approach based on performing discretized computations on the graphics rasterization hard- ware . It uses multi-pass rendering techniques for diﬀerent proximity queries between general rigid and deformable models, including penetration depth estimation. Kim et al. [43] have presented a fast approximation algorithm for general polyhedral models using a combina- tion of object-space as well discretized computations. Given the global nature of the PD problem, it decomposes the boundary of each polyhedron into convex pieces, computes the pairwise Minkowski sums of the resulting convex polytopes and uses graphics rasterization hardware to perform the closest point query up to a given discretized resolution. The results obtained are reﬁned using a local walking algorithm. To further speed up this computation and improve the estimate, the algorithm uses a hierarchical reﬁnement technique that takes advantage of geometry culling, model simpliﬁcation, accelerated ray-shooting, and local reﬁnement with greedy walking. The overall approach combines discretized closest point queries with geometry culling and reﬁnement at each level of the hierarchy. Its accuracy can vary as a function of the discretization error. It has been applied to haptic rendering and dynamic simulation.