Shortest rotation of vector around cone

Question

I am searching for the shortest rotation that transforms a 3D vector from intial to final direction, while circumventing a given cone.

More precisely:

Given:

• Cone B(lue): defined by its symmetry axis $\vec{v}_B$ and its half cone angle $\alpha_B$.
• Two direction vectors: initial direction $\vec{v}_1$ and final direction $\vec{v}_2$ (gray). The origins of the vectors and cone B are the same.

Wanted:

• Cone G(reen): defined by axis $\vec{v}_G$ and half cone angle $\alpha_G$. This cone represents the shortest single axis rotation between $\vec{v}_1$ and $\vec{v}_2$ around Cone B(lue).

Current Approach:

• the axis $\vec{v}_G$ must lie in a plane $P$ with normal $\vec{n}=\vec{v}_2-\vec{v}_1$ that is going through the origin. This leaves a single degree of freedom for $\vec{v}_G$
• the angle $\alpha_G = \sphericalangle(\vec{v}_G,\vec{v}_1) = \sphericalangle(\vec{v}_G,\vec{v}_2)$ must be equal to $\sphericalangle(\vec{v}_G,\vec{v}_B)$ + $\alpha_B$

With these findings it should be possible to determine $\vec{v}_G$ for the shortest path. But how? So far I failed in finding an explicite solution. Has somebody a good idea?

Answer 1

N.B.: This answer only considers the case $v_1\cdot v_B, v_2\cdot v_B\gt0$. There is a complete solution here, but I’m leaving this preliminary answer up because it’s already been accepted.

WLOG, place the apex of the blue cone at the origin and rotate so that its axis is the $z$-axis. The equation of the cone is then $x^2+y^2=z^2\tan^2\alpha_B$, where $\alpha_B$ is the half aperture angle. It will also be convenient to express this in matrix form: $$\mathbf x^TQ\mathbf x = \begin{bmatrix}x&y&z\end{bmatrix}\begin{bmatrix}1&0&0 \\ 0&1&0 \\ 0&0&-\tan^2\alpha_B \end{bmatrix}\begin{bmatrix}x\\y\\z\end{bmatrix}=0.\tag1$$

Assuming that by “shortest rotation” you mean the smallest rotation angle, then absent the obstructing cone this is achieved when the axis of rotation is perpendicular to $\mathbf v_1$ and $\mathbf v_2$. The first order of business is to determine whether or not you even need to adjust this axis. If the plane of rotation only intersects the cone at its apex, then no adjustment is necessary. A point $\mathbf p$ is exterior to the blue cone if $\mathbf p^TQ\mathbf p\gt0$, so the rotation axis needs adjustment when $(\mathbf v_1\times\mathbf v_2)^TQ(\mathbf v_1\times\mathbf v_2)\gt0$.

For the rotating vector to “just graze” the blue cone, the cone defined by $\mathbf v_1$ and the rotation axis must be tangent to the obstruction. The rotating vector traces part of a circle centered on the rotation axis, and the tangency condition is equivalent to the maximum $z$-coordinate of this circle being equal to $\|\mathbf v_1\|\sin\alpha_B$, that is, the $z$-coordinate value that corresponds to a slant height of $\|\mathbf v_1\|$. This condition in turn is equivalent to the plane of rotation intersecting the circle $x^2+y^2=\|v_1\|^2\sin^2\alpha_B$, $z=\|v_1\|\cos\alpha_B$ in exactly one point.

The axis of rotation lies along the plane that bisects the angle formed by $\mathbf v_1$ and $\mathbf v_2$. It’s easy to verify that if $\mathbf v_1$ and $\mathbf v_2$ are not parallel, then $\mathbf v_1\times\mathbf v_2$ and $\mathbf v_1+\mathbf v_2$ both line on this plane and, moreover, are orthogonal. So, $$\mathbf v_G={\mathbf v_1\times\mathbf v_2\over\|\mathbf v_1\times\mathbf v_2\|}\cos\theta + {\mathbf v_1+\mathbf v_2\over\|\mathbf v_1+\mathbf v_2\|}\sin\theta\tag2$$ is a parameterization of the rotation axes in which the parameter $\theta$ measures angular displacement from the “ideal” perpendicular axis. The plane of rotation can then be represented by the homogeneous vector $\mathbf p=\begin{bmatrix}\mathbf v_G^T \mid -\mathbf v_G^T\mathbf v_1\end{bmatrix}$ (the components of $\mathbf p$ are the coefficients of the point-normal form of the plane equation).

