Louis Montaut

Quentin Le Lidec

Vladimir Petrik

Josef Sivic

Justin Carpentier

GJK++: Leveraging Acceleration Methods for Faster Collision Detection

Keywords: Convex Optimization, Collision Detection, Computational Geometry, Computer Graphics, Simulation, Trajectory Optimization, Motion Planning

Collision detection is a fundamental problem in various domains, such as robotics, computational physics, and computer graphics. In general, collision detection is tackled as a computational geometry problem, with the so-called Gilbert, Johnson, and Keerthi (GJK) algorithm being the most adopted solution nowadays. While introduced in 1988, GJK remains the most effective solution to compute the distance or the collision between two 3D convex geometries. Over the years, it was shown to be efficient, scalable, and generic, operating on a broad class of convex shapes, ranging from simple primitives (sphere, ellipsoid, box, cone, capsule, etc.) to complex meshes involving thousands of vertices. In this article, we introduce several contributions to accelerate collision detection and distance computation between convex geometries by leveraging the fact that these two problems are fundamentally optimization problems. Notably, we establish that the GJK algorithm is a specific sub-case of the well-established Frank-Wolfe (FW) algorithm in convex optimization. By adapting recent works linking Polyak and Nesterov accelerations to Frank-Wolfe methods, we also propose two accelerated extensions of the classic GJK algorithm.

Through an extensive benchmark over millions of collision pairs involving objects of daily life, we show that these two accelerated GJK extensions significantly reduce the overall computational burden of collision detection, leading to computation times that are up to two times faster. Finally, we hope this work will significantly reduce the computational cost of modern robotic simulators, allowing the speed-up of modern robotic applications that heavily rely on simulation, such as reinforcement learning or trajectory optimization.

I. INTRODUCTION

P HYSICS engines designed to simulate rigid bodies are an essential tool used in a wide variety of applications, notably in robotics, video games, and computer graphics [1]- [START_REF] Todorov | Mujoco: A physics engine for model-51 based control[END_REF].

Collision detection, a crucial feature of any physics engine or robot motion planer [4]- [START_REF] Schulman | Motion planning with sequential convex optimization and convex collision checking[END_REF], consists of finding which objects are colliding or not, i.e. are sharing at least one common point or if there exists a separating hyper-plane between both. As simulation often needs to deal with multiple objects and run in real-time (i.e., in video games) or at very high frequencies (i.e., Louis Two distinct collision problems using shapes from the YCB dataset: in (a) the shapes A 1 (in green) and A 2 (in red) are not in collision (dist(A 1 , A 2 ) > 0) whereas in (b) the shapes are in collision (dist(A 1 , A 2 ) = 0). In the left column, the oriented bounding boxes (OBB) of the objects are represented in light colors. In the right column, the light colors represent the convex hull of each object. In both collision problems, (a) and (b), the broad phase finds a collision between the object's OBBs; the narrow phase must thus be called to confirm or infirm the collision. The right column corresponds to the narrow phase in which the GJK algorithm is called on the objects' convex hulls. In this paper, we propose the Polyak-accelerated GJK and Nesterov-accelerated GJK algorithms in order to accelerate collision detection.

in robotics), collision detection must be carried out as fast as 1 possible. To reduce computational times, collision detection is 2 usually decomposed into two phases thoroughly covered in [START_REF] Ericson | Real-time collision detection[END_REF].

3

The first phase is the so-called broad phase which consists 4 in identifying which pair of simulated objects are potentially 5 colliding. The broad phase relies on the simulated objects' 6 bounding volumes, as shown in Fig. 1, allowing to quickly 7 assess if the objects are not in collision. The second phase is 8 the so-called narrow phase in which each pair identified in 9 the broad phase is tested to check whether a collision is truly 10 occurring. Collision detection during the narrow phase is the 11 focus of this paper.

12 Problem formulation. We consider two convex shapes A 1 13 and A 2 in R n (with n = 2 or 3 in common applications). [START_REF] Li | A momentumguided frank-wolfe algorithm[END_REF] If the shapes are not convex, we use their respective convex 15 hulls or decompose them into a collection of convex sub-16 shapes [START_REF] Mamou | A simple and efficient approach for 3d mesh approximate convex decomposition[END_REF]. The separation distance between A 1 and A 2 , 17 denoted by dist(A 1 , A 2 ) ∈ R + , can be formulated as a [START_REF] Tedrake | Drake: Model-based design and verification for robotics[END_REF] minimization problem of the form:

1 d 1,2 = min x1∈A1,x2∈A2 ∥x 1 -x 2 ∥ 2 and dist(A 1 , A 2 ) = d 1,2 , (1)
where x 1 ∈ A 1 and x 2 ∈ A 2 are both vectors in R n , d 1,2 magnitude to generic quadratic programming solvers on collision detection problems;

→ Finally, we show that our methods can be used in any physics simulator by benchmarking them on trajectories generated by the Bullet simulator. Like GJK, our methods can benefit from being warm-started using the previous simulation time steps, enabling temporal coherence for our proposed accelerated collision detection algorithms.

This article is an extended version of a previously published paper [START_REF] Montaut | Collision Detection Accelerated: An Optimization Perspective[END_REF] which presented the Nesterov-accelerated GJK algorithm. To expand on our previous work, we introduce the Polyak-accelerated GJK algorithm, provide additional benchmarks, notably against existing collision detection librairies, and show that our proposed methods can be used in the context of physics simulation by benefiting from being warm-started using previous simulation steps.

In the rest of this section, we first formulate the problem of collision detection and then provide an overview of collision detection related works.

Related work. The so-called Gilbert-Johnson-Keerthi algorithm (GJK) [START_REF] Gilbert | A fast procedure for computing the distance between complex objects in three-dimensional space[END_REF] is the most well-known algorithm for collision detection between two convex shapes. It can handle the distance computation and the Boolean collision check [START_REF] Bergen | A fast and robust gjk implementation for collision detection of convex objects[END_REF]. The expanding polytope algorithm (EPA) [START_REF] Van Den Bergen | Proximity Queries and Penetration Depth Computation on 3D Game Objects[END_REF], an extension to GJK, can compute the penetration depth i.e. the norm of the separation vector, when shapes are in collision. The separation vector is the vector of smallest norm needed to translate one of the two shapes such that the two shapes do not intersect. The EPA solves a non-convex and more complex problem than (1), which is not the focus of this paper.

Most alternatives to GJK in the literature focus on computing collisions between convex polyhedra, such as the Lin-Canny algorithm [START_REF] Lin | A fast algorithm for incremental distance calculation[END_REF] or the V-Clip [START_REF] Mirtich | V-clip: Fast and robust polyhedral collision detection[END_REF] algorithm. Although GJK is equivalent in performance to these algorithms [START_REF] Cameron | A comparison of two fast algorithms for computing the distance between convex polyhedra[END_REF], it is not restricted to convex polyhedra. The strength of GJK is formulating the collision detection problem on the Minkowski difference. The properties of the Minkowski difference are used to cleverly compute support vectors on the Minkowski difference (these notions are introduced and detailed in Sec. II). GJK can thus handle collision detection, and distance computation for many different shapes such as convex polyhedra and basic primitives (i.e., spheres, ellipsoids, cylinders, capsules etc.) [START_REF] Ericson | Real-time collision detection[END_REF], [START_REF] Bergen | A fast and robust gjk implementation for collision detection of convex objects[END_REF], [START_REF] Van Den Bergen | Collision detection in interactive 3D environments[END_REF]. The Minkowski Portal Refinement (MPR) [START_REF] Snethen | Complex collision made simple[END_REF] is a variant of the GJK algorithm which is slightly simpler to implement; however, MPR can only perform boolean collision checks as it cannot be used to perform distance computation between non-overlapping shapes. Overall, the generality of GJK, efficiency, good precision, and ease of implementation make it the state-of-the-art algorithm for collision detection between two convex shapes.

Collision detection has been casted into a convex optimization problem for many years [START_REF] Gilbert | A fast procedure for computing the distance between complex objects in three-dimensional space[END_REF], [START_REF] Pan | Fcl: A general purpose library for collision and proximity queries[END_REF], [START_REF] Macklin | Simulation for learning and robotics: Numerical methods for contact, deformation, and identification[END_REF]. However, collision detection is traditionally presented and considered mainly as a computational geometry problem [START_REF] Ericson | Real-time collision detection[END_REF], [START_REF] Bergen | A fast and robust gjk implementation for collision detection of convex objects[END_REF], [START_REF] Mirtich | V-clip: Fast and robust polyhedral collision detection[END_REF], [START_REF] Cameron | A comparison of two fast algorithms for computing the distance between convex polyhedra[END_REF], [START_REF] Montanari | Improving the gjk algorithm for faster and more reliable distance queries between convex objects[END_REF], [START_REF] Canny | Collision detection for moving polyhedra[END_REF]. Over the years, this computational geometric perspective allowed enhancing the computational efficiency of GJK, thanks to improvements to its internal sub-routines [START_REF] Bergen | A fast and robust gjk implementation for collision detection of convex objects[END_REF], [START_REF] Montanari | Improving the gjk algorithm for faster and more reliable distance queries between convex objects[END_REF]. However, we argue that this view has also limited collision detection improvement. Instead, we propose to tackle collision from the perspective of convex optimization. This correlates with some observations raised in the original GJK papers. Indeed, as briefly mentioned already in their 1988 paper [START_REF] Gilbert | A fast procedure for computing the distance between complex objects in three-dimensional space[END_REF] and brought up again by [START_REF] Macklin | Simulation for learning and robotics: Numerical methods for contact, deformation, and identification[END_REF], [START_REF] Qin | Smoothing algorithms for computing the projection onto a minkowski sum of convex sets[END_REF], the ideas developed by Gilbert, Johnson, and Keerthi are rooted in convex optimization, notably in the works of [START_REF] Gilbert | An iterative procedure for computing the minimum of a quadratic form on a convex set[END_REF] and [START_REF] Wolfe | Finding the nearest point in a polytope[END_REF] for solving Minimum-Norm Point (MNP) problems. This article proposes exploiting the Frank-Wolfe convex optimization setting to tackle collision detection. In particular, by leveraging recent progresses in acceleration methods in convex optimization [START_REF] Aspremont | Acceleration methods[END_REF], we show how to accelerate collision detection by directly lowering the number of iterations needed to solve a collision problem instance compared to the vanilla GJK algorithm.

The Frank-Wolfe algorithm (FW) dates back to 1956 and is one of the first convex optimization algorithms. It has been heavily studied over the years by the optimization community. This algorithm iterates over the computation of support points to approach the optimal solution. The undesired zig-zagging behavior of FW, already identified by its authors, has been addressed by introducing corrections to the original FW method [START_REF] Gilbert | An iterative procedure for computing the minimum of a quadratic form on a convex set[END_REF], [START_REF] Wolfe | Finding the nearest point in a polytope[END_REF], [START_REF] Garber | Playing non-linear games with linear oracles[END_REF]- [START_REF] Lacoste-Julien | On the global linear convergence of frank-wolfe optimization variants[END_REF]. In [START_REF] Jaggi | Revisiting frank-wolfe: Projection-free sparse convex optimization[END_REF] and [START_REF] Lacoste-Julien | On the global linear convergence of frank-wolfe optimization variants[END_REF], widely used corrections of the FW algorithm are analyzed, and their convergence properties. In this work, we notably show in Sec. II that the GJK algorithm is an instance of the fully-corrective Frank-Wolfe algorithm, covered in [START_REF] Lacoste-Julien | On the global linear convergence of frank-wolfe optimization variants[END_REF], applied to solving a MNP problem. Finally, recent works have also tried accelerating the FW algorithm by applying the so-called Nesterov acceleration [START_REF] Nesterov | A method of solving a convex programming problem with convergence rate o\bigl(kˆ2\bigr)[END_REF], a classic acceleration technique in unconstrained optimization. Nesterov momentum has been successfully added by [START_REF] Li | A momentumguided frank-wolfe algorithm[END_REF] to accelerate FW.

