  Penetration depth (PD) is essential for robotics due to its extensive
applications in dynamic simulation, motion planning, haptic rendering, etc. The
Expanding Polytope Algorithm (EPA) is the de facto standard for this problem,
which estimates PD by expanding an inner polyhedral approximation of an
implicit set. In this paper, we propose a novel optimization-based algorithm
that incrementally estimates minimum penetration depth and its direction. One
major advantage of our method is that it can be warm-started by exploiting the
spatial and temporal coherence, which emerges naturally in many robotic
applications (e.g., the temporal coherence between adjacent simulation time
knots). As a result, our algorithm achieves substantial speedup -- we
demonstrate it is 5-30x faster than EPA on several benchmarks. Moreover, our
approach is built upon the same implicit geometry representation as EPA, which
enables easy integration and deployment into existing software stacks. We also
provide an open-source implementation for further evaluations and experiments.
