Supplementary material for the paperFree Space of Rigid Objects:Caging, Path Non-Existence, and Narrow
Passage Detection
Anastasiia Varava? , J. Frederico Carvalho? , Florian T. Pokorny, and Danica
KragicKTH Royal Institute of Technology{varava,jfpbdc,fpokorny,dani}@kth.se

In this document we provide the proofs of Propositions 1, 2, and 3, anddiscuss the computational complexity in detail.

1

Computational Complexity

Let us now discuss the total computational complexity of the algorithm. Let s
be the number of slices, n and m be the number of balls in the object’s andthe obstacle’s spherical representation, respectively. We have pre-computed thegrids over SO(3) corresponding to different dispersion values, and therefore weare only interested in the complexity of the connectivity graph construction. For
each slice, we execute two computationally expensive procedures: we computea weighted Voronoi diagram of the collision space, which allows us to extract
the balls representation of the free space, and then for each slice we compute its
intersections with adjacent slices. In practice, each orientation in Q has around20 adjacent orientation values, so each slice has around 20 neighbours1 .
In CGAL representation, the regular triangulation contains the corresponding weighted Voronoi diagram. Note that a weighted Voronoi diagram can beconstructed by other means using for example the algorithm from [1]. The complexity of this step is O(n2 m2 ). The computation of the connected componentsof each slice is linear on the number of balls in the dual diagram, which makes
the overall complexity of this step O(n2 m2 ).
The complexity of finding the intersections between two connected components belonging to different slices O(b log3 (b)+k) in the worst-case [3], where b isthe number of balls in both connected components, and k the output complexity,i.e., the number of pairwise intersections of the balls.
The complexity of the final stage of the algorithm — computing connectedcomponents of the connectivity graph — is linear on the number of vertices,and can be expressed as O(s c), where c is the average number of connectedcomponents per slice (a small number in practice).
?
1

The first two authors contributed equally.
This is the case for SO(3), in the case of SO(2) there are exactly 2 neighbours.

22A. Varava, J. F. Carvalho et al.

Proposition 1: distance-displacement correspondence

Proposition 1. Given two unit quaternions p, q, the following equation holds:
D(pq̄) = 2 sin(ρ(p, q)).

(1)

Proof. We proceed by reducing both sides of the equation to the same formula,starting with the left-hand-side. To do this we once again point out that wecan identify quaternions with vectors in R4 , and that a unit quaternion q =θθcos( q ) + sin( 2q )(qx i + qy j + qz k) is associated to a 3D rotation of an angle ofθq around the axis (qx , qy , qz ) which we will denote wq .
D(Rpq̄ ) = 2|=pq̄|
θq
θp
θp
θq
θp
θq
= 2k cos( ) sin( )wp − cos( ) sin( )wq − sin( ) sin( )wp × wq k222222r
θp
θp
θq
θθq
θq
p
= 2 k cos( ) sin( )wp − cos( ) sin( )wq k2 + k sin( ) sin( )wp × wq k2222222

Where the last equality is due to the fact that wp × wq is perpendicular to both
wp and wq , and is therefore a consequence of the Pythagorean theorem. Now
recall that kwp × wq k = sin(ωp,q ) where ωp,q is the angle between wp , wq andthat hwp , wq i = cos(ωp,q ), since kwp k = kwq k = 1. Recall also that sin2 (θ) =1 − cos2 (θ), whence we obtainD(Rpq̄ ) =r
θq
θp
θp
θq
θp
θq2 k cos( ) sin( )wp − cos( ) sin( )wq k2 + sin2 ( ) sin2 ( )(1 − hwp , wq i2 )
222222w −hw ,w iw

Furthermore let w̃p = kwqq −hwpp ,wqq iwpp k be the component of wq which is perpenθθdicular to wp , then we can rewrite cos( 2p ) sin( 2q )wq ascos(θqθpθqθp) sin( )wq = cos( ) sin( )(hwp , wq iwp + hw̃p , wq iw̃p )2
222

Substituting this into the formula, and using the pythagorean theorem toseparate he wp and w̃p components, we can proceed withrθqθpθpθqk(cos( )sin( )−cos( )sin( )hwp , wq i)wp k2
2222θqθpθq
θp+k cos( ) sin( )hwq , w̃p iw̃p k2 + sin2 ( ) sin2 ( )(1 − hwp , wq i2 )2222D(Rpq̄ ) = 2