II. COLLISION DETECTION FROM A FRANK-WOLFE

PERSPECTIVE

In this section, we highlight the natural connection between computing the distance between convex shapes and convex optimization, particularly within the frame of the Frank-Wolfe (FW) setting. The first part of this section is a tutorial on collision detection, starting with FW and gradually working towards GJK. We then show that the GJK algorithm can be seen as a variant of the FW algorithm that leverages properties of convex 3D shapes to lower the computational complexity drastically.

Distance computation and Boolean collision checking.

As recalled in Sec. I, collision detection is a sub-case of distance computation: dist(A 1 , A 2 ) > 0 means that the two shapes do not overlap while dist(A 1 , A 2 ) = 0 means that the shapes are in collision. In the case of dist(A 1 , A 2 ) > 0, finding a strictly positive lower bound on d 1,2 to solve the collision problem is sufficient. In the context of convex shapes, this is often simpler than computing the distance between the two shapes [START_REF] Stasse | Real-time (self)-collision avoidance task on a hrp-2 humanoid robot[END_REF] and can be done by finding a plane separating A 1 from A 2 . In the rest of the paper, we focus on the generic problem of computing the distance between A 1 1 and A 2 , as it encapsulates the more straightforward Boolean the first idea is to look at problem (1) through the lens of 9 quadratic programming. In the case of meshes, which are 10 shapes represented by soups of 3D points and which faces 11 represented as triangles, we can use the implicit description 12 of a convex mesh as a linear inequality of the form Ax ≤ b. [START_REF] Li | Heavy ball momentum for conditional gradient[END_REF] The collision detection problem between two meshes can thus 14 be cast as a quadratic programming (QP) problem:

15 d 1,2 = min x1,x2∈R 3 ∥x 1 -x 2 ∥ 2 s.t A 1 x 1 ≤ b 1 A 2 x 2 ≤ b 2 .
(

) 2
While many off-the-shelf solvers exist to solve QP problems, 16 their performances scale poorly with respect to the number 17 of constraints [START_REF] Nocedal | Numerical optimization[END_REF]. This is especially true in the presence 18 of complex meshes composed of hundreds or thousands of 19 vertices, for which QP solvers can take a few milliseconds to 20 assess a collision, as we experimentally highlight in Sec. IV-C. [START_REF] Bergen | A fast and robust gjk implementation for collision detection of convex objects[END_REF] Having established that solving collision detection requires 22 dedicated methods, we turn our attention to solutions such 23 as GJK, which has been shown to operate on a large class 24 of shapes, ranging from simple primitives to very complex 25 meshes.

Recasting the distance computation problem onto the 27 Minkowski difference. The first important idea of 1988's 28 paper by Gilbert, Johnson, and Keerthi [START_REF] Gilbert | A fast procedure for computing the distance between complex objects in three-dimensional space[END_REF] is to recast the dis-29 tance computation problem onto the Minkowski difference D 30 of the shapes and defined as follows:

31 D = A 1 -A 2 = {x = x 1 -x 2 | x 1 ∈ A 1 , x 2 ∈ A 2 } ⊂ C , (3)
where C = R n is the so-called collision space. The shapes A 1 32 and A 2 lie in the shape space and the Minkowski difference D 33 lies in the collision space.

34

Although both the shape space and the collision space 35 are isomorphic to R n , we distinguish between the two to 36 highlight the change in perspective. In Fig. 2, we illustrate 37 the link between a pair of two convex shapes and their corre-38 sponding Minkowski difference. We stress that the Minkowski 39 difference D is specific to shapes A 1 and A 2 . If the relative 40 position or relative orientation between A 1 and A 2 changes, 41 their Minkowski difference changes accordingly.

42

The following properties, illustrated in Fig. 2, hold for the 43 Minkowski difference D: 44 1) Since A 1 and A 2 are convex sets, D is also convex.

* = x * 1 -x * 2 separates A 1 from A 2 .
It is also equal to the projection of 0 C onto the Minkowski difference D, x * = proj D (0 C ). In (b), the shapes overlap, thus 0 C ∈ D. In this case, we have

x * = proj D (0 C ) = 0 C .
x * 1 ∈ A 1 and x * 2 ∈ A 2 , also called witness vectors in the computational geometry literature [START_REF] Ericson | Real-time collision detection[END_REF]. Contrary to x * , these vectors x * 1 and x * 2 are not necessarily unique, as is the case for non-strictly convex shapes such as two parallel boxes.

4) Finally, we always have

∥x * ∥ = dist(A 1 , A 2 ).
This final remark allows us to recast the distance computation problem (1) onto the Minkowski difference as follows:

d 1,2 = min x∈D ∥x -0 C ∥ 2 = min x∈D ∥x∥ 2 . (4)
The convex optimization problem (4) is equivalent to (1)

and is known as a Minimum-Norm Point problem in the optimization literature [START_REF] Wolfe | Finding the nearest point in a polytope[END_REF], [START_REF] Lacoste-Julien | On the global linear convergence of frank-wolfe optimization variants[END_REF], [START_REF] Bach | Learning with submodular functions: A convex optimization perspective[END_REF]. In our case, 0 C ∈ C = R n is the null vector i.e. the origin of the collision space. We thus aim at finding the point in D with the lowest norm. This vector x * is the optimal solution to (4), given by

d 1,2 = ∥x * ∥ 2 = dist(A 1 , A 2 ) 2 .
Directly computing the Minkowski difference D is neither analytically tractable nor computationally efficient. Most of the first and second-order methods for constrained convex optimization problems, such as projected gradient descent or interior point methods [START_REF] Boyd | Convex Optimization[END_REF], are thus sub-optimal choices.

However, computing support vectors of the Minkowski difference D, a notion defined hereinafter in this section, is relatively simple and largely demonstrated by [START_REF] Gilbert | A fast procedure for computing the distance between complex objects in three-dimensional space[END_REF]. As we discuss next, solving convex optimization problems by computing support vectors is the strengh of the Frank-Wolfe algorithm and its variants [START_REF] Jaggi | Revisiting frank-wolfe: Projection-free sparse convex optimization[END_REF].

Distance computation using the Frank-Wolfe algorithm.

The Frank-Wolfe algorithm (FW) [START_REF] Frank | An algorithm for quadratic programming[END_REF] is one of the oldest Algorithm 1 Frank-Wolfe algorithm with linesearch [START_REF] Jaggi | Revisiting frank-wolfe: Projection-free sparse convex optimization[END_REF] Let x 0 ∈ D, ϵ > 0 For k=0, 1, ... do

1: d k = ∇f (x k ) ▷ Direction of support 2: s k ∈ arg min s∈D ⟨d k , s⟩(= S D (d k )) ▷ Support (8) 3: If g F W (x k ) ≤ ϵ , return f (x k ) ▷ Duality gap (16) 4: γ k = arg min γ∈[0,1] f (γx k + (1 -γ)s k ) ▷ Linesearch 5: x k+1 = γ k x k + (1 -γ k )s k
▷ Update iterate In the case of the distance computation problem (4), where f (x) = ∥x∥ 2 , line 4-5 correspond to projecting 0 C on the segment [x k , s k ]:

6: x k+1 = proj [x k ,s k ] (0 C ) ▷ Project 0 C on [x k , s k ] Fig. 3.
Computing a support vector s k in direction ∇f (x k ) on convex set D. We illustrate with the example of distance computation. On the left, we draw the Minkowski difference D of which point of minimum norm (MNP) is x * i.e. x * is the projection of 0 C onto D, x * = proj D (0 C ). The iterate at iteration k of the FW algorithm is x k . In purple we draw the level sets of the function f (x) = ∥x∥ 2 . On the right, we draw in purple the level sets of the linearization of f at iterate x k , h k . The first step of the FW algorithm is to compute support vector s k in the direction of ∇f (x k ) (green arrow), s k ∈ S D (∇f (x k )). In the second step of the FW algorithm, we compute x k+1 as a convex combination of x k and s k i.e. x k+1 is a point on the segment [x k , s k ].

convex optimization methods and solves the following con-1 strained optimization problem:

2 f (x * ) = min x∈D f (x), (5)
where f : R n → R is a convex and differentiable function and orientation in space. We denote c 1 ∈ A 1 and c 2 ∈ A 2 the 10 origins of the frames attached to A 1 and A 2 , respectively. In 11 the rest of this paper, we take

x 0 = c 1 -c 2 .
The FW algorithm, summarized in Alg. 1, is a gradient-13 descent method. It consists in iteratively applying two steps in 14 order to converge towards the optimal solution x * of (5). If 15 we denote by x k the estimate of x * at iteration k, these two 16 steps correspond to: 1) First, we compute a support vector s k in the direction [START_REF] Tedrake | Drake: Model-based design and verification for robotics[END_REF] of ∇f (x k ), by solving a linear optimization problem 19 on D. 2) Second, we update our current iterate x k to obtain x k+1 , 21 by taking a convex combination of the current iterate x k

In the following, we detail these steps in the context of distance computation. At iteration k, the current iterate x k is the estimate of the optimal solution x * and f (x k ) is the estimate of the optimal value of (5), f (x * ), at iteration k. We write the linearization of the function f at x k and denote it as h k :

h k (s) = f (x k ) + ⟨∇f (x k ), s -x k ⟩ (6)
where s is a vector of R n , ∇f (x k ) is the gradient of f at x k and ⟨•, •⟩ denotes the dot product between two vectors of R n .

→ Step 1. The first step of the FW algorithm at iteration k consists of finding a minimizer s k ∈ D of h k on the convex set D (line 2 in Alg. 1). Such a vector s k is called a support vector of D or simply a support and is defined as follows:

s k ∈ arg min s∈D h k (s) = arg min s∈D ⟨∇f (x k ), s⟩ . (7)
Fig. 3 gives a graphical understanding of support s k . The vector s k belongs to D and is in the most opposite direction w.r.t. ∇f (x k ). In order to highlight the importance of the direction in which a support s k is computed, we now introduce the notion of support direction and support function. Given a support direction d ∈ R n , the support function S D returns a set of D and is defined as:

S D (d) = arg min s∈D ⟨d, s⟩ ⊂ D . (8)
The support function S D may return a set with more than one vector. We only need to use one vector of this set. Thinking in terms of the direction of support allows us to understand that this direction can be rescaled while preserving the output of the support function:

∀d ∈ R n , ∀α > 0, S D (αd) = S D (d). (9)
A support s k ∈ D at iteration k is thus computed in the direction d k = ∇f (x k ) and belongs to S D (∇f (x k )), s k ∈ S D (∇f (x k )).

