title: Numpy - Transformations between coordinate systems
body: <p>Using Numpy I want to transform position vectors between coordinate systems.</p>
  
  <p>To help visualize the problem: <a href="http://tube.geogebra.org/student/m1097765" rel="noreferrer">http://tube.geogebra.org/student/m1097765</a> </p>
  
  <p>I have two planes in 3D space.
  Each plane is defined by its center:</p>
  
  <pre><code>C[0] = (X0, Y0, Z0)
  
  C[1] = (X1, Y1, Z1)
  </code></pre>
  
  <p>(X,Y,Z are referred to a global coordinate system)</p>
  
  <pre><code>C = np.array([[0,0,0],[-4,2,1]])
  </code></pre>
  
  <p>and its normal vector:</p>
  
  <pre><code>H[0] = (cos(alpha[0])*sin(A[0]), cos(alpha[0])*cos(A[0]), sin(A[0])
  
  H[1] = (cos(alpha[1])*sin(A[1]), cos(alpha[1])*cos(A[1]), sin(A[1])
  </code></pre>
  
  <p><code>alpha</code> = elevation angle</p>
  
  <p><code>A</code> = azimuth angle</p>
  
  <pre><code>H = np.array([[-0.23, -0.45, 0.86], [-0.12, -0.24, 0.86]])
  </code></pre>
  
  <p>I have a point <code>p(xp, yp, 0)</code> lying on plane <code>0</code> (<code>xp</code>, <code>yp</code> are referred to a local coordinate system with center <code>C[0]</code> and its <code>xyz</code> axes are aligned with the global <code>XYZ</code> axes when <code>alpha = A = 0</code>)</p>
  
  <p>I transform from the local coordinate system of plane 0 to global with the following functions:</p>
  
  <pre><code>import numpy as np
  
  def rotateAxisX(alpha):
      '''
      Rotation about x axis
      :param alpha: plane altitude angle in degrees
      :return: x-axis rotation matrix
      '''
      rotX = np.array([[1, 0, 0], [0, np.cos(np.deg2rad(alpha)), np.sin(np.deg2rad(alpha))], [0, -np.sin(np.deg2rad(alpha)), np.cos(np.deg2rad(alpha))]])
      return rotX
  
  def rotateAxisZ(A):
      '''
      Rotation about z axis
      :param A: plane azimuth angle in degrees
      :return: z-axis rotation matrix
      '''
      rotZ = np.array([[np.cos(np.deg2rad(A)), np.sin(np.deg2rad(A)), 0], [-np.sin(np.deg2rad(A)), np.cos(np.deg2rad(A)), 0], [0, 0, 1]])
      return rotZ
  
  def local2Global(positionVector, planeNormalVector, positionVectorLocal):
      '''
      Convert point from plane's local coordinate system to global coordinate system
      :param positionVector: plane center in global coordinates
      :param planeNormalVector: the normal vector of the plane
      :param positionVectorLocal: a point on plane (xp,yp,0) with respect to the local coordinate system of the plane
      :return: the position vector of the point in global coordinates 
      &gt;&gt;&gt; C = np.array([-10,20,1200]) 
      &gt;&gt;&gt; H = np.array([-0.23, -0.45, 0.86])
      &gt;&gt;&gt; p = np.array([-150, -1.5, 0])
      &gt;&gt;&gt; P = local2Global(C, H, p)
      &gt;&gt;&gt; np.linalg.norm(P-C) == np.linalg.norm(p)
      True
      '''
      alpha = np.rad2deg(np.arcsin(planeNormalVector[2]))
      A = np.where(planeNormalVector[1] &gt; 0, np.rad2deg(np.arccos(planeNormalVector[1] / np.cos(np.deg2rad(alpha)))), 360 - np.rad2deg(np.arccos(planeNormalVector[1] / np.cos(np.deg2rad(alpha)))))
      positionVectorGlobal = positionVector + np.dot(np.dot(rotateAxisZ(A), rotateAxisX(90 - alpha)), positionVectorLocal)
      return positionVectorGlobal
  </code></pre>
  
  <p>The above seems to work as expected.</p>
  
  <p>Then I'm computing the intersection of a line passing from a point on plane 0 <code>p(xp,yp,0)</code> and has the direction vector of <code>S = (0.56, -0.77, 0.3)</code></p>
  
  <pre><code>&gt;&gt;&gt; C = np.array([[0,0,0],[-4,2,1]]) # plane centers
  &gt;&gt;&gt; H = np.array([[-0.23, -0.45, 0.86], [-0.12, -0.24, 0.86]]) # plane normal vectors
  &gt;&gt;&gt; S = np.array([0.56, -0.77, 0.3]) # a direction vector
  &gt;&gt;&gt; p = np.array([-1.5, -1.5, 0]) # a point on a plane
  
  &gt;&gt;&gt; intersectingPlaneIndex = 0 # choose intersecting plane, this plane has the point p on it
  &gt;&gt;&gt; intersectedPlaneIndex = 1 # this plane intersects with the line passing from p with direction vector s
  
  &gt;&gt;&gt; P = local2Global(C[intersectingPlaneIndex], H[intersectingPlaneIndex], p)   # point p in global coordinates
  &gt;&gt;&gt; np.isclose(np.linalg.norm(p), np.linalg.norm(P - C[intersectingPlaneIndex]), 10e-8)
  True
  </code></pre>
  
  <p>So the first transformation is successful.</p>
  
  <p>Now let's find intersection point E in global coordinates</p>
  
  <pre><code>&gt;&gt;&gt; t = np.dot(H[intersectedPlaneIndex], C[intersectedPlaneIndex, :] - P) / np.dot(H[intersectedPlaneIndex], S)
  &gt;&gt;&gt; E = P + S * t
  &gt;&gt;&gt; np.around(E, 2)
  array([ 2.73, -0.67,  1.19])
  </code></pre>
  
  <p>So far so good, I found the point <code>E</code> (global coordinates) which lies on plane 1.</p>
  
  <p>The problem:</p>
  
  <p>How can I convert point <code>E</code> from global coordinates to the coordinate system of plane 1 and obtain <code>e(xe, ye, 0)</code>?</p>
  
  <p>I tried:</p>
  
  <pre><code>def global2Local(positionVector, planeNormalVector, positionVectorGlobal):
      '''
      Convert point from global coordinate system to plane's local coordinate system
      :param positionVector: plane center in global coordinates
      :param planeNormalVector: the normal vector of the plane
      :param positionVectorGlobal: a point in global coordinates
      :note: This function translates the given position vector by the positionVector and rotates the basis axis in order to obtain the positionVectorCoordinates in plane's coordinate system
      :warning: it does not function as it should
      '''
      alpha = np.rad2deg(np.arcsin(planeNormalVector[2]))
      A = np.where(planeNormalVector[1] &gt; 0, np.rad2deg(np.arccos(planeNormalVector[1] / np.cos(np.deg2rad(alpha)))), 360 - np.rad2deg(np.arccos(planeNormalVector[1] / np.cos(np.deg2rad(alpha)))))
      positionVectorLocal = np.dot(np.dot(np.linalg.inv(rotateAxisZ(A)), np.linalg.inv(rotateAxisX(90 - alpha))), positionVectorGlobal - positionVector) + positionVectorGlobal
      return positionVectorLocal
  </code></pre>
  
  <p>And:</p>
  
  <pre><code>&gt;&gt;&gt; e = global2Local(C[intersectedPlaneIndex], H[intersectedPlaneIndex], E)
  &gt;&gt;&gt; e
  array([ -2.54839059e+00,  -5.48380179e+00,  -1.42292121e-03])
  </code></pre>
  
  <p>In first look this seem ok, as long as e[2] is near zero but,</p>
  
  <pre><code>&gt;&gt;&gt; np.linalg.norm(E-C[intersectedPlaneIndex])
  7.2440723159783182
  &gt;&gt;&gt; np.linalg.norm(e)
  6.0470140356703537
  </code></pre>
  
  <p>So the transformation is wrong. Any ideas?</p>