title: Checking if point inside a cone - values and as a result plot (with matplotlib) inaccurate
body: <p>I am receiving a stream of 3D points with <code>x</code>, <code>y</code> and <code>z</code> ranging from <code>0</code> to <code>65534</code>. Every <code>t</code> milliseconds I take a sample of points and check if the points lie outside an automatically defined cone. Here are the steps I use in my algorithm:</p>
  
  <ol>
  <li>The first point is used to define the <strong>tip of a right circular cone</strong> (I'll use the <code>x_tip</code>, <code>y_tip</code> and <code>z_tip</code> coordinates to mark). The cone's height <strong>h</strong> is equal to <code>z_tip</code></li>
  <li>Using <code>x_tip</code> and <code>y_tip</code> of the tip I calculate the tip's projection on the plane at <code>z = 0</code></li>
  <li><p>Given an angle <code>a</code> I calculate the cone's radius <code>r</code> using the Pythagorean theorem:</p>
  
  <pre><code>cos_angle = cos(a)
  if cos_angle != 0:
    hypotenuse = h / cos_angle
    sin_angle = sin(a)
    if sin_angle != 0:
      r = hypotenuse * sin_angle
  </code></pre></li>
  <li><p>After calculating the height and the radius I can apply a two-step test to determine whether the given point (beside the tip) is outside the cone (note that both points inside and lying on the surface of the cone are marked as <em>valid</em> that is part of the cone):</p>
  
  <ul>
  <li><p>Point above cone - all points have <code>z</code> between <code>0</code> and <code>65534</code> with the same range also applying to the height of the cone. So checking whether a point lies below the cone is pointless (and impossible :)). Doing this check is a simple one-dimensional check that is check whether <code>z_point &gt; z_tip</code> (if <code>true</code> then no further checking is necessary since the point is clearly above the cone). This test is in fact done as a prerequirement step for a point to land in the sample set. So all points that are being checked are at or below the level of <code>z_zip</code></p></li>
  <li><p>Point inside, lying on the circumference or outside the slice of the cone at the height of the point - this test is two-dimensional and here I use Euclidean distance (note that I have mapped the 0..65534 range to 0..1 because I'm printing this stuff in my terminal and floating point numbers are easier to compare visually (omho)):</p>
  
  <pre><code>def pointInsideOrOnCircle(x, y, z):
    if r == 0:
      return False
  
    # Map the point that we need to validate to 0..1
    x = x/65534.
    y = y/65534.
    z = z/65534.
  
    # Map the first from the samples points (tip of cone) to 0..1
    x_tip = points[0, 0]/65534.
    y_tip = points[0, 1]/65534.
  
    # Map cone's radius and height
    h = h/65534.
    r = r/65534.
  
    # Calculate Euclidean distance from point to center of slice
    d = sqrt((x_tip - x)**2 + (y_tip - y)**2)
  
    # Calculate the radius of the cone's slice
    r_slice = (r*z)/h
  
    return d &lt;= r_slice # If true, point lies inside or on surface of cone
  </code></pre></li>
  </ul></li>
  </ol>
  
  <p>I was thinking that this works but then I decided to plot it...The result looks rather strange:</p>
  
  <p><strong>Example:</strong></p>
  
  <p>For the sample </p>
  
  <pre><code>[
      [65534,62471,54690],
      [65534,59646,52585],
      [65534,56317,50283],
      [65240,52776,47825],
      [64508,49397,45249],
      [63357,46581,42549],
      [61838,44648,39719],
      [60011,43742,36772],
      [57902,43845,33730],
      [55473,44884,30622],
      [52618,46840,27472],
      [49215,49783,24311],
      [45291,53734,21167],
      [41294,58283,18073],
      [37942,62512,15059],
      [35498,65534,12158],
      [33748,65534,9399],
      [32488,65534,6808],
      [31445,65534,4405],
      [30533,65534,2205]
  ]
  </code></pre>
  
  <p>I get</p>
  
  <p><a href="https://i.stack.imgur.com/UwK5D.png" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/UwK5D.png" alt="enter image description here"></a></p>
  
  <p><strong>Example:</strong></p>
  
  <p>For the sample</p>
  
  <pre><code>[
      [39130,65534,50773],
      [39387,65534,47926],
      [39700,65534,44867],
      [39991,65534,41689],
      [40185,65534,38404],
      [40213,65534,35034],
      [40023,65534,31614],
      [39583,65534,28187],
      [37913,65534,21521],
      [36686,65534,18402],
      [35219,65534,15506],
      [33574,65534,12883],
      [31892,65534,10570],
      [30378,65534,8592],
      [28339,65534,5679],
      [27757,65534,4740],
      [27361,65534,4138],
      [27094,65534,3859],
      [26824,65534,4017],
      [26790,65534,5566]
  ]
  </code></pre>
  
  <p>I get</p>
  
  <p><a href="https://i.stack.imgur.com/nVkUJ.png" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/nVkUJ.png" alt="enter image description here"></a></p>
  
  <p>Valid points are marked as <strong>green</strong> circles while invalid ones - as <strong>red</strong> circles. As you can see things don't add up - I have some of the points marked as valid even though these lie outside the cone and others that lie inside the cone but are marked as invalid. I have no idea where my mistake is - is it the algorithm, is it some rounding issue, is it the plot itself...</p>
  
  <p>The full code below has some of the variables named a little bit different but it should be easy to follow up on these from the description above). The arrows are extra visualization for me to be able to see a couple of things easily (for example the red arrow points at the cone's tip, the blue one - at the cone's base and the yellow one - at the last point in the sample set) so if it bothers you you can remove those.</p>
  
  <pre><code>#!/usr/bin/env python3
  import matplotlib.pyplot as plt
  from mpl_toolkits.mplot3d import Axes3D
  from matplotlib.patches import FancyArrowPatch
  from mpl_toolkits.mplot3d import proj3d
  import numpy as np
  from math import pi, cos, sin, sqrt
  
  
  class Arrow3D(FancyArrowPatch):
      def __init__(self, xs, ys, zs, *args, **kwargs):
          FancyArrowPatch.__init__(self, (0, 0), (0, 0), *args, **kwargs)
          self._verts3d = xs, ys, zs
  
      def draw(self, renderer):
          xs3d, ys3d, zs3d = self._verts3d
          xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
          self.set_positions((xs[0], ys[0]), (xs[1], ys[1]))
          FancyArrowPatch.draw(self, renderer)
  
  points = np.array(
  [
      [65534,62471,54690],
      [65534,59646,52585],
      [65534,56317,50283],
      [65240,52776,47825],
      [64508,49397,45249],
      [63357,46581,42549],
      [61838,44648,39719],
      [60011,43742,36772],
      [57902,43845,33730],
      [55473,44884,30622],
      [52618,46840,27472],
      [49215,49783,24311],
      [45291,53734,21167],
      [41294,58283,18073],
      [37942,62512,15059],
      [35498,65534,12158],
      [33748,65534,9399],
      [32488,65534,6808],
      [31445,65534,4405],
      [30533,65534,2205]
  ]
  )
  
  fig = plt.figure()
  
  ax = Axes3D(fig)
  ax.set_xlabel('X')
  ax.set_ylabel('Y')
  ax.set_zlabel('distance')
  
  thetaLevels = np.arange(0, 2*pi + pi/50, pi/50)
  cone_height = points[0, 2]
  cone_angle = 20.
  cone_radius = 0.
  
  cos_cone_angle = cos(cone_angle)
  if cos_cone_angle != 0:
      hypotenuse = cone_height/cos_cone_angle
      sin_cone_angle = sin(cone_angle)
      if sin_cone_angle != 0:
          cone_radius = hypotenuse * sin_cone_angle
  
  
  def pointInsideCircle(x, y, z):
      if cone_radius == 0:
          return False
  
      # Normalize to scale 0..1
      x = x/65534.
      y = y/65534.
      z = z/65534.
  
      tip_x = points[0, 0]/65534.
      tip_y = points[0, 1]/65534.
  
      cone_radius_n = cone_radius/65534.
      cone_height_n = cone_height/65534.
  
      cone_slice_radius = (cone_radius_n * z)/cone_height_n
      d = cone_slice_radius**2 - (tip_x - x)**2 + (tip_y - y)**2
      d_2 = sqrt((tip_x - x)**2 + (tip_y - y)**2)
  
      print('''
      point[x=%f, y=%f, z=%f] | slice[x=%f, y=%f, z=%f]\n
      cone(radius=%f, height=%f)\n
      slice(radius=%f)\n
      d = %f\n
      d_2 = %f
      ''' % (x, y, z, tip_x, tip_y, z, cone_radius_n, cone_height_n, cone_slice_radius, d, d_2))
  
  #    if d &lt; 0:
  #        return False
      return d_2 &lt;= cone_slice_radius
  
  
  for x, y, z in points:
      # Slice circumference
      # X,Y of each point lying on the circumference
      sliceCircX = z * np.array([cos(t) for t in thetaLevels]) + points[0, 0]
      sliceCircY = z * np.array([sin(t) for t in thetaLevels]) + points[0, 1]
      ax.plot(sliceCircX,
              sliceCircY,
              -z + points[0, 2],
              'b-')
  
      # Plot center of cone slice at height of point
      ax.scatter(points[0, 0], points[0, 1], z, s=60, c='k', lw=0)
  
      valid = pointInsideCircle(x, y, z)
      print('point[%d, %d, %d] %s cone' % (x, y, z, ('inside of/lies on' if valid else 'outside of')))
  
      # Plot point
      ax.scatter(x, y, z, s=60, c=('g' if valid else 'r'), lw=0)
  
      # Point to center of slice
      p2cs = Arrow3D([x, points[0, 0]],
                     [y, points[0, 1]],
                     [z, z],
                     mutation_scale=20,
                     lw=1, arrowstyle='-', color='b', linestyle='dotted')
      ax.add_artist(p2cs)
  
  aStart = Arrow3D([0, points[0, 0]],
                   [0, points[0, 1]],
                   [0, points[0, 2]],
                   mutation_scale=20,
                   lw=3, arrowstyle="-|&gt;", color="r")
  ax.add_artist(aStart)
  
  aEnd = Arrow3D([0, points[-1, 0]],
                 [0, points[-1, 1]],
                 [0, points[-1, 2]],
                 mutation_scale=20,
                 lw=3, arrowstyle="-|&gt;", color="y")
  ax.add_artist(aEnd)
  
  base = [points[0, 0], points[0, 1], 0]
  aBase = Arrow3D([0, base[0]],
                  [0, base[1]],
                  [0, base[2]],
                  mutation_scale=20,
                  lw=3, arrowstyle="-|&gt;", color="c")
  ax.add_artist(aBase)
  
  tipToBase = [points[0, 0],
               points[0, 1],
               0]
  aTipToBase = Arrow3D([points[0, 0], tipToBase[0]],
                       [points[0, 1], tipToBase[1]],
                       [points[0, 2], tipToBase[2]],
                       mutation_scale=20,
                       lw=3, arrowstyle="-|&gt;", color="b")
  ax.add_artist(aTipToBase)
  
  fig.show()
  input('Press any key to exit')
  </code></pre>