We now explain how to compute the support vector s k in the case of the distance computation problem (4) where we minimize f (x) = ∥x∥ 2 on the Minkowski difference D of A 1 and A 2 . First, we have ∇f (x) = 2x. Therefore, in the case of problem (4), it follows that:

s k ∈ S D (x k ) = arg min s∈D ⟨x k , s⟩ . (10)
As demonstrated by [START_REF] Gilbert | A fast procedure for computing the distance between complex objects in three-dimensional space[END_REF], any vector s ∈ S D (d) related to the Minkowski difference can be decomposed as the difference be-

tween two support vectors s A1 ∈ S A1 (d) and s A2 ∈ S A2 (-d)
over the two individual shapes, leading to the following relation:

s = s A1 -s A2 ∈ S D (d). (11)
Equation [START_REF] Frank | An algorithm for quadratic programming[END_REF] shows that we can construct a support of the Minkowski difference from the supports of the original shapes.

This property highlights the powerful change of perspective of working on the Minkowski difference. Indeed, there exists a large number of shapes for which computing supports is simple: spheres, ellipsoids, cylinders, capsules, polytopes etc. [START_REF] Ericson | Real-time collision detection[END_REF], [START_REF] Bergen | A fast and robust gjk implementation for collision detection of convex objects[END_REF], [START_REF] Van Den Bergen | Collision detection in interactive 3D environments[END_REF]. Fig. 4 illustrates the construction of a support of the Minkowski difference D using the supports of the original shapes A 1 and A 2 .

4 x k+1 = γ k x k + (1 -γ k )s k , (12)
where γ k = k+1 k+2 ∈ [0, 1] controls the step size. Alternatively, 5 a line search can be carried out to find a better iterate x k+1 6 (line 4 in Alg. 1):

7 γ k = arg min γ∈[0,1] f (γx k + (1 -γ)s k ) x k+1 = γ k x k + (1 -γ k )s k . (13)
In the distance computation case where f (x) = ∥x∥ 2 , this 8 linesearch ( 13) is equivalent to projecting 0 C onto the seg- 12) and ( 13) updates are guaranteed to remain 11 in D.

