Points and Vectors

I occasionaly write scripts for geometry optimization, therefore I have to wrestle with geometry. Graphically very easy problems become Ye Royale Paine In Ye Buttocks to solve analytically. At first I wrestle with piles of points, lines, determinants, trigonometric functions and whatchamacallits. Then replace all of that with a few points and vectors. Here are a few simple functions I either found online or wrote myself to help me with geometric calculations.

Angle Between Vectors

I found this on Stackoverflow. Since it works, I had no reason to fix it.

import numpy as np
import scipy as sp

def vector(x, y, z):
    return np.array([x, y, z])

def norm(v):
    return np.linalg.norm(v)

def unit_vector(vector):
    return vector / norm(vector)

def angle_between(v1, v2):
    """ Kudos: https://stackoverflow.com/questions/2827393/
    Returns the angle in radians between vectors 'v1' and 'v2':
   
    >>> angle_between((1, 0, 0), (0, 1, 0))
    1.5707963267948966
    >>> angle_between((1, 0, 0), (1, 0, 0))
    0.0
    >>> angle_between((1, 0, 0), (-1, 0, 0))
    3.141592653589793
    """

    v1_u = unit_vector(v1)
    v2_u = unit_vector(v2)

    return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0))

Point Rotation

It’s a generic problem but I can still share the code that worked so far.

def rotate(point, angle, axis='x'):
    """ rotates a point around an axis by specified angle """
    if axis == 'y':
        rot_matrix = np.array([
            [np.cos(angle),  0, np.sin(angle)],
            [0,              1, 0            ],
            [-np.sin(angle), 0, np.cos(angle)],
        ])
    if axis == 'z':
        rot_matrix = np.array([
            [np.cos(angle), -np.sin(angle), 0],
            [np.sin(angle), np.cos(angle),  0],
            [0,             0,              1],
        ])
    else: # == 'x':
        rot_matrix = np.array([
                [1, 0, 0],
                [0, np.cos(angle), -np.sin(angle)],
                [0, np.sin(angle), np.cos(angle)],
            ])

    return point.dot(rot_matrix)

De-rotate a Point Back to XY-Plane

This is not a projection of the point to XY plane but a rotation around x-axis to that plane. There are two options: calculate the angle and rotate the point by that angle or transform cartesian coordinates to cylindrical and set angle to zero. The latter is the more elegant but the first works just as well (depending on your precision requirements).

def de_rotate(p):
    """ rotate the point p around x-axis so that z-axis=0 """
    # measure angle between normal and point
    normal = np.array([0, 1, 0])
    point = np.array([0, p[1], p[2]])
    phi = angle_between(point, normal)
    
    # rotate the oritinal point around x-axis by measured angle
    rotated = rotate(p, phi)
    return rotated, phi

def de_rotate(p):
    # alternative solution:
    # calculate r in cylindrical coordinate system;
    # also return the angle in cylindrical system
    return np.array([p[0], (p[1]**2 + p[2]**2)**0.5, 0]), np.arctan2(p[2], p[1])

Intersection of Two Lines

This is a 2D-only problem. We have two lines, each is given by two points. The objective is to calculate the intersection between these two lines.

This code is just a transcription of dictation of a recipe from Wikipedia.

def xy_line_intersection(p_1, p_2, p_3, p_4):
    """ p_1 and p_2 define the first line, p_3 and p_4 define the second; 
        return a point of intersection between these two lines in x-y plane

        Kudos: https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
    """
    # only take x and y coordinates
    x1 = p_1[0]
    y1 = p_1[1]

    x2 = p_2[0]
    y2 = p_2[1]

    x3 = p_3[0]
    y3 = p_3[1]

    x4 = p_4[0]
    y4 = p_4[1]

    def det(p1, p2, p3, p4):
        return np.linalg.det(np.array([[p1, p2], [p3, p4]]))

    Dx1 = det(x1, y1, x2, y2)
    Dx2 = det(x1,  1, x2,  1)
    Dx3 = det(x3, y3, x4, y4)
    Dx4 = det(x3,  1, x4,  1)
    Dx5 = Dx2
    Dx6 = det(y1,  1, y2,  1)
    Dx7 = Dx4
    Dx8 = det(y3,  1, y4,  1)

    # x-coordinate
    Px = det(Dx1, Dx2, Dx3, Dx4)/det(Dx5, Dx6, Dx7, Dx8)

    # y-coordinate
    Dy1 = Dx1
    Dy2 = Dx6
    Dy3 = Dx3
    Dy4 = Dx8
    Dy5 = Dx2
    Dy6 = Dx6
    Dy7 = Dx7
    Dy8 = Dx8

    Py = det(Dy1, Dy2, Dy3, Dy4)/det(Dy5, Dy6, Dy7, Dy8)
    return vector(Px, Py, 0)

