#!/usr/bin/env python
# coding: utf-8

# In[1]:


get_ipython().run_line_magic('matplotlib', 'inline')

# This is a cell to hide code snippets from displaying
# This must be at first cell!

import numpy as np
from IPython.display import HTML, Image, display
import matplotlib.pyplot as plt

def Figure1():
    font = {'family': 'serif',
            'color':  'black',
            'weight': 'normal',
            'size': 18,
            }

    np.seterr(divide='ignore')
    x = np.linspace(-1, 1, num=500, endpoint=True)
    y1 = 1.0/np.sqrt(1.0-x**2) #1st derivative sin^-1
    y2 = 1.0/(x**2 + 1.0) #1st derivative tan^-1
    y3 = -1.0/np.sqrt(1.0-x**2) #1st derivative cos^-1
    plt.ylim(ymax = 10, ymin = -10)
    plt.xlim(xmax = 1.01, xmin = -1.01)
    ##plt.title('Asymptotically Vanishing 1st Derivative Plot')
    plt.text(-0.65, 3.2,
             r'$y_1=\frac{d}{dx}(\sin^{-1}(x))=+\frac{1}{\sqrt{1-x^2}}$',
             fontdict=font)
    plt.text(-0.65, -3.2,
             r'$y_2=\frac{d}{dx}(\tan^{-1}(x))=-\frac{1}{1+x^2}$',
             fontdict=font)
    plt.text(-0.65, -7.2,
             r'$y_3=\frac{d}{dx}(\cos^{-1}(x))=-\frac{1}{\sqrt{1-x^2}}$',
             fontdict=font)
    plt.plot(x, y1, 'g')
    plt.plot(x, y2, 'r')
    plt.plot(x, y3, 'b')
    plt.xlabel('x', fontdict=font)
    plt.ylabel('$y_1 vs. y_2 vs. y_3$', fontdict=font)
    plt.grid(True)
    plt.show()
 

def Figure2():
    display(Image(filename='img/Kahan Angle 2.png', embed=True))


hide_me = ''
HTML('''<script>
code_show=true; 
function code_toggle() {
  if (code_show) {
    $('div.input').each(function(id) {
      el = $(this).find('.cm-variable:first');
      if (id == 0 || el.text() == 'hide_me') {
        $(this).hide();
      }
    });
    $('div.output_prompt').css('opacity', 0);
  } else {
    $('div.input').each(function(id) {
      $(this).show();
    });
    $('div.output_prompt').css('opacity', 1);
  }
  code_show = !code_show
} 
$( document ).ready(code_toggle);
</script>
<form action="javascript:code_toggle()"><input style="opacity:0" type="submit" value="Click here to toggle on/off the raw code."></form>''')



# ## On the Angle Between Vectors
# 
# <b>David M. Baker, PG</b>
# 
# <b>April 2018</b>

# ### Introduction
# 
# This note will compare the numerical stability of various methods used to calculate the angle between two 3D vectors.  This problem is of great interest because the subtended angle between two vectors figures prominently in many wellbore positioning algorithms, in particular, the Minimum Curvature Method, where tangent vectors of the orientation of a wellbore at consecutive stations are calculated from survey measurements and inputs to a circular arc model of the well path.  In this application, the consecutive pairs of unit length vectors define the orientation of the beginning and ending positions of the arc.  As advocated by Sawaryn and Thorogood, using stable methods leads to simpler, more accurate and robust position calculations [S&T].  In the conclusion, the method offered by Kahan as “Numerically Stable,” using the half-angle arctangent method is presented as a better alternative and briefly presented now.
# 
# Given unit length 3D vectors $\mathbf{t_1}$ and $\mathbf{t_2}$, $\lVert\mathbf{t_1}\rVert=\lVert\mathbf{t_2}\rVert = 1$, the subtended angle $\alpha$ between these vectors is,
# 
# $$\begin{align}
# \alpha &= \angle(\mathbf{t_1},\mathbf{t_2}) \\
# &= 2{\tan^{-1}\left({\lVert\mathbf{t_2}-\mathbf{t_1}\rVert\over{\lVert\mathbf{t_1}+\mathbf{t_2}\rVert}}\right)},
# \end{align}$$
# 
# for all ${\lVert\mathbf{t_1}+\mathbf{t_2}\rVert}^{2}>0$.