9 ment [x k , s k ], x k = proj [x k ,s k ] (0 C ) (line 4 in Alg. 1). Since D 10 is convex, both (
12 Stopping criteria. As Frank-Wolfe deals with convex prob-13 lems, the duality gap associated with problem (5) can be used 14 as a stopping criterion. Due to its convexity, the function f is 15 always above its linearization. Otherwise said, for any x ∈ R n 16 and any s ∈ R n :

17 f (s) ≥ f (x) + ⟨∇f (x), s -x⟩ . ( 14
)
Reworking this inequality and applying the min operator en-18 ables us to compute the Frank-Wolfe duality gap g FW (x) ∈ R + 19 which gives an upper-bound on the difference

f (x) -f (x * ): 20 f (x) -f (x * ) ≤ -min s∈D ⟨∇f (x), s -x⟩ = g FW (x) . ( 15
)
In particular, at iteration k of the FW algorithm, we have:

21 f (x k ) -f (x * ) ≤ g FW (x k ) = ⟨∇f (x k ), x k -s k ⟩ , (16)
where s k ∈ S D (∇f (x k )) is the support vector computed at it-22 eration k in the direction of ∇f (x k ). The duality-gap g FW (x k ) 23 serves as a convergence criterion for the Frank-Wolfe method 24 and is cheap to compute. Applied to the distance computation 25 problem (4), the duality gap at iteration k, g FW (x k ), guarantees 26 that:

27 ∥x k ∥ 2 -∥x * ∥ 2 ≤ g FW (x k ) = 2⟨x k , x k -s k ⟩. ( 17
)
Algorithm 2 Boolean collision checking: separating plane condition Insert after line 2 in Alg. 1:

1: If max(0, ⟨d k / ∥d k ∥ , s k ⟩) > ϵ col , return False If after termination d 1,2 ≤ ϵ col , return True, otherwise return False.
Using the triangular inequality of the Euclidian norm and the convexity of the Minkowski difference D, we can show that:

∥x k -x * ∥ 2 ≤ ∥x k ∥ 2 -∥x * ∥ 2 ≤ g FW (x k ) . (18)
Inequality ( 18) is useful in practice as it allows the fine control of the desired tolerance on the distance to the optimal solution x * (line 3 in Alg. 1). Indeed, if ones wants to compute an estimate x of the optimal solution x * at precision ϵ,

meaning that ∥x -x * ∥ ≤ √ ϵ, it is sufficient to check that g FW (x) ≤ ϵ.
Boolean collision checking. As mentioned earlier, the problem of distance computation encompasses the problem of collision checking. Indeed, in collision checking, we are only interested in finding a separating plane between A 1 and A 2 , if it exists. This is equivalent to finding a separating plane between D and 0 C . For any support direction d, if we have:

⟨d, s⟩ > 0, s ∈ S D (d), (19)
then the plane supported by the vector d separates D from 0 C [START_REF] Bergen | A fast and robust gjk implementation for collision detection of convex objects[END_REF]. This also means that, in the case where the two shapes intersect, collision checking has the same computational complexity as distance computation. In general, at iteration k, the value max(0, ⟨d k / ∥d k ∥ , s k ⟩) is a lower bound on the distance between the shapes, dist(A 1 , A 2 ), and it can be used as a stopping criterion in the boolean collision check.

As soon as the lower bound on dist(A 1 , A 2 ) is guaranteed to be positive, the algorithm can be stopped. Otherwise, the algorithm continues until the stopping criterion defined by ( 18 This corresponds to a precision on the distance between shapes on the order of micro-meters ( √ ϵ = 10 -6 m). In robotics applications where the entire pipeline is no more precise than millimeters, it is often sufficient to consider that below a threshold of 10 -4 m (i.e ϵ = 10 -8 ) the shapes are in collision;

above that threshold the shapes are not in collision. In our applications, we use ϵ col = √ ϵ.

Computing support vector on meshes. While the support vector of basic primitives (sphere, ellipsoid, box, etc.) presents closed-form solutions, this is not the case for meshes. In the case of convex meshes, an efficient approach for computing the Algorithm 3 Frank-Wolfe algorithm with line-search (see Alg. 1) rewritten with active-sets and applied to the distance computation problem (4)

Let x 0 ∈ D, W 0 = {x 0 }, ϵ > 0 For k=0, 1, ... do 1: d k = x k ▷ Direction of support 2: s k ∈ S D (d k ) ▷ Support (8) 3: If g F W (x k ) ≤ ϵ , return f (x k ) ▷ Duality gap (16) 4: W k+1 = W k ∪ {s k } ▷ Augment active-set 5: x k+1 = proj conv( W k+1 ) (0 C ) ▷ Project 0 C on conv( W k+1 ) 6: W k+1 = {x k+1 } ▷ Update active-set
support direction of meshes is the hill-climbing algorithm [START_REF] Van Den Bergen | Collision detection in interactive 3D environments[END_REF],

1
which allows retrieving the supporting vertex or face of the 2 meshes thanks to a simple neighbor-descent procedure. Yet,

3
this procedure is sensitive to the initial-guess solution. By Sec. IV that this helps the hill-climbing algorithm to perform 8 less iterations in practice, leading to faster computation times.

9

The Frank-Wolfe active-set. As with many gradient-descent 10 algorithms, the FW method tends to zig-zag towards the 11 optimal solution [START_REF] Lacoste-Julien | On the global linear convergence of frank-wolfe optimization variants[END_REF], slowing down the convergence to the 12 optimum. This behavior is undesired and amplified if the 13 optimal solution x * lies close to the boundary of the constraint 14 set D. In collision detection, this corresponds to the case 15 where the two shapes are not intersecting. This zig-zagging 16 behavior is due to the way that Frank-Wolfe approaches the 17 set of active constraints [START_REF] Lacoste-Julien | On the global linear convergence of frank-wolfe optimization variants[END_REF], also called active-set in the 18 optimization literature [START_REF] Boyd | Convex Optimization[END_REF]. In the FW setting, the active set 19 at iteration k, denoted W k = {s 0 , ..., s r } ⊂ D, is the set 20 of vectors in D used by the algorithm to maintain a convex 21 combination of the iterate x k :

22 x k = r i=0 λ i s i , r i=0 λ i = 1 with s i ∈ W k ⊂ D and λ i > 0.
(20) In Alg. 3, we rewrite the FW algorithm with line search 23 (Alg. 1) in order to highlight the notion of active set:

24 • At iteration k, the active-set is only composed 25 of x k , W k = {x k }.

34

• Finally, the active-set is updated W k+1 = {x k+1 } (line 6 35 in Alg. 3).

36

In practice, discarding previously computed supports when 37 updating the active set is inefficient and causes the zig-38 zagging phenomenon observed in the FW algorithm [START_REF] Lacoste-Julien | On the global linear convergence of frank-wolfe optimization variants[END_REF]. In 39 the optimization literature, a rich and wide variety of variants 40 of the FW algorithm have been introduced to efficiently cope with the active set in order to improve the convergence rate of the FW method [START_REF] Wolfe | Finding the nearest point in a polytope[END_REF], [START_REF] Garber | Playing non-linear games with linear oracles[END_REF], [START_REF] Guélat | Some comments on wolfe's 'away step[END_REF], [START_REF] Holloway | An extension of the frank and wolfe method of feasible directions[END_REF], [START_REF] Kerdreux | Restarting frank-wolfe[END_REF]. However, these variants remain too generic and are not suited for the specific problem of collision detection. In the following, we propose instead incorporating the active-set strategy used in GJK within the Frank-Wolfe setting.

Connection between GJK and Frank-Wolfe. In the case of collision detection, [START_REF] Gilbert | A fast procedure for computing the distance between complex objects in three-dimensional space[END_REF] developed an efficient strategy to handle the active set at a minimal cost. To represent the current estimate x k and the optimal solution x * , GJK exploits the concept of simplexes in R 3 . A simplex in R n corresponds to a set containing at most n + 1 vectors of R n , and the rank r of a simplex is the number of vectors it contains (0 < r ≤ n+1). For 3-dimensional spaces, a simplex corresponds either to a point (r = 1), a segment (r = 2), a triangle (r = 3), or a tetrahedron (r = 4). Similarly to the simplex methods for Linear Programming [START_REF] Dantzig | Linear programming and extensions[END_REF], the Carathéodory theorem [START_REF] Carathéodory | Über den variabilitätsbereich der koeffizienten von potenzreihen, die gegebene werte nicht annehmen[END_REF] motivates the use of simplexes. Let Y be a set of N ≥ n vectors in R n , Y = {y i ∈ R n } 0≤i≤N . The Carathéodory theorem states that any vector x ∈ conv(Y) can be expressed as the convex combination of at most n + 1 vectors of Y:

x = r j=0 λ j y j , with y j ∈ Y, λ j > 0, r i=0 λ j = 1. (21)
Hence, any vector in D, and particularly the optimal solution x * ∈ D = conv(D) of the distance computation problem (4), can be identified as a convex combination of the vectors composing a simplex W . Relying on simplexes is attractive as there is no need to run any algorithm to compute the convex hull of a simplex as they are convex by construction. Frank-Wolf algorithms may operate on more complex active sets, which might become hard to tackle from a computational point of view [START_REF] Jaggi | Revisiting frank-wolfe: Projection-free sparse convex optimization[END_REF], [START_REF] Lacoste-Julien | On the global linear convergence of frank-wolfe optimization variants[END_REF]. In other words, the problem of finding the optimal solution x * can be reformulated as the problem of identifying the optimal simplex W * on which x * can be decomposed into a convex combination. This is precisely the approach followed by GJK that we now detail as well as illustrate in Fig. 5.

At iteration k of GJK, the current iterate x k is a convex combination of the vectors composing the simplex W k of rank r k ≤ n. This corresponds to Fig. 5a. To update x k and W k , the following procedure is applied:

• After computing support vector s k (line 2 in Alg. 3, illustrated in Fig. 5b), we add

s k to W k to ob- tain W k+1 = W k ∪ {s k } (line 4 in Alg. 3). The set W k+1
is now a simplex of rank r k+1 ≤ n + 1, as shown in Fig. 5c.

• We then minimize function f (x) = ∥x∥ 2 on W k+1 to obtain x k+1 , corresponding to projecting 0 C onto W k+1 : x k+1 = proj conv( W k+1 ) (0 C ) 1 (line 5 in Alg. 3
). This projection is illustrated in figures 5c and 5d.

• We then have two cases, summarized in Alg 4: this face. This ensures that W k+1 is necessarily of 10 rank r k+1 < r k+1 ≤ n + 1. For example, Fig. 5d 11 shows the result of the simplex update obtained in 12 Fig. 5c.

D x k x x x 0 C 0 0 0 W k (a) D x k x x x 0 C 0 0 0 W k s k s s s (b) D x k x x x 0 C 0 0 0 Wk+1 s k s s s (c) D x k+1 x x x 0 C 0 0 0 W k+1 (d)

13

Through this discussion, it is clear that GJK is a particular 14 case of Frank-Wolfe. More specifically, it is a sub-case of the 15 fully-corrective Frank-Wolfe algorithm analyzed by [START_REF] Lacoste-Julien | On the global linear convergence of frank-wolfe optimization variants[END_REF]. The 16 strategy used by GJK to handle the active set has proved to 17 be very efficient in practice and renders the GJK algorithm 18 state-of-the-art for collision detection. In the next section, [START_REF] Smith | Github: The open dynamics engine (ODE)[END_REF] we propose to leverage the formulation of collision detection 20 as a Frank-Wolfe sub-case to accelerate its convergence fol-21 lowing the well-established Polyak and Nesterov acceleration 22 paradigm [START_REF] Nesterov | A method of solving a convex programming problem with convergence rate o\bigl(kˆ2\bigr)[END_REF].

23

III. ACCELERATING COLLISION DETECTION

24

Gradient descent (GD) is the backbone of many convex 25 optimization methods and relies solely on the gradient of 26 the objective function. Second-order methods [START_REF] Boyd | Convex Optimization[END_REF], such as 27 Newton methods, have faster convergence rates than GD at 28 the price of requiring the computation and the inversion of 29 Hessian quantities. Momentum methods have thus been intro-30 duced in the optimization literature to provide gradient-based 31 methods with improved convergence rates without requiring 32 Algorithm 4 Fully-corrective FW using simplexes, applied to the distance computation problem (4). This algorithm is identical to GJK [START_REF] Gilbert | A fast procedure for computing the distance between complex objects in three-dimensional space[END_REF] In Alg. 3, let W 0 = ∅ and replace line 6 by:

1: If x k+1 = 0 C , return 0
If the algorithm has not terminated, update W k+1 to retain only the smallest number of vectors needed to express x k+1 : 2: W k+1 = {s 1 , ..., s r } where s 1 , ..., s r are the smallest number of vectors in W k+1 such that x k+1 is a convex combination of s 1 , ..., s r .

costly Hessian evaluation. In this section, we use recent work linking the Polyak and Nesterov accelerations of GD to the FW algorithm [START_REF] Li | Heavy ball momentum for conditional gradient[END_REF], [START_REF] Li | A momentumguided frank-wolfe algorithm[END_REF] to globally accelerate collision detection. These global accelerations of collision detection are experimentally evaluated in Sec. IV on several benchmarks.

A. Background on acceleration methods in convex optimization

Polyak acceleration for unconstrained optimization. We initially consider the following unconstrained minimization problem:

f (x * ) = min x∈R n f (x) , (22)
where f : R n → R is a convex and differentiable function. The vanilla gradient-descent algorithm follows the slope of f given by its gradient ∇f . The following scheme is applied iteratively until a given convergence criterion is met (e.g., ∥∇f (x k )∥ < ϵ, with ϵ being the desired precision):

x k+1 = x k + α k ∇f (x k ), (23)
where x k ∈ R n is the current iterate and α k ∈ R is the gradient step. This standard setting leads to a simple implementation with linear convergence rate (O(1/k)).

To go beyond this linear convergence regime, acceleration techniques have been devised in the optimization community to provide quadratic convergence rate (O(1/k 2 )) or more [START_REF] Aspremont | Acceleration methods[END_REF], by relying on relatively cheap gradient evaluations. Among these gradient-descent acceleration techniques, the Polyak (or Heavy-Ball) [START_REF] Polyak | Some methods of speeding up the convergence of iteration methods[END_REF] and Nesterov acceleration [START_REF] Nesterov | A method of solving a convex programming problem with convergence rate o\bigl(kˆ2\bigr)[END_REF] are two of the better-studied and most popular in practice [START_REF] Aspremont | Acceleration methods[END_REF]. These techniques are based on accumulating previously computed gradients in a momentum term d k and using this momentum d k to update the current iterate x k . The Polyak update scheme for unconstrained gradient descent is illustrated in Fig. 6a and goes as follows:

d k = δ k d k-1 + α k ∇f (x k ) (24a) x k+1 = x k + d k , (24b)
where schemes δ k ∈ R is the momentum parameter. The role of momentum d k is to smooth the trajectory of iterates converging towards the optimum by geometrically averaging previously computed gradients. The δ k momentum parameter is selected to prevent damping or overshooting of the iterate trajectory when going towards the optimal solution x * . Nesterov acceleration for unconstrained optimization. The Nesterov update scheme is the second most well-known method for accelerating unconstrained gradient descent and it is only a slight modification on top of the Polyak scheme. Contrary to the Polyak case, in the Nesterov acceleration scheme the current iterate x k is extrapolated using the momentum term d k to compute the intermediate vector

x k ∇f(x k ) d k-1 x k+1 x* x x x x x x x x d d (a) x k ∇f(x k + δ k d k-1 ) d k-1 x k+1 x* x x x x x x d d x x d d (b)
y k = x k + δ k d k .
The gradient is then computed at the vector y k . The Nesterov update scheme for unconstrained gradient descent is illustrated in Fig. 6b and goes as:

y k = x k + δ k d k-1 (25a) d k = δ k d k-1 + α k ∇f (y k ) (25b) x k+1 = x k + d k (25c)
where δ k is the momentum parameter as in the Polyak scheme, In the original FW algorithm, the support vector at iteration k, s k , is computed in the direction of the gradient ∇f (x k ) (line 1 in Alg. 1). In the Polyak acceleration of FW proposed by [START_REF] Li | Heavy ball momentum for conditional gradient[END_REF], the direction of support for computing s k is instead defined by:

d k = δ k d k-1 + (1 -δ k )∇f (x k ) (26a) s k = S D (d k ), (26b)
where δ k = k+1 k+3 ∈ [0, 1] is the momentum parameter and S D is the support function as defined in [START_REF] Mamou | A simple and efficient approach for 3d mesh approximate convex decomposition[END_REF]. In the Nesterov acceleration of FW proposed by [START_REF] Li | A momentumguided frank-wolfe algorithm[END_REF], the direction of support for computing s k is slightly different from the Polyak scheme (26) as it introduces y k , an intermediary vector as in the GD Nesterov scheme (25) in order to evaluate the Algorithm 5 Polyak-accelerated and Nesterov-accelerated Frank-Wolfe [START_REF] Li | Heavy ball momentum for conditional gradient[END_REF], [START_REF] Li | A momentumguided frank-wolfe algorithm[END_REF] In Alg. 1 and Alg. 3, let d -1 = s -1 = x 0 , δ k = k+1 k+3 and replace line 1 by:

1: y k = x k Polyak δ k x k + (1 -δ k )s k-1 Nesterov 2: d k = δ k d k-1 + (1 -δ k )∇f (y k )
gradient ∇f (y k ):

y k = δ k x k + (1 -δ k )s k-1 (27a) d k = δ k d k-1 + (1 -δ k )∇f (y k ) (27b) s k = S D (d k ), (27c)
where s k-1 is the support vector computed at the previous 1 iteration. To ensure y k stays in D, it is a convex combination The works [START_REF] Li | Heavy ball momentum for conditional gradient[END_REF], [START_REF] Li | A momentumguided frank-wolfe algorithm[END_REF] have experimentally shown that these Adapting Nesterov and Polyak fully-corrective Frank-Wolfe to distance computation. Preserving GJK's simplex strategy is crucial for collision detection as it greatly speeds up the vanilla FW algorithm. Therefore, we adapt ( 26) and ( 27) accordingly as:

y k = x k if Polyak δ k x k + (1 -δ k )s k-1 if Nesterov (28a) d k = δ k d k-1 + (1 -δ k )∇f (y k ) (28b) s k = S D (d k ), (28c)
W k+1 = W k ∪ {s k }, (28d) x k+1 = proj conv( W k+1 ) (0 C ). (28e)
These steps are also summarized in Alg. 6. The update of simplex W k+1 from W k+1 is then identical to the one described in Alg. 4. The original duality gap defined in Sec. II (Eq. 16) can no longer be used as a convergence criterion. Indeed, the following inequality:

∥x k -x * ∥ 2 ≤ g FW (x k ) = 2⟨x k , x k -s k ⟩, s k ∈ S D (x k ),
is no longer valid because the support vector s k is no longer computed in the direction of the gradient ∇f (x k ) = 2x k .

16

Next we will show that the original stopping criterion devised 17 in Sec. II cannot be used and we need to derive a new one.

18

Algorithm 6 Polyak and Nesterov-accelerated GJK

Let x 0 ∈ D, W 0 = ∅, d -1 = s -1 = x 0 , ϵ > 0 For k=0, 1, ... do 1: δ k = k+1 k+3 ▷ Momentum parameter value 2: y k = x k Polyak δ k x k + (1 -δ k )s k-1 Nesterov ▷ Intermediary point (28a) 3: d k = δ k d k-1 + (1 -δ k )∇f (y k ) ▷ Support dir. (28b) 4: s k ∈ S D (d k )
▷ Support (8) 5: if g(x k ) ≤ ϵ then ▷ Fixed-point condition [START_REF] Qin | Smoothing algorithms for computing the projection onto a minkowski sum of convex sets[END_REF] 6: In order to cope with this issue, we use the following strategy. Suppose x k ̸ = 0 C . Since x k = proj conv(W k ) (0 C ) we have:

If d k = x k , return f (x k ) ▷ Algorithm terminates 7: s k ∈ S D (∇f (x k )) ▷ Compute s k in dir. ∇f (x k ) Replace line 3 by: d k = x k until termination. 8: W k+1 = W k ∪ {s k } ▷ Augment active-set 9: x k+1 = proj conv( W k+1 ) (0 C ) ▷ Project 0 C on conv( W k+1 ) 10: If x k+1 = 0 C ,
∀s i ∈ W k , ⟨x k , x k -s i ⟩ = 0. (29)
After computing s k ∈ S D (d k ), if we have:

⟨x k , x k -s k ⟩ ̸ = 0, (30)
then s k is not a linear combination of vectors in W k . Therefore, augmenting W k with s k to obtain W k+1 and projecting 0 C onto conv( W k+1 ) to obtain x k+1 will result in the algorithm progressing toward the optimum x * . Suppose on the contrary that:

⟨x k , x k -s k ⟩ = 0, (31)
then s k is a linear combination of vectors in W k . Adding s k to W k will thus not result in any progress towards the optimum. As a consequence, Eq. ( 31) encompasses two cases:

• If the support direction d k is aligned with ∇f (x k ), Eq. ( 31), corresponding to g FW (x k ) = 0, matches the termination criterion of the distance computation problem and therefore we have reached the optimum i.e x k = x * . • Otherwise, if d k is not aligned with ∇f (x k ), the algorithm cannot stop as a null duality gap is not met. The algorithm thus enters a cycle where it iterates until Eq. ( 31) does not hold. To cope with this undesired behavior we simply stop the Polyak or the Nesterov acceleration as soon as Eq. ( 31) is met and switch back to the non-accelerated version Alg. 4.

We thus define the function g such that for any s k ∈ D:

g(x k ) = 2⟨x k , x k -s k ⟩, (32)
g is used in Alg. 6 as an optimality criterion ( g ≤ ϵ)

either for stopping the Polyak and Nesterov accelerations in order to continue with the vanilla GJK, or as stopping criteria qualifying an optimal solution, in which case g = g FW and [START_REF] Tedrake | Drake: Model-based design and verification for robotics[END_REF] holds. The entire algorithm is summarized in Alg. 6.

Nesterov acceleration for non-strictly convex shapes. Let us explain the effect of the Nesterov acceleration on the support direction update (28b) and distinguish between strictly convex and non-strictly convex D:

• If D is
d k = δ k d k-1 ∥d k-1 ∥ + (1 -δ k ) ∇f (y k ) ∥∇f (y k )∥ , (33)
summarized in Alg. 7. In Sec. IV, we experimentally prove this heuristic to significantly reduce the number of steps for distance computations for non-strictly convex shapes. We also show that this heuristic does not need to be applied to the Polyak acceleration, as, contrary to the Nesterov acceleration, the Polyak acceleration does not compute an intermediary point y k .

Algorithm 7 Normalize direction for non-strictly convex shapes in Nesterov-accelerated GJK Replace line 3 in Alg. 6 by:

1: d k = δ k d k-1 ∥d k-1 ∥ + (1 -δ k ) ∇f (y k ) ∥∇f (y k )∥ IV. EXPERIMENTS 1
In this section, we study the performance of both Polyak for all variants of GJK. The benchmark of non-strictly convex 10 shapes represents shapes typically used in robotic or computer 11 graphics applications. In Sec. IV-C, we benchmark GJK and 12 our proposed accelerated gradients against the state-of-the-art 13 quadratic programming solver ProxQP [START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF]. We show that 14 GJK and our proposed accelerated variants vastly outperform 15 generic quadratic programming (QP) solvers, making these QP 16 solvers prohibitive for collision detection. Then, in Sec. IV-D, 17 we compare our methods and our implementation of vanilla 18 GJK against different collision detection solvers of various col-19 lision detection libraries. Finally, in Sec. IV-E, we benchmark 20 vanilla, Polyak-accelerated, and Nesterov-accelerated GJK on 21 a dataset of trajectories obtained using a physics simulator. [START_REF] Van Den Bergen | Proximity Queries and Penetration Depth Computation on 3D Game Objects[END_REF] We show that, similarly to vanilla GJK, our accelerated GJK 23 algorithms can benefit from being warm-started with previ-24 ous simulation time steps, outperforming the vanilla GJK in 25 physics simulation scenarios.

26

Implementation. We leverage the HPP-FCL C++ library [START_REF] Pan | Fcl: A general purpose library for collision and proximity queries[END_REF], 27 [START_REF] Pan | Hpp-fcl: an extension of the flexible collision library[END_REF], an extension of the original FCL library [START_REF] Pan | Fcl: A general purpose library for collision and proximity queries[END_REF] Shapes datasets. To distinguish between pairs of strictly con-37 vex and non-strictly convex shapes, we build a first benchmark 38 only composed of pairs of ellipsoids (strictly convex shapes) 39 and a second benchmark using pairs of standard meshes 40 (represented by their convex hulls) which are taken from the 41 commonly-used YCB dataset [START_REF] Calli | The ycb object and model set: Towards common benchmarks for manipulation research[END_REF].

Ellipsoids. In the ellipsoids benchmark, the ellipsoids are 43 randomly generated by sampling positive-definite matrices. In 44 total, we generate 1000 random pairs of ellipsoids. Given a 45 pair of ellipsoids, we randomly sample relative poses between 46 the shapes, using a uniform distribution for the relative rotation 47 between the shapes. Regarding the translation part of the 48 random poses, the directions are selected at random, but the 49 norms are chosen so that we control the distance dist(A 1 , A 2 ) 50 between the objects. This enables us to measure the influence of the separation distance on the performance of the studied algorithms. The values used for dist(A 1 , A 2 ) range from -0.1 m to 1 m. Negative values correspond to scenarios where the shapes intersect, with dist(A 1 , A 2 ) corresponding to the separating vector's norm. The separating vector is the vector of the smallest norm needed to translate one of the two shapes such that the two shapes do not intersect. Therefore, for each pair of ellipsoids, 100 random relative poses are sampled, so the shapes do not intersect. We translate the shapes along the axis given by their closest points for each relative pose to study the impact of dist(A 1 , A 2 ). We then set dist(A 1 , A 2 ) to fixed values between -0.1 m to 1 m.

YCB meshes. On the other hand, the YCB mesh dataset contains about 60 shapes commonly used for robotic manipulation tasks (kitchen appliances, tools, toys, etc.). Each object has three different resolution levels corresponding to the number of points representing the mesh. For each object, we take the lowest resolution, i.e., the google-16k versions of the meshes, as it is resolute enough for any robotic task. As GJK-like algorithms work on convex shapes, we pre-compute the convex hulls of each object in the YCB dataset. This procedure needs only to be done once; if more precision is required for a certain robotic task, it is common to decompose a non-convex object into a set of convex sub-objects. For the sake of simplicity, we will not decompose YCB objects into sub-objects, as the results presented in this section would essentially be the same. In the rest of this section, when we mention a shape, we refer to its convex hull unless explicitly stated otherwise. The resulting meshes extracted from the YCB dataset contain between 100 and 8000 vertices. About 50% of meshes contain between 100 and 1000 vertices. As in the ellipsoids benchmark, 100 random relative poses are sampled for each pair such that the shapes do not intersect and then set dist(A 1 , A 2 ) to fixed values between -0.1 m and 1 m.

In both benchmarks (ellipsoids and YCB meshes), the characteristic sizes of the shapes range from a few centimeters up to a meter. Finally, for the distance computation problem, we select a convergence tolerance of ϵ = 10 -8 .

Initialization strategy. Apart from Sec. IV-E, the GJK algorithm and our proposed accelerated GJK algorithms are initialized with the centers of the shapes' bounding boxes. The bounding box of a shape fully encapsulates it, as is shown in Fig. 1. Hence, if we denote c 1 and c 2 the geometric centers of the bounding boxes of A 1 and A 2 , then we initialize vanilla GJK, Polyak-accelerated GJK and Nesterov-accelerated GJK

to x 0 = c 1 -c 2 .
Metrics. To measure the performances of Poylak-accelerated GJK, Nesterov-accelerated GJK, and the vanilla GJK algorithms, we measure the number of iterations N k to solve a given collision problem. For the mesh benchmark, we also measure the execution time T µ of both methods. We solve each generated collision problem 100 times to cope with CPU throttling. We then report the average of the 90% lowest computation times. All the benchmarks in this paper were run on an Apple M1 Max CPU.

-10 -1 -10 -3 0 10 -3 10 -1 The curve shows the mean value over 100,000 random trials. The shaded region corresponds to the standard deviation. The Nesterov-accelerated GJK algorithm requires fewer iterations when the shapes are in close proximity. The Polyak-accelerated GJK algorithm is more robust when shapes are strongly overlapping or distant.

dist(A 1 , A 2 ) [m]
A. Worst case scenario: strictly convex shapes -ellipsoids

1
We first focus on the ellipsoid benchmark to get a statistical 2 understanding of the performance of Polyak and Nesterov-3 accelerated GJK against vanilla GJK. In the following, we 4 explain why these shapes are interesting to study experimen-5 tally, as they represent the worst-case scenario that GJK-6 like algorithms can be confronted with. First, as previously 7 explained, GJK-like algorithms look for the optimal active set 8 of the solution x * . Otherwise said, GJK-like methods find 9 a set of support points W * = {s 1 , s 2 , ...} such that the 10 optimal solution x * is a convex combination of the points 11 of W * , where s i are support points computed while running 12 GJK or our proposed accelerations. Then, contrary to non 13 strictly-convex shapes, strictly-convex shapes have an infinite 14 amount of support points. As explained at the end of Sec. III, 15 each normalized support direction d corresponds to a unique 16 support point S D (d). Therefore, it is fundamentally harder 17 to identify the optimal active set when considering strictly-18 convex shapes, as there is an infinite amount of potential 19 support points to consider. In contrast, there is a finite amount 20 of support points to consider when using non strictly-convex 21 shapes.

22

In Fig. 7, we show the performance of the vanilla, the 23 Polyak-accelerated, and the Nesterov-accelerated GJK algo-24 rithms on the ellipsoids benchmark. Fig. 7a and Fig. 7b 25 show the mean and standard deviation of the number of 26 iterations N k of each method for the distance computation and 27 the Boolean collision checking problems, respectively. When 28 the shapes are shallowly intersecting, Polyak and Nesterov-29 accelerated GJK converge with the same or even fewer number 30 of iterations than vanilla GJK. However, the shallower the 31 penetration, the more Polyak and Nesterov accelerate over 32 vanilla GJK, with Nesterov providing the most acceleration. [START_REF] Gilbert | An iterative procedure for computing the minimum of a quadratic form on a convex set[END_REF] The irregularity in standard deviation at -0.01 m is a crit-34 ical zone for the Nesterov momentum where the variance 35 increases. When shapes are in close proximity, the Nesterov 36 acceleration of GJK significantly reduces the number of 37 iterations compared to vanilla GJK and Polyak-accelerated 38 GJK. Finally, when shapes are distant, 1 m ≤ dist(A 1 , A 2 ), 39 the Nesterov acceleration is detrimental to convergence on 40 the distance computation problem while Polyak-accelerated 41 Here, the two shapes are in close-proximity:

0 m < dist(A 1 , A 2 ) ≤ 0.1 m.
Normalizing the support direction benefits Nesterov-accelerated GJK, reducing the overall number of iterations compared to GJK and non-normalized Nesterov-accelerated GJK.

GJK remains competitive against vanilla GJK. This indicates that the Polyak acceleration is generally more robust than the Nesterov acceleration. However, it offers less acceleration over vanilla GJK when the shapes are in close-proximity or shallowly overlapping. A similar pattern of speed-ups of Polyak and Nesterov-accelerated GJK over vanilla GJK is shown for the collision detection problem in Fig. 7b.

B. Non-strictly convex shapes: meshes

Effect of support direction normalization. For meshes, the importance of normalizing the support direction (see Eq. ( 33))

in the Nesterov-accelerated GJK is highlighted in Fig. 8. For both the distance computation and Boolean collision checking problems, the normalization heuristic prevents the Nesterov acceleration from reaching a fixed point too early, and consequently, it reduces the overall amount of iterations needed to converge. This is, however, not the case for the Polyakaccelerated GJK algorithm, which does not benefit from support normalization. As explained at the end of Sec. III, the Polyak acceleration does not compute an intermediary point, unlike the Nesterov acceleration scheme. In the following, we thus focus only on Polyak-accelerated GJK without support normalization and Nesterov-accelerated GJK with support normalization. We compare the performances of these two algorithms against the vanilla GJK algorithm.

Statistical validation over the YCB dataset. In Fig. 9 and From this benchmark involving shapes from the YCB dataset, we can distinguish two use cases in which one would prefer using Polyak-accelerated GJK compared to Nesterovaccelerated and vice-versa. In tasks where the exact distance between the shapes needs to be computed and where this distance separating the shapes can take any value, due to its robustness, the Polyak-accelerated GJK algorithm is better suited than its Nesterov counterpart. However, in a situation involving shapes interacting at close proximity, like in a contact physics simulation, it is preferable to choose the Nesterov-accelerated GJK. Before studying the performance of GJK and our proposed accelerations for physics simulation, we first show the benefits of using GJK-based algorithms for collision detection instead of standard off-the-shelf optimization solvers and provide a comparison of the implementations of our methods against baselines from other collision detection librairies. All the libraries listed before are written either in C or in C++.

In Fig. 12, we report the performance of the methods listed before on the YCB benchmark used in Sec. IV-B.

We measure the execution time of the different collision detection solvers on the boolean collision check task and divide the results into three distance categories: when shapes are distant (dist(A 1 , A 2 ) ≥ 1m), when shapes are in closeproximity (0m ≤ dist(A 1 , A 2 ) ≤ 0.1m) and when shapes are overlapping (dist(A 1 , A 2 ) ≤ 0m). The collision problems corresponding to the distant case would typically be filtered by the broadphase. We report them to give a clear picture of the performance of the different implementations. In practice, the close-proximity and overlapping cases correspond to situations when the different collision detection solvers are actually called; the broad phase cannot filter such collision problems and the narrow phase is then called.

The results show our implementation of GJK and our proposed methods outperform the solvers of the other collision detection libraries. We find similar results for the distance computation task. We now turn our attention to the context of physics simulation and show that our proposed methods, just like GJK, can be warm-started by using previous simulation steps.

E. Collision detection for physics simulation

In the previous benchmarks, we have experimentally shown the improvement of our methods, Polyak-accelerated GJK and Nesterov-accelerated GJK, over the vanilla GJK algorithm for collision problems which are important in practice, i.e. when the broadphase has not filtered collision pairs and are thus overlapping or in close proximity. So far, the benchmarks have been constructed by randomly selecting poses for our shapes. However, in robotics applications such as trajectory optimization, motion planning, or computer graphics, the successive poses between objects are usually correlated by time. In this sub-section, we study how vanilla GJK, Polyakaccelerated GJK, and Nesterov-accelerated GJK can be warmstarted using the previous time instant, as occurring inside physics simulators.

To do so, we create a dataset of trajectories using pairs of objects from the YCB dataset used in Sec. IV-B. We randomly select 1000 pairs of YCB objects and drop them in a funnel as shown in Fig. 14. At the beginning of the simulation, each object is given a random pose and random translational and rotational velocities. The simulation is then run at 120Hz for 1 second. When a collision occurs, the GJK and EPA (expanding polytope algorithm) algorithms are called to determine the position of the contact points and the corresponding normal for the considered pair of objects. The collision is then resolved using a contact solver based on the Projected Gauss-Seidel [START_REF] Brogliato | Numerical simulation of finite dimensional multibody nonsmooth mechanical systems[END_REF] algorithm to account for a second-order cone representing friction, following the implementation proposed in [START_REF] Lidec | Contact models in robotics: a comparative analysis[END_REF]. In total, 120k collision problems are generated. For

3) The third strategy is the WS prev strategy, where x 0 = 12

x t-1 is initialized using the solution found by GJK or 13 EPA in the previous simulation time step.

4) The fourth and last strategy is the WS T(prev) strategy. [START_REF] Fiser | Github: libccd[END_REF] The difference with the WS prev strategy is that we use 16 the relative transformation of the shapes between time steps t and t -1 to anticipate how x t-1 might move between these two time steps.