This can also be done with vectors. Instead of line a vector between two points is created. Then each is scaled so that the new vectors produce the same point. Here’s some ASCII math for clarification (vectors are in bold):

P1(P1x, P1y), P2(P2x, P2y) - points that define first line
P3(P3x, P3y), P4(P4x, P4y) - points that define the second line
P1 + k1*(P2 - P1) = P3 + k2*(P4 - P3)

Now the above equation  contains vectors so it is in fact two equations, one for x-component and one for y.

P1x + k1*(P2x - P1x) = P3x + k2*(P4x - P3x)
P1y + k1*(P2y - P1y) = P3y + k2*(P4y - P3y)

Above is a linear system of two equations and two unknowns. By solving it we get k1 and k2 and in theory, the intersecting point can be calculated with either the left or the right side of the first equation. In practice (my specific case) the left side works (within 1e-16 of the previous method) but right doesn’t (more like within 1e-5 to 1e-3). I am not sure why but it might be due to positions of the points P3 and P4. Any comment would be most welcome.

def xy_line_intersection(p_1, p_2, p_3, p_4):
    # alternative solution with vectors
    A = np.array([
        [p_2[0] - p_1[0], p_4[0] - p_3[0]],
        [p_2[1] - p_1[1], p_4[1] - p_3[1]],
    ])
    
    b = np.array([p_3[0] - p_1[0], p_3[1] - p_1[1]])
    
    k1k2 = np.linalg.solve(A, b)
    k1 = k1k2[0]
    k2 = k1k2[1]
    
    va = vector(
        p_1[0] + k1*(p_2[0] - p_1[0]),
        p_1[1] + k1*(p_2[1] - p_1[1]),
        0
    )
    
    vb = vector(
        p_3[0] + k2*(p_4[0] - p_3[0]),
        p_3[1] + k2*(p_4[1] - p_3[1]),
        0
    )
    
    # print(P-va, P-vb)
    return va

Extend a Vector to Specified y

In this problem we look for the point where the line given by two points meets given y-value. This is a quick hack and doesn’t work with y=constant lines. But works in all other cases.

def extend_to_y(p_1, p_2, y):
    """ return a point that lies on a line defined by p_1 and p_2 and on y=y """
    fk_3 = lambda k: p_1[1] + k*(p_2 - p_1)[1] - y
    k_3 = sp.optimize.newton(fk_3, 0)

    return p_1 + k_3*(p_2 - p_1)

Check If a Point is on a Line

We have three points, A, B, and C. Points A and B define a line and we would like to check if point C lies on that line within certain tolerances.

This problem was the most Paineful in me Buttocks until I switched to vector notation. The recipe is now quite simple (again, mind the bold vectors):

AB = B - A
AC = C - A
k = |AC| / |AB|
C' = A + k*AB

Now C‘ is a new point that we got to using another route. If it is on the same line it should be the same as point C. Within certain tolerance:

|C - C'| < tol ---> the point is on line AB.

And the code:

def is_between(A, B, C, tol):
    # returns True if point C lies on a line between A and B;

    # take an alternative path to point C:
    AB = B - A
    AC = C - A
    k = np.linalg.norm(AC) / np.linalg.norm(AB)
    C__ = A + k*AB

    # it's the same point if the two are close enough
    return np.linalg.norm(C - C__) < tol

If this is of any use to anyone, I will very pleased and even more surprised. If this is the case, I wish you all the best and good luck with your points and vectors.

Leave a Reply