Collision Detection for Paths of 3D Clothoids

Collision Detection for Paths of 3D Clothoids Marco Frego1, Nicola Mimmo2, Enrico Saccon3, Filip Dyba4and Luigi Palopoli3 Abstract— This preliminary work introduces a method to inspect intersections between two 3D clothoid splines for detecting collisions between non-holonomic autonomous robotic systems trajectories. The proposed algorithm operates by decomposing each spline into a set of small tangent tetrahedra, which are then arranged within a hierarchical tree structure to enable efficient intersection detection. An Axis-Aligned Bounding Box (AABB) tree is adopted, as it provides a balanced tradeoff between construction complexity and evaluation efficiency. Collision checks at the curve level are performed exclusively for those pairs of tetrahedra identified as intersecting. This approach significantly reduces computational effort. Thereby, we aim at employing the algorithm in real-time applications. I. INTRODUCTION The computation of intersections between two general curves constitutes a complex and multifaceted problem. Challenges typically arise from the occurrence of multiple intersections distributed irregularly, as well as from singularities that appear when the curves are tangent. For 3D clothoid curves [1], these difficulties are further exacerbated by the fact that the curve winds at two points at infinity, thereby generating a large number of potential intersections, see Fig. 1. In addition, the computational burden of intersection detection grows with the length of the splines. A conventional approach consists of approximating the curves with line segments and subsequently applying standard methods to test intersections between segment pairs. While conceptually straightforward, this strategy suffers from high computational costs since it requires fine segmentation to preserve resolution. To address these limitations, we propose a segmentation technique in which a 3D clothoid arc is divided into subintervals, with each subarc inscribed within a tangent tetrahedron. The intersection problem is thus reduced to checking overlap between tetrahedra. If a tetrahedron from the first curve intersects with one from the second, we can either declare collision or apply the Newton method to find the intersection point(s). A key advantage of this formulation lies in the fact that the curvilinear abscissas of the subarcs are explicitly known and controlled, ensuring 1Faculty of Engineering of the Free University of Bozen-Bolzano, Bolzano, Italy. Email: [email protected] 2Department of Electrical, Electronic, and Information Engineering “Guglielmo Marconi”, University of Bologna, Bologna, Italy. Email: [email protected] 3Department of Information Engineering and Computer Science, University of Trento, Trento, Italy. Email: {enrico.saccon,luigi.palopoli}@unitn.it 4Department of Cybernetics and Robotics, Wroclaw University of Science and Technology, Wroclaw, Poland. Email: [email protected] This work has been supported by the Grant “Clothoids in 3D for engineering and biomechanical applications” funded by the Italian MUR, n. 2022E954LL, CUP n. I53C24002460006. Fig. 1. A 3D clothoid with marked inflection point and asymptotes. that the solver’s output can be verified as lying within the current subintervals. In the next section, we briefly describe the construction of such tetrahedra and how to efficiently intersect two of them [2]. Later, we focus on efficient treebased data structures designed to minimize the number of tetrahedron–tetrahedron intersection tests [3]. II. CONSTRUCTION OF THE TANGENT TETRAHEDRON The idea of the tangent tetrahedron is to extend to 3D the concept of the 2D tangent triangle to a planar curve [4]. P0 P1 Pm Fig. 2. A tangent triangle to an arc of a planar clothoid. Definition 1. The tangent triangle to a convex curve arc is defined taking as vertices the initial and the final point of the arc, respectively P0= (x0, y0)and P1= (x1, y1). The last vertex Pm= (xm, ym)is defined as the intersection of the tangents at P0and P1, see Fig. 2. We propose the following 3D extension. Definition 2. The tangent tetrahedron to a convex curve arc is defined taking as two vertices the initial and final points of the arc (P0and P1), the other two vertices are obtained projecting P0and P1onto the line given by the intersection of the osculating planes (identified by the tangent and normal vectors) at P0and P1, see Fig. 3. 2025 I-RIM Conference October 17-19, Rome, Italy ISBN: 9788894580570 10.5281/zenodo.17629800 169 0 0 0.2 0.4 0.6 0.8 0.5 0.6 0.4 0.2 10 Fig. 3. Tangent tetrahedron to an arc of 3D clothoid. The construction of this tetrahedron contains Definition 1 as a special case, when the curve is planar. Indeed, for the third vertex, the two osculating planes reduce to the tangent vectors, resulting in the third and fourth vertices to overlap, yielding the tangent triangle. III. INTERSECTION OF TETRAHEDRA The intersection between two tetrahedra is a convex polyhedron [2]. The intersection process involves a systematic checking for vertices of one tetrahedron inside the other, and identifying intersections between their edges and faces. Given two tetrahedra Xand Y, it is possible to map them to standard tetrahedra by means of an affine invertible map. Then, there are four types of corners to the polyhedron resulting from the intersection: vertices of Xthat lie inside Y; intersections between the edges of Xand the faces of Y; intersections between the edges of Yand the faces of X and vertices of Ythat lie inside X. IV. AXIS-ALIGNED BOUNDING BOX TREES We partition the curve into a collection of smaller convex sub-arcs, each of which is approximated by an enclosing tetrahedron. This decomposition enables the implementation of an initial filtering stage, wherein pairs of tetrahedra are tested for potential overlap. Only in cases where two tetrahedra intersect is it necessary to consider the corresponding clothoid arcs as possible candidates for intersection, see Fig. 4. For these cases, the more accurate Newton-based algorithm is employed to compute the precise solution. However, when applied to clothoid splines of increasing complexity, this straightforward strategy — based on exhaustively comparing all pairs of tetrahedra —leads to a significant growth in computational cost. The approach adopted in this work relies on the construction of Bounding Volume Hierarchies (BVHs) to efficiently represent the space occupied by a set of tetrahedra. In a BVH, each node is associated with a Bounding Volume Primitive (BVP) that encloses the geometric primitives contained in 0 0 0.2 0.4 0.6 0.8 1 0.5 1 10.5 1.5 0 Fig. 4. Example of intersection of two 3D clothoids with bounding boxes. its subtree, providing a coarse but efficient spatial approximation. Several types of bounding primitives are commonly employed, including spheres, axis-aligned bounding boxes (AABBs), oriented bounding boxes (OBBs), and convex hulls. Within this context, the adopted BVP is the axisaligned bounding box (AABB), which allows for a simple and extremely efficient construction and test for overlap. The construction of an AABB tree has an average computational complexity of Θ(nlog n), with the worst-case bound of O(n2)[3]. Similarly, the cost of determining intersections between two AABB trees is Θ(mlog n)on average and O(nm)in the worst case, where nand mdenote the number of primitives in the respective trees. In contrast, the naïve approach of testing intersections directly between two sets of tetrahedra entails a computational complexity of O(nm) in all cases, as each tetrahedron in the first set must be compared against every tetrahedron in the second. As an example of performance, checking the collision of two paths with a small number of arcs that results in comparing two sets of about 25 tetrahedra each, required about 0.5ms using AABBs and 6ms without. For longer splines, resulting in a set of about 50 and one of 300 tetrahedra, the times were 1ms with AABBs and 70ms without, respectively. V. CONCLUSIONS We presented some preliminary findings on the problem of intersecting splines of 3D clothoids. Since they emulate exactly helices, planar clothoids, circles and lines, our framework is capable of exactly handling also other curves, e.g., 3D Dubins and Reeds-Sheep, biarcs and others. REFERENCES [1] M. Frego, “Closed form parametrisation of 3d clothoids by arclength with both linear varying curvature and torsion,” Applied Mathematics and Computation, vol. 421, p. 126907, 2022. [2] C. McCoid and M. J. Gander, “Intersection of tetrahedra,” ACM Trans. Math. Softw., vol. 51, no. 2, Jun. 2025. [3] Y.-S. Xing, X. P. Liu, and S.-P. Xu, “Efficient collision detection based on AABB trees and sort algorithm,” in IEEE 2010 International Conference on Control and Automation, 2010, pp. 328–332. [4] E. Bertolazzi, P. Bevilacqua, and M. Frego, “Efficient intersection between splines of clothoids,” Mathematics and Computers in Simulation, vol. 176, pp. 57–72, 2020, Applied Scientific Computing XV: Innovative Modelling and Simulation in Sciences. 170