The last two warm-starting strategies might not always be actionable. Indeed, if at time step t -1 the broad phase finds no collision between the two considered shapes, the GJK and EPA algorithms are not called, and therefore , x t-1 does not exist. Consequently, if GJK needs to be called at time step t, it cannot use x t-1 . In such a case, these two strategies fall back to the second strategy, which exploits the objects' OBBs.

We run vanilla, Polyak-accelerated and Nesterovaccelerated GJK on the dataset of trajectories described previously; the results of this benchmark are summed up in Fig. 13. In this figure, we report the computation time of the boolean collision check for GJK and our proposed accelerations. Importantly, this figure only considers the collision problems which were not filtered by the broad phase, as GJK or its accelerations would not be called otherwise. In doing so, we aim to provide the clearest possible picture of the computation time dedicated to GJK in a physics computation. Due to the filtering of the broad phase, the typical distance separating the shapes is less than a few centimeters; this corresponds to the overlapping and closeproximity cases described in the previous benchmarks. First, the results show that for the three studied methods, the No WS and WS T(prev) warm-start strategies provided a worse initial guess than the two other warm-start strategies. It appears that the WS T(prev) strategy is often the worse strategy; this observation means that the separation vector computed by GJK and/or EPA moves in a non-trivial manner between time steps t -1 and t of the simulation. For vanilla GJK, the best warm-starting strategy is the WS prev strategy, which re-uses the separation vector computed by GJK and EPA at time step t -1 of the simulation. For Polyak-accelerated 1 GJK, both the OBB WS and WS prev strategies perform better and WS prev strategies, the Nesterov acceleration allows 10 the median of computation times to reach close to 0.5µs, 11 compared to a median above 1µs in the case of GJK's best 12 warm-starting strategy. Like the Polyak acceleration, the 13 Nesterov-accelerated GJK algorithm significantly reduces the 14 spread of the distribution of computation times compared 15 to GJK. This is especially visible when using the OBB WS 16 strategy together with the Nesterov acceleration. Finally, this 17 benchmark shows that physics simulation strongly benefits 18 from using Nesterov-accelerated GJK warm-started using 19 the OBB WS strategy. [START_REF] Le Lidec | Differentiable 12 rendering with perturbed optimizers[END_REF] and constrained optimal control involving 18 contact interactions [START_REF] Mastalli | Crocoddyl: An efficient and versatile framework for multi-contact optimal control[END_REF], [66], [START_REF] Jallet | Constrained 19 differential dynamic programming: A primal-dual[END_REF].