If $\mathbf v_1$ and $\mathbf v_2$ are parallel, then either $\mathbf v_1=\mathbf v_2$, in which case there’s nothing to do, or $\mathbf v_1=-\mathbf v_2$. In the latter case, $\mathbf v_1$ itself is normal to the bisecting plane (though you might want to choose $\mathbf v_2$ instead because of orientation), and a pair of orthogonal unit vectors in the plane for the parameterization of $\mathbf p$ can be found via a variety of well-known techniques (see this question for some).

The circle to be intersected by $\mathbf p$ lies in the plane $z=\|\mathbf v_1\|\cos\alpha_B$, so it suffices to examine the line of intersection of $\mathbf p$ with this plane. This line’s projection onto the $x$-$y$ plane is obtained by setting $z=\|\mathbf v_1\|\cos\alpha_B$, which can be expressed in matrix form as $$\mathbf l=M\mathbf p=\begin{bmatrix}1&0&0&0\\0&1&0&0\\0&0&\|\mathbf v_1\|\cos\alpha_B&1\end{bmatrix}\begin{bmatrix}\mathbf v_G\\-\mathbf v_G^T\mathbf v_1\end{bmatrix}.\tag3$$ The condition that this line is tangent to the circle $x^2+y^2=\|\mathbf v_1\|^2\sin^2\alpha_B$ can be expressed by the equation $\mathbf l^TC^*\mathbf l=0$, where $$C^*=\begin{bmatrix}\|\mathbf v_1\|^2\sin^2\alpha_B&0&0\\0&\|\mathbf v_1\|^2\sin^2\alpha_B&0\\0&0&-1\end{bmatrix}\tag4$$ is the matrix of the dual conic to the circle. Once you multiply all of this out, you end up with a quadratic form in $\cos\theta$ and $\sin\theta$, which you can then solve numerically for $\theta$. In general, there will be four solutions that produce two pairs of opposite-pointing vectors when substituted back into equation (2). You will still need to determine which of these two axes results in the smallest rotation angle, but that’s a fairly straightforward calculation.

For example, let $\alpha_B=\pi/6$, $\mathbf v_1=(1,1,2)^T$ and $\mathbf v_2=(-2,-1,1)^T$. We then have $Q=\operatorname{diag}(1,1,-1/3)$. We compute $(\mathbf v_1\times\mathbf v_2)^TQ(\mathbf v_1\times\mathbf v_2)=\frac{101}3$, so the rotation axis needs adjustment. Plugging these values into the above equations, we find that $$\mathbf v_G=\left(\frac{3 \cos\theta}{\sqrt{35}}-\frac{\sin\theta}{\sqrt{10}},-\sqrt{\frac{5}{7}} \cos\theta,\frac{3 \sin \theta}{\sqrt{10}}+\frac{\cos\theta}{\sqrt{35}}\right) \\ \mathbf v_G^T\mathbf v_1 = \sqrt{\frac52}\sin\theta$$ and so must solve $$\frac{1}{70} \left(93 \cos ^2\theta+\sin\theta \left(7 \left(45 \sqrt{2}-64\right) \sin\theta-6 \sqrt{7} \left(6 \sqrt{2}-5\right) \cos\theta\right)\right)=0.$$ Per Mathematica, the solutions to this equation are approximately $\theta=1.00328$, $\theta=1.61332$, $\theta=4.14487$ and $\theta=4.75492$. These give the following potential rotation axes: $\pm(0.00592596, -0.454303, 0.890828)^T$ and $\pm(-0.337501, 0.0359312, 0.940639)^T$. If you plot the resulting cones, you’ll see that they are indeed tangent to the blocking blue cone.

---

I’ve likely missed some clever geometric insight that would simplify the calculations further. For instance, the equation that results from expanding $\mathbf l^TC^*\mathbf l=0$ can be rewritten as a linear equation in $\sin2\theta$ and $\cos2\theta$, which then turns the problem into one of computing the intersection of a line and the unit circle. As well, one of the conics that results from refactoring the matrix expressions above represents a pair of intersecting lines, and again the problem becomes one of finding the intersection of these lines with the unit circle. This sort of thing makes me suspect that there might be useful geometric relationships to be found.

Answer 2

[Posted as a separate answer since my previous one has already been accepted.]

I sometimes find it easier to generate several potential solutions and then winnow them down instead of trying to compute the single correct solution directly. The tests made in order to discard potential solutions can be simpler than the case analysis required to narrow things down up front. This is the approach that I’ll take for this problem.

