How to draw arrows by rotating lines in 3d space?

Question

I am trying to figure out direction vectors of the arrowheads of an arrow. Basically I'm given a normalized direction vector (u,v,w) and I need the normalized direction vectors of the its two arrow heads which make a 15 degree angle.

My plan is to first start off with a simple normalized vector (0,0,1). The direction vectors of its arrow heads are (-sin(15), 0, -cos(15)) and (sin(15), 0, -cos(15)), and then rotate (0,0,1) so its parallel to the given (u,v,w). I do this by projecting (u,v,w) on its x-axis, and getting its angle relative to (0,0,1), then projecting on the y-axis, and getting its angle relative to (0,0,1), then I use the 3d rotation matrices to use those found angles to rotate the arrow head direction vector.

Approach: Let $p = (u, v, w)$, and let the projections of this vector on the $yz$ and $xz$ planes be $p^x = (0, v, w)$ and $p^y = (u, 0, w)$. The vectors $p^x$ and $p^y$ are normalized as follows: $$ p^x = \begin{cases} p^x & \quad \text{if} \quad u = 1 \\ \frac{p^x}{\sqrt{v^2+w^2}} & \quad \text{otherwise} \end{cases} $$ and $$ p^y = \begin{cases} p^y & \quad \text{if} \quad v = 1 \\ \frac{p^y}{\sqrt{u^2+w^2}} & \quad \text{otherwise} \end{cases} $$ The angles made by $p$ with the projections on the $xz$ and $yz$ planes are $$ \theta_x = \cos^{-1} (p^x_3) \quad \text{and} \quad \theta_y = \cos^{-1} (p^y_3) $$ Define $c_{px} = \cos\theta_x$, $s_{px} = \sin\theta_x$, $c_{py} = \cos\theta_y$, $s_{py} = \sin\theta_y$. Let the arrow head vectors be defined as $a^h_1 = (-\sin\alpha, 0, -\cos\alpha)$ and $a^h_2 = (\sin\alpha, 0, -\cos\alpha)$ whether $\alpha$ is the arrow head angle. Then the rotated version $a_{\text{rot}}$ of the arrowhead vector $a$ is given by $$ a_{\text{rot}} = \begin{bmatrix} c_{py} & 0 & s_{py} \\ 0 & 1 & 0 \\ -s_{py} & 0 & c_{py} \end{bmatrix} \begin{bmatrix} x_{r1}\\ x_{r2} \\ x_{r3} \end{bmatrix} $$ where $$ \begin{bmatrix} x_{r1}\\ x_{r2} \\ x_{r3} \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & -s_{px} \\ 0 & 0 & c_{px} \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix} $$

I have this pythoncode below, but its not working properly. Does anyone see whats wrong?

Thanks

        ra = math.radians(15) 
        ca = math.cos(ra)
        sa = math.sin(ra)

        px = (0,v,w)
        if u!=1:
            px = [i/float(math.sqrt(v**2 + w**2)) for i in px]

        py = (u,0,w)
        if v!=1:
            py = [i/float(math.sqrt(u**2 + w**2)) for i in py]

        pxangle = math.acos(px[2])
        pyangle = math.acos(py[2])

        cpx = math.cos(pxangle)
        spx = math.sin(pxangle)
        cpy = math.cos(pyangle)
        spy = math.sin(pyangle)

        def rotatefunction(ah):
            xr = (ah[0], -spx*ah[2], cpx*ah[2])
            return (cpy*xr[0]+spy*xr[2], xr[1], -spy*xr[0]+cpy*xr[2]) 

        lah = rotatefunction((-sa, 0, -ca))
        rah = rotatefunction((sa, 0, -ca))

Answer 1

There seems to be an error in one of your rotation matrices. However, your approach seems unnecessarily complicated and can be simplified if you use the Euler-Rodrigues formula.

The Matlab script below uses that formula and should be easily translatable to Python. The output from the script is shown in the figure.

function drawArrow

  origin = [0 0 0];

  % This is the vector you want to attach an arrowhead to
  vector = [1 2 1.5];
  vectorlen = norm(vector);
  vectornorm = vector/vectorlen;

  ra = 15*pi/180;
  ca = cos(ra);
  sa = sin(ra);

  xaxis = [1 0 0];

  downarrow = [-ca -sa 0]
  downarrow = xaxis + downarrow;

  uparrow = [-ca sa 0]
  uparrow = xaxis + uparrow;

  plotArrow(origin, xaxis, downarrow, uparrow, [0 0 1], 1);

  angle = acos(dot(vectornorm,xaxis))
  axis = cross(vectornorm, xaxis)

  [vRot, dRot, uRot] = rotateAndPlot(origin, xaxis, downarrow, uparrow, angle, axis);

  scaleAndPlot(origin, vRot, dRot, uRot, vectorlen);

  % Plot reference vector and axis vector
  plot3([origin(1) vector(1)],[origin(2) vector(2)],[origin(3) vector(3)], 'k-'); hold on;
  plot3([origin(1) axis(1)],[origin(2) axis(2)],[origin(3) axis(3)], 'k-'); hold on;

function scaleAndPlot(origin, vec, down, up, scale_fac)

  vecScaled = vec*scale_fac;
  downScaled = down*scale_fac;
  upScaled = up*scale_fac;

  plotArrow(origin, vecScaled, downScaled, upScaled, [1 0.2 0.2], 3);

function [vecRotated, downRotated, upRotated] = rotateAndPlot(origin, vec, down, up, angle, axis)

  R = rotation(angle, axis);

  vecRotated = R*vec';
  downRotated = R*down';
  upRotated = R*up';

  plotArrow(origin, vecRotated, downRotated, upRotated, [0 1 0], 2);

function [R] = rotation(angle, axis)

   axis = axis/norm(axis);
   ca = cos(angle);
   sa = sin(angle);
   I = [[1 0 0];[0 1 0];[0 0 1]];
   aa = axis'*axis
   A = [[0 axis(3) -axis(2)];[-axis(3) 0 axis(1)];[axis(2) -axis(1) 0]]
   R = (I - aa)*ca + aa + A*sa;

function plotArrow(origin, vector, downArrow, upArrow, color, linewidth)

  grid on;
  plot3([origin(1) vector(1)],[origin(2) vector(2)],[origin(3) vector(3)], ...
        '-', 'Color', color,  'LineWidth', linewidth); hold on;
  plot3([downArrow(1) vector(1)],[downArrow(2) vector(2)],[downArrow(3) vector(3)], ...
        '-', 'Color', color,  'LineWidth', linewidth); hold on;
  plot3([upArrow(1) vector(1)],[upArrow(2) vector(2)],[upArrow(3) vector(3)], ...
        '-', 'Color', color,  'LineWidth', linewidth); hold on;

  axis equal;