19

Finally, one can expect this work to be largely adopted in

  Fig. 1.Two distinct collision problems using shapes from the YCB dataset: in (a) the shapes A 1 (in green) and A 2 (in red) are not in collision (dist(A 1 , A 2 ) > 0) whereas in (b) the shapes are in collision (dist(A 1 , A 2 ) = 0). In the left column, the oriented bounding boxes (OBB) of the objects are represented in light colors. In the right column, the light colors represent the convex hull of each object. In both collision problems, (a) and (b), the broad phase finds a collision between the object's OBBs; the narrow phase must thus be called to confirm or infirm the collision. The right column corresponds to the narrow phase in which the GJK algorithm is called on the objects' convex hulls. In this paper, we propose the Polyak-accelerated GJK and Nesterov-accelerated GJK algorithms in order to accelerate collision detection.

2 3 particular6

 3 collision check covered later in this section. Results for the Boolean collision checking case are analyzed in the 4 experimental section IV.5Collision detection from the perspective of quadratic pro-7 gramming. From the perspective of numerical optimization,

8

 8

45 2 ) 3 )Fig. 2 .

 232 Fig. 2. (a) Distant vs. (b) overlapping pairs of shapes and their respective Minkowski difference. Left column: two convex shapes in 2D. Right column: the Minkowski difference D of A 1 and A 2 . Since A 1 and A 2 are convex, D is also convex. In (a), the shapes are not in collision hence the origin of the configuration space C, 0 C (in red) lies outside the Minkowski difference, 0 C ̸ ∈ D. The vector x * = x * 1 -x * 2 separates A 1 from A 2 . It is also equal to the projection of 0 C onto the Minkowski difference D, x * = proj D (0 C ). In (b), the shapes overlap, thus 0 C ∈ D. In this case, we have x * = proj D (0 C ) = 0 C .

3 D 6 following discussed algorithms all require an initial starting 7 point x 0

 3670 is a compact convex set. For our distance computation 4 problem (4), we use f (x) = ∥x∥ 2 and the Minkowski 5 difference D as convex constraint set. As a side note, the ∈ D. Shapes used in physics engines are usually 8 attached to a frame to keep track of their position and 9

20

 20

Fig. 4 .

 4 Fig. 4.Computing a support vector on the Minkowski difference using support vectors (represented by star shapes in the drawing) on the individual shapes. The vector s A 1 is a support vector of shape A 1 in direction d. The vector s A 2 is a support vector of shape A 2 in direction -d. The constructed vector s = s A 1 -s A 2 is a support vector of the Minkowski difference D in the direction d.

  ) is met. As shown in Alg. 2, we add this separating plane condition before line 2 in Alg. 1. The condition on the lower bound of dist(A 1 , A 2 ) is met relative to a threshold ϵ col . This threshold sets at what distance the shapes are considered to be in collision and depends on the application. In practice, since we use double-precision floats in our benchmarks (see Sec. IV), the test on the lower bound of dist(A 1 , A 2 ) can be computed down to machine precision. This means the distance computation threshold ϵ can take values down to 10 -12 in (18).

4

  leveraging Nesterov and Polyak acceleration schemes intro-5 duced in Sec. III, which both tend to reduce the oscillations 6 hindered by gradient-descent type algorithms, we show in7

Fig. 5 .

 5 Fig. 5. Illustration of the GJK simplex strategy in 2D: (a) beginning of the k th iteration, (b) support point computation, (c) simplex augmentation, (d) simplex update.

2 2 )

 2 Otherwise, we construct W k+1 from W k+1 . To do 3 so, we retain only the minimal number of vectors 4 in W k+1 needed to express x k+1 as a convex com-5 bination (line 2 in Alg. 4). Indeed, as 0 C / ∈ W k+1 , 6 the projection x k+1 of 0 C on W k+1 necessarily 7 lies on a face of W k+1 , and can be expressed as 8 a convex combination of the vectors composing 9

Fig. 6 .

 6 Fig. 6. (a) Polyak and (b) Nesterov acceleration schemes for unconstrained gradient descent. The gradient descent algorithm aims at finding the optimum x * by following the slope given by the gradient of function f , ∇f . The vector d k-1 is the momentum accumulated over the optimization trajectory. The two schemes differ in where the gradient is computed at iteration k; the Nesterov scheme introduces an intermediary point y k = x k + δ k d k-1 to compute the gradient.