I assume that you’ve already determined the that an adjustment must be made to the rotation axis, so I’ll only cover finding that adjusted rotation. The task is to find an axis (and angle) of rotation such that the rotating vector just grazes the blocking blue cone and the rotation angle is minimal among all such axes. Equivalently, we seek a rotation plane such that the circular arc traced on that plane by the rotating vector intersects the cone in exactly one point. For this to occur, the vector must be parallel to the cone’s generator at that point. WLOG, let the cone’s vertex be at the origin and its axis the $z$-axis. An equivalent condition to the preceding one is that the plane of rotation is tangent to the intersection of the cone and one of the planes $z=\pm\|\vec v_1\|\cos\alpha_B$, a circle of radius $\|\vec v_1\|\sin\alpha_B$. If we consider only the circle’s plane, the problem is reduced to the well-studied one of finding the intersections of the tangents to this circle through the common point of intersection $p$ of the rotation planes and the circle’s plane. A simple way to compute these points is to find the intersection of the circle with $p$’s polar line. Lifting these intersections back to 3-D gives you up to four candidate graze points, from which you can then select the one that results in the minimal rotation.

These intersection problems can be solved in a variety of ways. I’ll describe a procedure that computes the points directly. This procedure jumps back and forth between two- and three-dimensional spaces and also switches between Cartesian and homogeneous coordinates, so a bit of notation to help keep things straight: lower-case bold letters such as $\mathbf p$ denote homogeneous coordinate column vectors in $\mathbb{RP}^2$, and upper-case $\mathbf P$ denotes the same for $\mathbb{RP}^3$. In both cases, a tilde ($\tilde{\mathbf p}$,$\tilde{\mathbf P}$) indicates inhomogeneous (Cartesian) coordinates. I’ll also indulge in a bit of common abuse of notation in showing two homogeneous tuples as equal when they represent the same object, when properly speaking they might only be equivalent. To reduce clutter a bit, let $v=\|\vec v_1\|=\|\vec v_2\|$.

We have $\tilde{\mathbf V}_1=[x_1,y_1,z_1]^T$ and $\tilde{\mathbf V}_2=[x_2,y_2,z_2]^T$. The upper circle’s plane is $$\mathbf\Pi_+=[0,0,1,-v\cos\alpha_B]^T.$$ The intersection of this plane with the line through $\mathbf V_1$ and $\mathbf V_2$ is easily found via the Plücker matrix $\mathscr L(\mathbf V_1,\mathbf V_2)=\mathbf V_1\mathbf V_2^T-\mathbf V_2\mathbf V_1^T$ of the line: it is simply $\mathbf P_+=\mathscr L(\mathbf V_1,\mathbf V_2)\mathbf\Pi_+$. This will be a point at infinity if $z_1=z_2$, but the rest of the computation handles this case as well.

Project $\mathbf P_+$ onto the $x$-$y$ plane by dropping its third coordinate. More precisely, compute $\mathbf p_+=\mathscr H\mathbf P_+$, where $$\mathscr H=\begin{bmatrix}1&0&0&0\\0&1&0&0\\0&0&0&1\end{bmatrix}.$$ The circle has matrix $C=\operatorname{diag}(1,1,-v^2\sin^2\alpha_B)$, and the polar line to $\mathbf p_+$ is $\mathbf l_+=C\mathbf p_+$. Putting this cascade all together and simplifying produces the point $$\mathbf p_+=\mathscr H\mathscr L(\mathbf V_1,\mathbf V_2)\mathbf\Pi_+ = \begin{bmatrix} x_1z_2-x_2z_1-(x_1-x_2)v\cos\alpha_B \\ y_1z_2-y_2z_1-(y_1-y_2)v\cos\alpha_B \\z_2-z_1\end{bmatrix}$$ and line $$\mathbf l_+ = C\mathbf p_+ = \begin{bmatrix} x_1z_2-z_1x_2-(x_1-x_2)v\cos\alpha_B \\ y_1z_2-z_1y_2-(y_1-y_2)v\cos\alpha_B \\ (z_1-z_2)v^2\sin^2\alpha_B \end{bmatrix}.\tag1$$ Presented here without proof† is a general formula for the intersection(s) of the line $[\lambda,\mu,\nu]^T$ with a circle of radius $r$ centered at the origin: $$\begin{bmatrix}-\lambda\nu\pm\mu\sqrt{r^2(\lambda^2+\mu^2)-\nu^2} \\ -\mu\nu\mp\lambda\sqrt{r^2(\lambda^2+\mu^2)-\nu^2} \\ \lambda^2+\mu^2 \end{bmatrix}.\tag2$$ If you prefer to work with the pole $\mathbf p_+$ instead, the formula for the intersections of the tangents through a point $[x,y,w]^T$ and a circle of radius $r$ is quite similar: $$\begin{bmatrix} r^2xw\pm ry\sqrt{x^2+y^2-r^2w^2} \\ r^2yw\mp rx\sqrt{x^2+y^2-r^2w^2} \\ x^2+y^2 \end{bmatrix}.$$ (The similarity of these two formulas is not coincidental. They are in fact related by the pole/polar relationship $[\lambda,\mu,\nu]^T=[x,y,-r^2w]^T$ of the point and line.)

