Numpy - Transformations between coordinate systems

Question

Using Numpy I want to transform position vectors between coordinate systems.

To help visualize the problem: http://tube.geogebra.org/student/m1097765

I have two planes in 3D space. Each plane is defined by its center:

C[0] = (X0, Y0, Z0)

C[1] = (X1, Y1, Z1)

(X,Y,Z are referred to a global coordinate system)

C = np.array([[0,0,0],[-4,2,1]])

and its normal vector:

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])

alpha = elevation angle

A = azimuth angle

H = np.array([[-0.23, -0.45, 0.86], [-0.12, -0.24, 0.86]])

I have a point p(xp, yp, 0) lying on plane 0 (xp, yp are referred to a local coordinate system with center C[0] and its xyz axes are aligned with the global XYZ axes when alpha = A = 0)

I transform from the local coordinate system of plane 0 to global with the following functions:

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 
    >>> C = np.array([-10,20,1200]) 
    >>> H = np.array([-0.23, -0.45, 0.86])
    >>> p = np.array([-150, -1.5, 0])
    >>> P = local2Global(C, H, p)
    >>> np.linalg.norm(P-C) == np.linalg.norm(p)
    True
    '''
    alpha = np.rad2deg(np.arcsin(planeNormalVector[2]))
    A = np.where(planeNormalVector[1] > 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

The above seems to work as expected.

Then I'm computing the intersection of a line passing from a point on plane 0 p(xp,yp,0) and has the direction vector of S = (0.56, -0.77, 0.3)

>>> C = np.array([[0,0,0],[-4,2,1]]) # plane centers
>>> H = np.array([[-0.23, -0.45, 0.86], [-0.12, -0.24, 0.86]]) # plane normal vectors
>>> S = np.array([0.56, -0.77, 0.3]) # a direction vector
>>> p = np.array([-1.5, -1.5, 0]) # a point on a plane

>>> intersectingPlaneIndex = 0 # choose intersecting plane, this plane has the point p on it
>>> intersectedPlaneIndex = 1 # this plane intersects with the line passing from p with direction vector s

>>> P = local2Global(C[intersectingPlaneIndex], H[intersectingPlaneIndex], p)   # point p in global coordinates
>>> np.isclose(np.linalg.norm(p), np.linalg.norm(P - C[intersectingPlaneIndex]), 10e-8)
True

So the first transformation is successful.

Now let's find intersection point E in global coordinates

>>> t = np.dot(H[intersectedPlaneIndex], C[intersectedPlaneIndex, :] - P) / np.dot(H[intersectedPlaneIndex], S)
>>> E = P + S * t
>>> np.around(E, 2)
array([ 2.73, -0.67,  1.19])

So far so good, I found the point E (global coordinates) which lies on plane 1.

The problem:

How can I convert point E from global coordinates to the coordinate system of plane 1 and obtain e(xe, ye, 0)?

I tried:

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] > 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

And:

>>> e = global2Local(C[intersectedPlaneIndex], H[intersectedPlaneIndex], E)
>>> e
array([ -2.54839059e+00,  -5.48380179e+00,  -1.42292121e-03])

In first look this seem ok, as long as e[2] is near zero but,

>>> np.linalg.norm(E-C[intersectedPlaneIndex])
7.2440723159783182
>>> np.linalg.norm(e)
6.0470140356703537

So the transformation is wrong. Any ideas?

Answer 1

I'd recommend reading this and this. For the first one, look at the concept of homogenous coordinates, as for spatial transforms with different origins that is kinda needed. For the second one, look at how the camera "look-at" transform is performed. As long as you have the orthonormal basis vectors (easy enough to get from the angles), you can use the equations in the second one to do the transform. The post linked in the comments seems to cover similar material.