14 6 the 8 to the context of collision detection, by notably extending the 9 FW

 689 and y k ∈ R n is an intermediate quantity. Computing the 2 term y k leads to an anticipatory behavior in similar spirit to 3 extra-gradient methods[START_REF] Aspremont | Acceleration methods[END_REF].Accelerating the Frank-Wolfe algorithm with Polyak and 5 Nesterov. Recent works of[START_REF] Li | Heavy ball momentum for conditional gradient[END_REF],[START_REF] Li | A momentumguided frank-wolfe algorithm[END_REF] have proposed to adapt Polyak and Nesterov accelerations to the FW setting. We 7 propose to leverage and adapt this FW acceleration scheme formulation of collision detection previously developed in 10 Sec. II.11

2

  of x k and s k-1 , both vectors of D. The direction of support is 3 then obtained by taking a convex combination of the previous 4 support direction d k-1 and the gradient ∇f (y k ). Both the 5 Polyak and Nesterov accelerations of Frank-Wolfe are summed 6 up in Alg. 5.

7

 7

8 accelerations strategies lead to a better convergence rate of the 9 FW 12 B

 8912 algorithm when compared to the original FW algorithm. 10 In the following, we explain how to adapt the Polyak and 11 Nesterov accelerations of FW to collision detection. . Acceleration of collision detection and distance computa-13 tion 14

  strictly convex, any vector s belonging to the surface of D has a unique corresponding direction d such that s = S D (d). Here, we stress the fact that the support function S D returns only one vector. Consequently, we have d k ̸ = d k-1 and therefore s k ̸ = s k-1 . The fixed point condition (31) is thus not met unless δ k = 1 and Nesterov acceleration continues to be applied in Alg. 6. In practice, the algorithm runs until δ k gets close to 1 or x k gets close to 0 C . The condition (31) is then satisfied as the algorithm starts to cycle. The Nesterov acceleration is thus removed and the algorithm runs until the convergence criteria is satisfied, guaranteed by the Frank-Wolfe algorithm. • Otherwise, if D is non-strictly convex, multiple support directions {d 1 , ..., d m , ...} can yield the same support vector s ∈ S D (d 1 ) = ... = S D (d m ) = ... etc. Consequently, it is possible to have d k-1 ̸ = d k and s k = s k-1 . Therefore, even though δ k is not close to 1, the fixed point condition (31) can be verified. The Nesterov acceleration is stopped, possibly prematurely. The latter case is especially problematic when shapes A 1 and A 2 are in close-proximity, which is ultimately the type of collision problems commonly encountered in simulation or motion planning with contacts. In (28b), this is due to the norm of ∇f (y k ) being predominant over the norm of d k-1 as k increases, ∥d k-1 ∥ ≪ ∥∇f (y k )∥. As a consequence, the Nesterov acceleration enters a cycle: the support direction d k does not change enough compared to d k-1 , hence the support point s k is identical to s k-1 and therefore the intermediary point y k does not change and the cycle repeats. As a consequence, the criterion (31) is met and the Nesterov acceleration is stopped to escape the cycle, possibly prematurely. To prevent this phenomenon observed on non-strictly convex D, we propose to replace (28b) by a simple heuristic which normalizes the gradient and momentum directions as follows:

2 and 3 (4

 23 Nesterov-accelerated GJK (Alg. 6) against the vanilla GJK Alg. 4) algorithm.In sections IV-A and IV-B, we benchmark our proposed 5 Polyak-accelerated and Nesterov-accelerated GJK algorithms 6 against the vanilla GJK algorithm on these two distinct bench-7 marks. The benchmark made of strictly-convex shapes repre-8 sents a worst-case scenario regarding the number of iterations9

Fig. 7 .

 7 Fig. 7. Comparison of Polyak-accelerated GJK, Nesterov-accelerated GJK, and vanilla GJK on the ellipsoid benchmark for (a) distance computation and (b) Boolean collision checking. The graphs show the number of iterations (y-axis) vs. the signed distance between the two shapes (x-axis).The curve shows the mean value over 100,000 random trials. The shaded region corresponds to the standard deviation. The Nesterov-accelerated GJK algorithm requires fewer iterations when the shapes are in close proximity. The Polyak-accelerated GJK algorithm is more robust when shapes are strongly overlapping or distant.

Fig. 8 .

 8 Fig. 8. Impact of support direction normalization in Polyak and Nesterovaccelerated GJK on the YCB benchmark. The graph shows the computation time T µ (lower is better) for vanilla GJK, Polyak-accelerated GJK, and Nesterov-accelerated GJK with and without support direction normalization.Here, the two shapes are in close-proximity:0 m < dist(A 1 , A 2 ) ≤ 0.1 m.Normalizing the support direction benefits Nesterov-accelerated GJK, reducing the overall number of iterations compared to GJK and non-normalized Nesterov-accelerated GJK.

Fig. 10 ,Fig. 9 .Fig. 10 .D 2 . 7 ±Fig. 11 .

 109102711 Fig. 10, we report the number of iterations N k and execution time T µ for Polyak-accelerated GJK, Nesterov-accelerated GJK and vanilla GJK. In Fig. 11, we report relative accelerations T µ GJK /T µ polyak and T µ GJK /T µ Nesterov of Polyak-accelerated compared to GJK. These relative accelerations are computed on a given collision problem, and Fig. 11 reports their statistical distributions. These relative measures allow analyzing the effects of the studied algorithms on the same collision problems, which are not captured when using absolute values. Overall, Polyak and Nesterov-accelerated GJK significantly reduce the execution time when compared to GJK in cases where shapes are shallowly intersecting or in close-proximity.It is worth recalling, at this stage, that when two shapes are relatively far from each other, any broadphase algorithm will

Fig. 14 .

 14 Fig. 14.Two different trajectories (a) and (b) with two different pairs of objects from the YCB dataset. The objects are dropped with a random initial velocity for each trajectory in a funnel (the grey walls). At each time step, if the broadphase cannot discriminate if the shapes are in collision or not, we use the vanilla GJK algorithm or our proposed Polyak and Nesterov-accelerated variants of GJK to determine if a collision occurs between the convex-hulls of the collision pair.

2

  than vanilla GJK's best warm-starting strategy. However, 3 contrary to GJK, the OBB WS strategy is arguably better than 4 the WS prev strategy as it greatly reduces the variance of the 5 computation timings distribution. For Nesterov-accelerated 6 GJK, the results are even more significant: both the OBB 7 WS and WS prev strategy significantly outperform GJK with 8 its best warm-starting strategy. When using the OBB WS

9

 9

20 V. CONCLUSION 21 In 9 The

 20219 this work, we have first established that the well-known 22 GJK algorithm can be understood as a variant of the Frank-[START_REF] Lin | A fast algorithm for incremental distance calculation[END_REF] Wolfe method, well studied within the convex optimization 24 community, and more precisely, GJK can be identified as a 25 sub-case of fully-corrective Frank-Wolfe. Subsequently, this 26 connection has enabled us to accelerate the GJK algorithm 27 in the sense of Nesterov acceleration by adapting recent 28 contributions on applying Polyak and Nesterov acceleration 29 to the context of Frank-Wolfe. Through extensive bench-30 marks, we have shown that this acceleration is beneficial for 31 both collision detection and distance computation settings for 32scenarios where shapes intersect or are close, accelerating 1 collision detection by up to a factor of two. Interestingly, 2 these two scenarios notably encompass the generic contexts 3 of planning and control as well as physical simulation, which 4 are essential areas of modern robotics. Therefore, although the 5 proposed accelerations correspond to improvements of GJK's 6 execution time on the order of a few microseconds, modern 7 robotics applications may solve millions to billions of collision 8 problems when, for instance, learning a policy with RL [59]. Polyak and Nesterov accelerations for GJK are al-10 ready included in the HPP-FCL library [51], notably used by 11 the HPP framework [4] for motion planning, the Pinocchio 12 framework [60] dedicated to simulation and modeling, the 13 Croccodyl [61] and the OSC-2 [62] software dedicated to tra-14 jectory optimization, to name a few. In future work, we plan to simulation [64],

  20 the current available GJK implementations, as it only requires 21 minor algorithmic changes. This work should benefit a large 22 audience within robotics (e.g., simulation, planning, control) 23 and beyond by addressing issues shared by other communities, 24 including computer graphics and computational geometry.

25 ACKNOWLEDGMENTS 26

 2526 We warmly thank Francis Bach, Adrien Escande, Joseph 27 Mirabel, and Mehdi Bennalegue for fruitful discussions on the 28 various topics covered by this article. We also warmly thank 29 the cohort of developers who contribute to developing open-30 source, useful, reproducible, and extensible software, which 31 primarily benefits this project and, more widely, the robotics 32 ecosystem. 33 This work was partly supported by the European Re-34 gional Development Fund under the project IMPACT (reg.

  35 no. CZ.02.1.01/0.0/0.0/15 003/0000468), by the French gov-36 ernment under the management of Agence Nationale de la 37 Recherche as part of the "Investissements d'avenir" program, 38 reference ANR-19-P3IA-0001 (PRAIRIE 3IA Institute), by 39 the AGIMUS project, funded by the European Union under 40 GA no.101070165 -views and opinions expressed are those 41 of the author(s) only and do not necessarily reflect those of 42 the European Union or the European Commission, neither the 43 European Union nor the European Commission can be held 44 responsible for them -and by the Louis Vuitton ENS Chair 45 on Artificial Intelligence.

  Montaut is with Inria, Département d'Informatique de l' École Normale Supérieure, PSL Research University in Paris, France and also with the Czech Institute of Informatics, Robotics and Cybernetics in Prague, Czech Republic. Vladimir Petrik and Josef Sivic are with the Czech Institute of Informatics, Robotics and Cybernetics, Czech Technical University in Prague. Quentin Le Lidec and Justin Carpentier are with Inria and Département d'Informatique de l' École Normale Supérieure, PSL Research University in Paris, France.

  The active-set W k is then augmented by computing a 27 support s k (line 2 in Alg. 3) to obtain W k+1 = {x k , s k } 28 (line 4 in Alg. 3). We then minimize function f on the convex-hull 30 of W k+1 , conv( W k+1 ), which is simply the seg-31 ment [x k , s k ]. For the distance computation problem (4), 32 this linesearch operation is equivalent to projecting 0 C

26 • 29 • 33 onto the segment [x k , s k ] (line 5 in Alg. 3).

  return 0 11: W k+1 = {s 1 , ..., s r } where s 1 , ..., s r are the smallest number of vectors in W k+1 such that x k+1 is a convex combination of s 1 , ..., s r .

Stopping criterion. As the number of iteration k increases, δ k → k→∞ 1 in

[START_REF] Pan | Fcl: A general purpose library for collision and proximity queries[END_REF]