# ### Number Format
# Throughout this note, IEEE 754 Standard for Floating-Point Arithmetic in double precision will be assumed.  The IEEE Standard is used as nearly all modern computer systems now support the standard.  The standard allows for the portability of calculations across various computing systems and guarantees a set of operations across all supporting platforms [754].
# 
# Because of the relatively limited accuracy of wellbore survey measurements, such data may safely be stored in single precision floats.  Though the data may be adequately represented in single precision, carrying out all arithmetic calculations in double precision will minimize the potential deleterious effects of rounding errors and subtractive cancellation accumulated through consecutive arithmetic operations, typical of positioning calculations.
# 
# In addition and without loss of generality, in what follows, all vectors are assumed to be unit length unless otherwise specified.

# ### Angle Between 3D Vectors
# The unoriented angle, in radians, between two vectors in 3D space is the distance along the circumference of the unit circle swept out by the vectors in the plane defined by the vector pair.  The calculation of the angle can become problematic as the vectors approach parallel, the degenerate case, and the singular case when the vectors near the anti-parallel position.  In both cases, the swept plane becomes undefined as the now collinear vectors no longer span a plane but are a line.  A line is embedded in infinitely many planes.
# 
# In what follows, $\mathbf{t_1}$ and $\mathbf{t_2}$ are unit length 3D vectors, $\lVert\mathbf{t_1}\rVert=\lVert\mathbf{t_2}\rVert = 1$, the vectors being parameterized by inclination, $\theta$, and azimuth, $\phi$.
# 
# #### Arccosine
# The textbook example to calculate the angle between two vectors employs the definition of the dot or scalar product.  That is, the dot product equals the cosine of the angle between the vectors, and to recover the angle one takes the inverse cosine of the dot product [DOT].
# 
# The subtended angle $\alpha$ between two unit length vectors using the arccosine is [D20],
# 
# $$\begin{align}
# \alpha &= \angle(\mathbf{t_1},\mathbf{t_2}) \\
# &= {\cos^{-1}\left(\mathbf{t_1}\bullet\mathbf{t_2}\right)} \\
# &= \cos^{-1}\left[\cos(\theta_2-\theta_1)-\sin(\theta_1)\sin(\theta_2)\cos(1-(\phi_2-\phi_1))\right] \\
# &= 2\,{\cos^{-1}\left({\lVert\mathbf{t_1}+\mathbf{t_2}\rVert\over{2}}\right)}.
# \end{align}$$
# 
# The last expression is equivalent to the dot product using the length of the adjacent side of the half angle, <b>Figure 2</b>.
# 
# Though this formula well describes the geometry of the dot product, its use in general to calculate the desired angle becomes numerically unstable as the above described limits, (anti-)parallel, are approached, that is, when the angle approaches 0 or π [KHN].  The instability is reflected in the asymptotically vanishing first derivative of the arccosine function where a small change in the input to the function yields an ever increasing large change in the output, <b>Figure 1</b>, the blue curve.  Thus, small measurement errors in the input can become very large on output.  In the context of wellbore positioning, small errors in the angular measurements of a wellbore survey can be amplified and become large displacements from true in the calculated Cartesian position of the station.
# 
# #### Arcsine
# One remedy to the arccosine is to use a formula that incorporates the arcsine [S&T].  Like the arccosine, the inverse sine suffers similar asymptotic degradation but only when the angle between the vectors is perpendicular near +/- π/2, <b>Figure 1</b>, the green curve.  But, the half-angle arcsine formula presented by Sawaryn and Thorogood takes advantage of this fact and allows safe calculation of the angle as the vectors approach parallel, when the angle nears zero.
# 
# The subtended angle $\alpha$ between two unit length vectors using the arcsine is,
# 
# $$\begin{align}
# \alpha &= \angle(\mathbf{t_1},\mathbf{t_2}) \\
# &= {\sin^{-1}\left({\lVert\mathbf{t_2}\times\mathbf{t_1}\rVert}\right)} \\
# &= 2\,\sin^{-1}\left\lbrace\left[{\sin^2\left(\frac{\theta_2-\theta_1}{2}\right)+\sin(\theta_1)\sin(\theta_2)\sin^2\left({\phi_2-\phi_1\over{2}}\right)}\right]^\frac12\right\rbrace \\
# &= 2\,{\sin^{-1}\left({\lVert\mathbf{t_2}-\mathbf{t_1}\rVert\over{2}}\right)}.
# \end{align}$$
# 
# The last expression is equivalent to the length of the cross product vector using the length of the opposite side of the half angle [CRS], <b>Figure 2</b>.
# 
# Though the half-angle arcsine formula mitigates degradation at common straight hole conditions, the formula degrades when the vectors approach anti-parallel, where the subtended angle nears π, because there the half angle nears π/2 and the first derivative vanishes as in the arccosine case.  The anti-parallel position is not common during regular surveying operations but can be encountered in wellbore planning and should be handled.
# 
# #### Arctangent
# As reported by Kahan, the use of the “numerically stable” half-angle arctangent should be favored when calculating the subtended angle between two vectors because the arctangent suffers only moderate rounding error over the range of input covering the subtended angle from 0 to π [KHN], <b>Figure 1</b>, the red curve.
# 
# The subtended angle $\alpha$ between two unit length vectors using the arctangent is,
# 
# $$\begin{align}
# \alpha &= \angle(\mathbf{t_1},\mathbf{t_2}) \\
# &= 2\,{\tan^{-1}\left({\lVert\mathbf{t_2}-\mathbf{t_1}\rVert\over{\lVert\mathbf{t_1}+\mathbf{t_2}\rVert}}\right)},
# \end{align}$$
# 
# for all ${\lVert\mathbf{t_1}+\mathbf{t_2}\rVert}^{2}>0$.  The last expression uses the ratio of the length of the opposite side over the length of the adjacent side of the half angle [KHN], <b>Figure 2</b>.
# 
# The rational expression for the definition of the angle shows no asymptotic behavior over the valid range of input and so does not exacerbate error in the data as its cousins do in the extreme.