Note that kwq k, kwp k, kw̃p k = 1, and therefore khwp , wq iwp +hwq , w̃p iw̃p k2 =1 which by the Pythagorean theorem gives hwq , w̃p i2 = 1 − hwp , wq i2 , which we
can substitute once again.

Title Suppressed Due to Excessive LengthD(Rpq̄ )s= 2 (cos(+ cos2 (32θqθpθpθq) sin( ) − cos( ) sin( )hwp , wq i)2222θpθqθpθq) sin2 ( )(1 − hwp , wq i2 ) + sin2 ( ) sin2 ( )(1 − hwp , wq i2 )2222Now we want to deal only with a combination of tangents, therefore we divideθθ
the term inside the square root by cos2 ( 2p ) cos2 ( 2q ) yielding:s2θqθpθqθpD(Rpq̄ ) = 2| cos( ) cos( )| (tan( ) − tan( )hwp , wq i)
2222+ tan2 (θqθpθq)(1 − hwp , wq i2 ) + tan2 ( ) tan2 ( )(1 − hwp , wq i2 )222Now, expanding the sqares and multiplying into all the terms under thesquareroot sign, as well as eliminating terms that cancel out, results in:
r
θq
θp
θp
θq
θpD(Rpq̄ ) = 2| cos( ) cos( )| tan2 ( ) − 2 tan( ) tan( )hwp , wq i
22222θθθθθqpqpq+ tan2 ( ) + tan2 ( ) tan2 ( ) − tan2 ( ) tan2 ( )hwp , wq i222222
By introducing an extra 1 − 1 into the square root, we can use these terms
to complete products in order to simplify the equation.
rθpθqθpθq
θpθqD(Rpq̄ ) =2| cos( ) cos( )| 1 + tan2 ( ) + tan2 ( ) + tan2 ( ) tan2 ( )2222222θpθq) tan( )hwp , wq i)
2 r 2
θpθq
θp
θq=2| cos( ) cos( )| (1 + tan2 ( ))(1 + tan2 ( ))2222−(1 + tan(−(1 + tan(2θpθq) tan( )hwp , wq i)
22Finally, recall that 1 + tan2 (θ) = cos12 (θ) , which gives uss2θpθqθpθqD(Rpq̄ ) = 2 1 − cos2 ( ) cos2 ( )(1 + tan( ) tan( )hwp , wq i)
2222

4

A. Varava, J. F. Carvalho et al.

Now we begin to explore the right-handequation, by noting thatq side of thep2
when sin(θ) > 0 then sin(θ) = | sin(θ)| = sin (θ) = 1 − cos2 (θ). Furthermore
we note that cos−1 maps [−1, 1] to [0, π] and particularly it maps [0, 1] to [0, π/2]
where the sine function is positive, therefore, we get
2 sin(ρ(p, q)) = 2 sin(cos−1 (|hp, qi|))p= 2 1−cos2 (cos−1 (|hp, qi|))p= 2 1−hp, qi2
s2
θpθq
θp
θq
= 2 1−(cos( ) cos( )+sin( ) sin( )hwp , wq i)
2222

And finally, we get the same formula as before:
s2
θpθq
θp
θq
2 sin(ρ(p, q)) = 2 1 − cos2 ( ) cos2 ( )(1 + tan( ) tan( )hwp , wq i)
2222Hence concluding the proof of Proposition 1.

3Proposition 2: correctness

Proposition 2 (correctness). Consider an object O and a set of obstacles S.
Let c1 , c2 be two collision-free configurations of the object. If c1 and c2 are notpath-connected in G(aCεf ree (O)), then they are not path-connected in C f ree (O).
Proof. Recall that the approximation of the free space is constructed as follows:aCεf ree (O) =s[f reeaSlU(φi ,ε) ,i=1wheref ree
colaSlU(Oεφi )) × U (φi , ε)(φi ,ε) = Dual(C(2)c

Now, recall that by definition (Dual(C col (Oεφi ))) ⊂ C col (Oεφi ) [2], and that
we choose ε and U (φi , ε) so that for any φ ∈ U (φi , ε), C col (Oεφi ) ⊂ C col (Oφ ). Thisc
implies that (Dual(C col (Oεφi ))) ⊆ C col (Oφ ) for any φ ∈ U (φi , ε), and conversely
S f ree φf reethat C f ree (Oφ ) ⊂ Dual(C col (Oεφi )). Finally, since SlU(O ) ×φC(φi ,ε) ={φ}, we have:f reef ree
SlU(φi ,ε) ⊆ aSlU (φi ,ε) ,(3)Title Suppressed Due to Excessive Length5

We now want to show that if there is no path between two vertices v =(aC, U ) and v 0 = (aC 0 , U 0 ) in G(aCεf ree ), then there is no path between connectedcomponents of aCεf ree (O) corresponding to them. It is enough to show that iftwo vertices corresponding to adjacent slices are not connected by an edge, thenf reef reethey represent two components which are disconnected in SlU∪ SlU.
0Consider two adjacent slices SlU (φi ,ε) and SlU (φj ,ε) , and two path-connected
f reef reecomponents C1 ⊂ aSlU(φi ,ε) and C2 ⊂ aSlU (φj ,ε) . Let aC1 and aC2 be theirrespective representations as unions of balls.
Let v1 and v2 be the vertices of G(aCεf ree (O)) corresponding to these components: v1 = (aC1 , U (φi , ε)) and v2 = (aC2 , U (φj , ε)). By construction, these areadjacent slices, therefore U (φi , ε) ∩ U (φj , ε) 6= ∅ and since there is no edge between v1 and v2 , we get aC1 ∩aC2 6= ∅. But, by construction C1 ⊆ aC1 ×U (φi , ε)and C2 ⊆ aC2 × U (φj , ε), therefore, we get:C1 ∩ C2 ⊆ (aC1 × U (φi , ε)) ∩ (aC2 × U (φj , ε)) = ∅And so C1 and C2 are disjoint in the union of the corresponding slices.

4Proposition 3: δ−completenessProposition 3 (δ-completeness). Let c1 , c2 be two configurations in C f ree (O).
If they are not path-connected in C f ree (O), then for any δ > 0 there exists
ε > 0 such that the corresponding configurations are not path-connected inG(aCεf ree (O+δ )), where the graph is produced according to the procedure outlined
in Rem. 1.
In proving this proposition we make used of the notion of the signed distance
between two sets:(minp∈O d(p, S)
if O ∩ S =6 ∅ds (O, S) =− maxp∈O∩S d(p, S) otherwise
Note that ds (A, B) is not necessarily the same as ds (B, A).
Proof. Recall from (see Rem. 1) that there is an edge between vertices (aC1 , φ1 ),(aC2 , φ2 ) only if U (φ1 , ε) overlaps with U (φ2 , ε) and C1 overlaps with C2 where
Ci = aCi ∩ C f ree (Oεφi ) for i = 1, 2 i.e. the components of the actual free spaceof Oεφi (i = 1, 2) corresponding to the approximations aC1 and aC2 . This meansthat we can perform the analysis in terms of collisions in workspace, rather thanlooking at the configuration space.
Recall now that we want to prove that for a pair of configurations c1 , c2
which are not path-connected in C f ree (O), then for any δ > 0 they are not pathconnected in G(aCεf ree (O+δ )) for some ε > 0. Therefore, we start by noting thatsince c1 and c2 are not path-connected there exists a collision configuration c inany path between them, and since collision implies regular intersection, we haveds (c(O), S) < 0. Thus, for the same configuration c we have ds (c(O+δ ), S) < −δ.

6A. Varava, J. F. Carvalho et al.

To see that this will result in path non-existence, we take an arbitrary ε > 0and consider the collision space aCεcol (O+δ ). Further, we let c = (p, φ) ∈ R3 ×SO(3), and for any φi such that φ ∈ U (φi , ε), we define ci = (p, φi ). Finally, werestrict ourselves to the case where ε < δ and define δ 0 = δ − ε, then we get:ds (ci (O+δ0 ), S) ≤ dist(c(O+δ0 ), ci (O+δ0 )) + ds (c(O+δ0 ), S)≤ ε − δ0Which implies that, as long as we choose ε such that ε − δ 0 < 0 (i.e. ε < δ/2)
we obtain the required result.

References
1. Aurenhammer, F. and Edelsbrunner, H., 1984. An optimal algorithm for constructing the weighted Voronoi diagram in the plane. Pattern Recognition, 17(2), pp.251257.
2. Edelsbrunner, H.: Deformable smooth surface design. Discrete and ComputationalGeometry (1999), 21(1), 87–115.
3. Zomorodian, A., Edelsbrunner H.: Fast software for box intersections, Proceedingsof the 16th annual symposium on Computational geometry, 129–138 (2000).