Lift each tangent point $\mathbf g=[x_g,y_g,w_g]^T$ back to 3-D. You might as well convert to Cartesian coordinates now because the next step will use Cartesian coordinates exclusively, so this transformation is $$[x_g,y_g,w_g]^T\mapsto\left[\frac{x_g}{w_g},\frac{y_g}{w_g},v\cos\alpha_B\right]^T.$$ (If $w_g=0$, one of the end points of the rotation is interior to the cone, so no solution is possible.) Repeat the intersection algorithm for the plane $z=-v\cos\alpha$. If you’re using the above formulas, a slightly sneaky way to accomplish this is to flip the sign of $v$.

You now have a set of up to four graze points $\tilde{\mathbf G}_i$ from which the minimal rotation is to be selected. As the rotation plane is tilted away from the ideal axis $\tilde{\mathbf A}_0=\tilde{\mathbf V}_1\times\tilde{\mathbf V}_2$ the rotation angle increases, but the radius of the rotation arc decreases. Fortunately, the angle wins, so both rotation angle and arc length are increasing functions of the angle between the rotation axis and $\tilde{\mathbf A}_0$. We want the orientation of the rotation axis to capture the fact that $\tilde{\mathbf G}_i$ also lies on the rotation arc, so set $\tilde{\mathbf A}_i=(\tilde{\mathbf G}_i-\tilde{\mathbf V}_1)\times(\tilde{\mathbf V}_2-\tilde{\mathbf G}_i)$, normalized. If this is zero, that means that the graze point is one of the two end points of the rotation. You already know that the rotation with that graze point is no good—that’s why you’re doing all of this in the first place—so discard that point. Effectively, the rotation will have to go “the long way around.” Finally, compute $\tilde{\mathbf A}_0^T\tilde{\mathbf A}_i$ for each of the surviving points and select the one that maximizes this value.

The above computation can easily be adapted to a cone in general position, though I’m not sure that it’s worth the extra complexity: The planes $\mathbf\Pi_+$ and $\mathbf\Pi_-$ just need to use $\tilde{\mathbf V}_B$, normalized, instead of $[0,0,1]^T$, but you’ll still have to implicitly translate the apex to the origin to compute the rotation axes, and both $\mathscr H$ and the boost back to 3-D will involve a rotation. It seems much easier to me to transform $\mathbf V_1$ and $\mathbf V_2$ and then transform back once you’ve found the new rotation axis.

---

† I derived these formulas using the procedure for computing the intersection of a line and conic described in Richter-Gebert’s Perspectives on Projective Geometry, a variant of which is described here. The algorithm in the book is a direct computation that doesn’t require solving any equations.

---

For example, let $\alpha=\pi/6$, $\vec v_1=(1,1,2)$ and $\vec v_2=(-2,-1,-1)$. Plugging these values into formula (1) gives $\mathbf l_+\approx[-3.36396, -3.24264, 4.5]^T$ and formula (2) yields $\mathbf g_1\approx[3.675, 26.4836, 21.831]^T$, $\mathbf g_2\approx[26.6007, 2.70018, 21.831]^T$. Repeating this for the lower plane produces $\mathbf g_3\approx[22.6048, -139.23, 115.169]^T$ and $\mathbf g_4\approx[-106.88, 92.0459, 115.169]^T$. Converting to Cartesian and lifting to 3-D gives the four graze points $$\tilde{\mathbf G}_1\approx[0.168339, 1.21312, 2.12132]^T \\ \tilde{\mathbf G}_2\approx[1.21848, 0.123686, 2.12132]^T \\ \tilde{\mathbf G}_3\approx[0.196275, -1.20892, -2.12132]^T \\ \tilde{\mathbf G}_4\approx[-0.928031, 0.799224, -2.12132]^T.$$ The corresponding axis dot products are $3.22549$, $0.31171$, $-5.79138$ and $-5.38299$, respectively, so the best one is the first. The results of this calculation are illustrated below.