. Therefore, d k tends to be equal to d k-1 (28b) and thus s k = s k-1 (28c). As a consequence, augmenting W k with s k to construct W k+1 (see (28d)) and then projecting 0 C onto W k+1 (28e) will not result in any progress. Therefore, x k+1 = x k : the algorithm reaches a fixed point and is stuck on constant support direction d.

  . Unlike 28 FCL, HPP-FCL provides its own implementation of GJK, 29 which we have extended by implementing the Polyak and 30 Nesterov-accelerated GJK algorithms (Alg.[START_REF] Schulman | Motion planning with sequential convex optimization and convex collision checking[END_REF]). The open-31 source code of the HPP-FCL library is publicly available 32 at https://github.com/humanoid-path-planner/hpp-fcl under the 33 BSD-3 license. The benchmark code is publicly available 34 at https://github.com/lmontaut/colbench under the GNU AGP 35 License.

	36

TABLE II COMPUTATION

 II TIME IN MICRO-SECONDS OF GJK-LIKE SOLVERS VS. SOTA QUADRATIC PROGRAMMING PROXQP SOLVER.

			Nv = 8	Nv = 250	Nv = 940
			N f = 6	N f = 496	N f = 1876
		ProxQP	5.3 ± 2.7 µs	(2 ± 0.6) • 10 3 µs (20 ± 14) • 10 3 µs
		GJK	0.2 ± 0.03 µs	0.8 ± 0.3 µs	2.1 ± 0.5 µs
		Nesterov	0.2 ± 0.05 µs	0.7 ± 0.2 µs	1.4 ± 0.3 µs
		Polyak	0.2 ± 0.05 µs	0.6 ± 0.2 µs	1.4 ± 0.4 µs
	22
	23	vanilla, Nesterov-accelerated and Polyak-accelerated GJK
	24	against the following baselines from other collision detection
	25	librairies: CCD's MPR and GJK implementations [15], FCL's
		GJK implementation [16] and Bullet's GJK implementation.
	27
	28	lators like Drake [18], MuJoCo [3], ODE [19] and Bullet [1].
	29	Since GJK is the state-of-the-art algorithm for narrow phase
	30	collision detection, it is no surprise that most collision de-
	31	tection libraries implement only this algorithm. In addition
	32	to being a general, robust and computationally fast method,
		GJK-like algorithms also have the advantage of producing a

C. GJK-like algorithms vs. generic quadratic programming

1 solvers 2

As explained in Sec. II, in the case of two convex meshes, 3 the collision problem can be formulated as a Quadratic Pro-4 gram (2) (QP), which can be solved using any generic QP 5 solver

[START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF]

,

[START_REF] Stellato | Osqp: An operator splitting solver for quadratic programs[END_REF]

-

[START_REF] Tracy | Differentiable collision detection for a set of convex primitives[END_REF]

. In Table

II

, we compare the perfor-6 mance of GJK and our proposed accelerations against the 7 state-of-the-art ProxQP solver

[START_REF] Bambade | Prox-qp: Yet another quadratic programming solver for robotics and beyond[END_REF]

. We report the computation 8 timings in micro-seconds for pairs of identical shapes with 9 an increasing number of vertices (N v ) and faces (N f ). The 10 results are staggering: for very simple convex meshes like 11 a cube, GJK, and its accelerated variants are already more 12 than 10 times faster than the QP solver. When the complexity 13 of the meshes increases, GJK and its variants are thousands to 14 tens of thousands of times faster than the QP solver, making 15 generic QP solvers prohibitive for collision detection in real-16 time applications like robotics or computer graphics. Although 17 these results are not surprising, they clearly showcase why 18 dedicated solvers such as GJK-like methods are crucial for 19 collision detection. 20 D. Comparison against other collision detection librairies 21 In this sub-section, we compare our implementations of 26 These collision detection algorithms are used in physics simu-33 simplex surrounding the origin when shapes are in collision 34 (see Sec. II). This simplex is then fed to the Expanding 35 Polytope Algorithm (EPA) in order to estimate the penetration 36 depth and separation vector [22]; these contact informations 37 are used in physics simulation to resolve contact constraints. 38 Just like GJK, the core of EPA consist in computing support 39 points in order to expand a polytope inside the Minkowski 40 difference. Therefore, since GJK and EPA are made to work in succession with one another, it is almost always the case that physics simulators use GJK to first detect collisions and then EPA to compute contact features if a collision is detected.

and the computed support vector s k .

The efficient projection onto simplexes in R 3 , named the distance subalgorithm by[START_REF] Gilbert | A fast procedure for computing the distance between complex objects in three-dimensional space[END_REF], is thoroughly covered in[START_REF] Ericson | Real-time collision detection[END_REF],[START_REF] Bergen | A fast and robust gjk implementation for collision detection of convex objects[END_REF] and its robustness is improved in[START_REF] Montanari | Improving the gjk algorithm for faster and more reliable distance queries between convex objects[END_REF].

Computation time in micro-seconds of implementations of different collision detection solvers from various C/C++ librairies on the YCB benchmark for boolean collision checking. The y-axis is a log-scale. We compare our implementation of vanilla GJK, Nesterov-accelerated GJK and Polyak-accelerated GJK against FCL's GJK implementation, CCD's GJK implementation and CCD's MPR implementation. The results are split into three different categories: (a) when shapes are distant (dist

each collision problem, we extract the YCB shapes and their poses.

This dataset allows us to evaluate the vanilla, Polyakaccelerated, and Nesterov-accelerated GJK algorithms on the same collision problems generated by a physics simulation.

Interestingly, this dataset allows us to study only the collision problems not filtered by the broadphase of the physics simulator, as explained in Sec. I. During the broad phase, the oriented bounding boxes of the objects (OBBs, as shown in Fig. 1) are used to assess if objects are not in collision. Therefore, if the broad phase does not filter a collision, the GJK algorithm and our proposed accelerations are called and solve the boolean collision check problem. Finally, this dataset allows us to test different strategies to warm-start (WS) the GJK algorithm and our proposed accelerations. We denote by x t 0 the initial guess given to vanilla, Polyak-accelerated and Nesterov-accelerated GJK at time step t of the simulation. We also denote by x t-1 the separation vector found by GJK (accelerated or not) or EPA at time-step t -1 of the simulation. We consider four Trajectory -Boolean collision check (c) Fig. 13. Boolean collision checking of YCB objects' trajectories (see Fig. 14) for different warm-start strategies for (a) vanilla GJK, (b) Polyak-accelerated GJK, and (c) Nesterov-accelerated GJK (with normalization). In the three figures, WS is an abbreviation of warm-start. The No WS strategy signifies the algorithm is initialized with x 0 = (1, 0, 0) T . The OBB WS strategy uses the objects' current OBBs centers to compute x 0 . In both WS prev and WS T(prev), x 0 is computed using GJK or EPA's previous solution, when this solution is available (i.e., when the previous collision problem was not discarded by the broadphase). Contrary to WS prev, WS T(prev) corrects the previous solution using the relative displacement of the shapes between the two considered time steps. different warm-start strategies for the vanilla GJK algorithm 1 and our proposed accelerations: 2) The second strategy is the OBB WS strategy, where x t 0 =


Leobacher Michael Fischer
email: michael.fischer@uni-graz.at

G Leobacher

C Krause

INTEREST DEVELOPMENT WHEN MODELLING DISEASES (HANDOUT)

Simulation science has become more present in

hence provide new opportunities to draw students' interests, making us wonder how can modern mathematics and current science topics in mathematics classes inuences learners' interest in the subject mathematics?. To answer this question, we conducted a school experiment in the summer of 2023.

We follow the four-phase model of interest development [START_REF] Hidi | The Four-Phase Model of Interest Development[END_REF] to characterize the phases of interest, focusing on triggered and maintained situational interest -the catch and hold phases, complemented by noninterest. By analysing responses to a survey, we empirically interrogate whether the topicality of the subject as well as the novelty of the mathematical method are suitable for triggering situational interest.

Regarding interest structure, the mathematical project itself appeals to a variety of types according to the RIASEC+N model due to its variety and novelty [START_REF] Dierks | Interest in science: A RIASEC-based analysis of students' interests[END_REF]. A special focus is on networking, realized due to a permanent exchange during modelling, implementation and analysis. Furthermore, we measure whether the active involvement of learners in modelling and analysing the results as well as the social relevance of the learning content are suitable as inuencing factors in order to maintain situational interest.

From a mathematical perspective, the mentioned simulations are a system of ODEs in time and space with collision, similar to the famous Boltzmann equation [START_REF] Boltzmann | Weitere studien über das wärmegleichgewicht unter gasmolekülen[END_REF]. An alternative, which is also widely used, are cellular automata. They are discrete in time and space and are, in its simplest form, based on a combination of recursive sequences and random variables, both parts of the curricula in the Austrian tenth grade but not a central focus in the centralized Matura. Cellular automata can be easily implemented in e.g. Excel and are therefore suitable to be actively used in many types of schools without a computer science background.

Initial results already show that the topic is suitable for introducing certain mathematical concepts. Programming a discrete model is also suitable for cross-referencing to computer science lessons. We specically avoid ready-made simulation software which has already been used successfully [START_REF] Bauer | Modellieren von Epidemien: Exponentiell und darüber hinaus[END_REF][START_REF] Maxara | Students' probabilistic simulation and modeling competence after a computer-intensive elementary course in statistics and probability[END_REF].

However, implementing it themselves poses challenges for many children due to curricular circumstances. The topic of COVID-19 was suitable as a context to raise interest, emotional and value-related components were increasingly addressed (Krapp, 2002a[START_REF] Krapp | Structural and dynamic aspects of interest development: Theoretical considerations from an ontogenetic perspective[END_REF].

Thus, the teaching unit can also contribute to the critical evaluation of scientic ndings by students and teachers [START_REF] Kollosche | COVID-19, mathematics education, and the evaluation of expert knowledge[END_REF]). • 63 pupils participated in both units

An online-article about COVID-19 as a first catch

Modeling and simulation of diseases

• tracking of students via an identification number

Collected data

Variety of data collection:

• students' feedback and interactions with the authors

• worksheets are evaluated in terms of understanding and modelling approaches (ongoing)

• access to slides and collection of links and materials is monitored

• the questionnaire allows a quantitative evaluation regarding interest in mathematics of the participating groups

• through the self-selected identification number, one can carry out an individual analysis in terms of interest development (ongoing).

Open feedback:

• topic was perceived as new and interesting, working in teams (networking in RIASEC+N) was positively mentioned:

Access to slides and accompanying material

• 25 users have visited the material-homepage (50 visits total), 13 users slides of unit 1 (18 total), 16 users slides of unit 2 (24 total)

• animated links (bottom left, top left) are more often clicked then related non-moving links (low total clicks)

• Matura-material and topics related to the classroom more often than comparable links (low total clicks)

First analysis of the questionnaire:

• high self-esteem in working and modelling with sequences and random variables (significant)

• increase in catch and decrease in disinterest

• implementing simulations was challenging for many pupils

• overview:

  Typical SIR-plots (left) based on microscopic simulations (right) in an article by the Washington Post. All simulations are calculated live.

  How can modern mathematics and current science topics in mathematics classes influence learners' interest in the subject mathematics? First unit • motivation, collecting ideas, linking with known topics • mathematically focus on concept of random walk Second unit • hypothesizing and testing using Excel files • random walk, drift, social distancing, aerosols Teaching and accompanying material Audience • conducted in five classes at different high schools in three states in Austria, summer 2023 • 1st unit, 83 participants (36 m, 43 f, 1d, 3N/S) in 10th and 11th grades (40 as well as 43). 2nd unit, same classes, 80 participants (37 m, 39 f, 0d, 4N/S),