# ### Conclusion
# 
# As illustrated in <b>Figure 1</b>, both the arccosine and arcsine, when near their degenerate and singular cases respectively, parallel for the inverse cosine and anti-parallel for the inverse sine, can suffer large errors in their output for relatively small variance in their input.   Because of the asymptotic nature of the first derivatives of these functions, as the limits are approached, small steps in the input data yield ever increasing large and greatly amplified output and should be avoided.
# 
# On the other hand, as shown in <b>Figure 1</b>, the arctangent exhibits near constant change over the whole range of input admitted by the orientation of a pair of unit length vectors, suffering only modest rounding errors up to and at the limits of the extremes, when the vectors are pointing in either parallel or anti-parallel directions.  One criticism of this method is the potential for divide-by-zero errors.  The denominator in the half-angle arctangent method goes to zero when the vectors are anti-parallel.  To avoid divide-by-zero errors, the proposed method tests the length squared of the summation vector ${\lVert\mathbf{t_1}+\mathbf{t_2}\rVert}^{2}>0$.  Though the other two methods do not specifically require this test in their application to wellbore positioning, if the vectors are anti-parallel to each other, the summation vector vanishes and no solution can be found anyway.  So, this singular condition must be resolved in all methods, either for the first two methods, by testing if the calculated angle is less than $\pi$, or for the length of the summation vector in the latter.  Because of its stability, the half-angle arctangent should be preferred for most applications.
# 

# In[2]:


Figure1()


# <b>Figure 1.</b> Asymptotically vanishing 1st derivative plot. The plot shows as the angle approaches zero and $\pi$, the 1st derivative of the inverse cosine (blue) goes to negative infinity. Similarly, as the angle approaches $\pm\frac{\pi}{2}$, the 1st derivative of the inverse sine (green) goes to positive infinity. Conversely, the 1st derivative of the inverse tangent (red) remains bounded between zero and one inclusively, as the angle ranges between $\pm\frac{\pi}{2}$ (off the plot and not shown, as the domain, x, goes to $\pm\infty$, the tails go to zero in the limit).

# In[3]:


Figure2()


# <b>Figure 2.</b> Derivation of the angle between two vectors per William Kahan.  Both t1 and t2 are unit length vectors.  The tangent of half alpha is equal to the ratio of the length of the opposite side of the triangle, (length of t2-t1) = $2 * \sin(\alpha/2)$ (red), over the length of the adjacent side of the triangle, (length of t1+t2) = $2 * \cos(\alpha/2)$ (green).

# ### References
# 
# [754] IEEE 754. (2018, April 06). Retrieved April 07, 2018, from https://en.wikipedia.org/wiki/IEEE_754.
# 
# [D20] API Bulletin. D20 (1985, December 01). Directional Drilling Survey Calculation Methods and Terminology, first edition, API, Dallas, Texas
# 
# [DOT] Dot product. (2018, April 06). Retrieved April 07, 2018, from https://en.wikipedia.org/wiki/Dot_product.
# 
# [KHN] Kahan, W. (2016, February 25). Cross-Products and Rotations in 2- and 3-Dimensional Euclidean Spaces. Retrieved April 7, 2018, from http://people.eecs.berkeley.edu/~wkahan/MathH110/Cross.pdf.
# 
# [S&T] Sawaryn, S. J., & Thorogood, J. L. (2005, March 1). A Compendium of Directional Calculations Based on the Minimum Curvature Method. Society of Petroleum Engineers. doi:10.2118/84246-PA.