planes.py

# -*- coding: utf-8 -*-

import numpy as np
import matplotlib.pyplot as plt


C1 = np.array([-3.46, 14.991, 1.969])
C9 = np.array([-3.797, 13.86, 2.93])
C8 = np.array([-2.541, 13.306, 3.48])

C5 = np.array([-0.836, 21.582, 5.383])
C4 = np.array([-0.907, 21.181, 3.896])
C3 = np.array([-2.197, 20.412, 3.585])

p1, p2, p3 = C1, C9, C8
q1, q2, q3 = C5, C4, C3


def calculate_normal_vector(p1, p2, p3):
    """Calculates the normal vector of the plane given by the three points."""
    return np.cross((p1 - p2), (p3 - p2))


def unit_vector(vector):
    """Returns the unit vector of the vector."""
    return vector / np.linalg.norm(vector)


def angle_between(v1, v2):
    """
    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))


def angle_between_planes(p1, p2, p3, q1, q2, q3):
    """
    Returns the angle given by the normal vectors of plane 1 and plane 2.

    Plane 1 is defined by containing points p1, p2 and p3.
    Plane 2 is defined by containing points q1, q2 and q3.
    """
    n1 = calculate_normal_vector(p1, p2, p3)
    n2 = calculate_normal_vector(q1, q2, q3)
    angle = angle_between(n1, n2)
    print(angle, np.degrees(angle))
    return angle


def plot_planes_from_points(p1, p2, p3, q1, q2, q3):
    """"""
    n1 = calculate_normal_vector(p1, p2, p3)
    d1 = -p1.dot(n1)
    n2 = calculate_normal_vector(q1, q2, q3)
    d2 = -q1.dot(n2)

    minx = min(p1[0], p2[0], p3[0], q1[0], q2[0], q3[0])
    maxx = max(p1[0], p2[0], p3[0], q1[0], q2[0], q3[0])
    miny = min(p1[1], p2[1], p3[1], q1[1], q2[1], q3[1])
    maxy = max(p1[1], p2[1], p3[1], q1[1], q2[1], q3[1])

#    mean_px = np.mean([p1[0], p2[0], p3[0]])
#    mean_py = np.mean([p1[1], p2[1], p3[1]])
#    mean_pz = np.mean([p1[2], p2[2], p3[2]])
#
#    mean_qx = np.mean([q1[0], q2[0], q3[0]])
#    mean_qy = np.mean([q1[1], q2[1], q3[1]])
#    mean_qz = np.mean([q1[2], q2[2], q3[2]])

    x = np.arange(minx, maxx, 0.05)
    y = np.arange(miny, maxy, 0.05)
    xx, yy = np.meshgrid(x, y)

    zz1 = ((-n1[0] * xx) - (n1[1] * yy) - d1) * 1.0 / n1[2]
    zz2 = ((-n2[0] * xx) - (n2[1] * yy) - d2) * 1.0 / n2[2]

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.plot_surface(xx, yy, zz1, alpha=0.2)
    ax.plot_surface(xx, yy, zz2, alpha=0.2)

    # ax.quiver(mean_px, mean_py, mean_pz, n1[0], n1[1], n1[2], length=5)
    # ax.quiver(mean_qx, mean_qy, mean_qz, n2[0], n2[1], n2[2], length=5)

    ax.scatter(p1[0], p1[1], p1[2],  color='green')
    ax.text(p1[0], p1[1], p1[2], 'C1')
    ax.scatter(p2[0], p2[1], p2[2],  color='green')
    ax.text(p2[0], p2[1], p2[2], 'C9')
    ax.scatter(p3[0], p3[1], p3[2],  color='green')
    ax.text(p3[0], p3[1], p3[2], 'C8')

    ax.scatter(q1[0], q1[1], q1[2],  color='red')
    ax.text(q1[0], q1[1], q1[2], 'C5')
    ax.scatter(q2[0], q2[1], q2[2],  color='red')
    ax.text(q2[0], q2[1], q2[2], 'C4')
    ax.scatter(q3[0], q3[1], q3[2],  color='red')
    ax.text(q3[0], q3[1], q3[2], 'C3')

    ax.set_aspect('equal')
    ax.grid(False)
    plt.axis('off')
    plt.show()


angle_between_planes(p1, p2, p3, q1, q2, q3)
plot_planes_from_points(p1, p2, p3, q1, q2, q3)