For posterity, here is the solution I came up with:
WE:
"""
MWE of the subspace plotting method
"""
# Import packages
import numpy as np
import vg # For signed angles on the plane
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.art3d import Poly3DCollection # For plotting polygons in 3d
# ------------ METHODS ------------
def get_plot_subspace(V, x_bounds, ax_exist = None, color = 'blue', alpha = 0.5):
"""
V ∈ ℝ^{n_x × dim_v} is a basis for the subspace
# FIXME: I think not specifying x_bounds leads to trouble.
# TODO: Currently, we are assuming the vectors in V are linearly independent: we need to rigorously check this when the argument are first passed.
"""
#: Retrieve the dimensionality
if V.size != 0:
n_x, dim_v = V.shape[0], V.shape[1]
else: # Empty basis
#: Retrieve the projection
if ax_exist is not None:
#: Retrieve the projection
if ax_exist.name == '3d':
n_x, dim_v = 3, 0
else:
n_x, dim_v = 2, 0
else: # NOTE: This assumes as standard that the plotting is in 3d
n_x, dim_v = 3, 0
#: Add to existing axes (if they exist)
if ax_exist is not None:
#: Set equal
fig, ax = None, ax_exist
else:
if n_x == 3:
#: Set the projection to 3d
fig = plt.figure()
ax = fig.add_subplot(projection='3d')
else:
#: Create a figure
fig, ax = plt.subplots()
#: Get the bounds
if x_bounds is None:
if n_x == 2:
x_bounds = ((ax.get_xlim()), (ax.get_ylim()))
else:
x_bounds = ((ax.get_xlim()), (ax.get_ylim()), (ax.get_zlim()))
#: Check the dimensionality of the subspace V
match dim_v:
case 0: # Origin
if n_x == 2: # Planar
raise NotImplementedError(f"Planar plotting is not yet implemented")
else: # 3-dimensional
ax.plot(0, 0, 0, '.', color=color)
case 1: # Line
if n_x == 2: # Planar
raise NotImplementedError(f"Planar plotting is not yet implemented")
else: # 3-dimensional
ax = get_plot_line_3d(ax, V[:, 0], np.array([0, 0, 0]), x_bounds, color, alpha)
case 2: # Plane
if n_x == 2: # Planar
raise NotImplementedError(f"Planar plotting is not yet implemented")
else: # 3-dimensional
ax = get_plot_plane_3d(ax, V[:, 0], V[:, 1], np.array([0, 0, 0]), x_bounds, color, alpha)
case 3: # Box
if n_x == 2: # Planar
raise NotImplementedError(f"Planar plotting is not yet implemented")
else: # 3-dimensional
ax = get_plot_box_3d(ax, x_bounds, color, alpha)
#: Set the axis labels
ax.set_xlabel(r'$x_{1}$')
ax.set_ylabel(r'$x_{2}$', rotation='horizontal')
if n_x == 3:
ax.set_zlabel(r'$x_{3}$')
# TODO: Make it such that the <label> parameter shows up with the correct icon
#: Create the bounds
if x_bounds is not None:
ax.set_xlim(x_bounds[0][0], x_bounds[0][1])
ax.set_ylim(x_bounds[1][0], x_bounds[1][1])
if n_x == 3:
ax.set_zlim(x_bounds[2][0], x_bounds[2][1])
#: Return the axis
if ax_exist is not None:
return ax
else:
return fig, ax
def get_plot_plane_3d(ax, v_1, v_2, v_offset, x_bounds, color = 'blue', alpha = 0.5):
# TODO: Merge this with the line plot
#: Get the list of intersection points
intersection_list = get_intersection_bounding_box_3d(v_1, v_2, v_offset, x_bounds)
#: Compute the normal vector to the plane
normal = np.cross(v_1, v_2)
#: Sort the points
intersection_list = get_sorted_list_of_vertices(intersection_list, normal)
#: Compute the hexadecimal value to rgb
if isinstance(color, str) and color[0] == '#':
#: Convert to rgb
color_rgba = [int(color[i:(i + 2)], 16) / 255. for i in (1, 3, 5)]
color_rgba.append(alpha)
else:
color_rgba = [0, 0, 0.8, alpha]
#: Add the polygon
ax.add_collection3d(Poly3DCollection([intersection_list], color=[color_rgba for _ in range(len(intersection_list))]))
#: Return the results
return ax
def get_plot_line_3d(ax, v, v_offset, x_bounds, color = 'blue', alpha = 0.5):
# TODO: Merge this with the line plot
#: Get the list of intersection points
intersection_list = get_intersection_bounding_box_3d(v, None, v_offset, x_bounds)
#: Create the color
if isinstance(color, str) and color[0] == '#':
#: Convert to rgb
color_rgba = [int(color[i:(i + 2)], 16) / 255. for i in (1, 3, 5)]
color_rgba.append(alpha)
else:
color_rgba = [0, 0, 0.8, alpha]
#: Add the polygon
ax.add_collection3d(Poly3DCollection([intersection_list], color=[color_rgba for _ in range(len(intersection_list))]))
#: Return the results
return ax
def get_plot_box_3d(ax, bounds_box, color = 'blue', alpha = 0.5):
#: Set the dimension of the state-space
n_x = 3
#: Create a list
list_planes = ['xmin', 'xmax', 'ymin', 'ymax', 'zmin', 'zmax']
#: Create a dictionary
dict_planes = get_dict_planes(bounds_box)
#: Loop over all the planes
for name_plane in list_planes:
#: Extract the planes data
v_1_plane, v_2_plane, offset_plane = dict_planes[name_plane]
#: Plot the plane
ax = get_plot_plane_3d(ax, v_1_plane, v_2_plane, offset_plane, bounds_box, color, alpha)
#: Return the results
return ax
def get_intersection_three_planes(v_1_plane_1, v_2_plane_1, offset_plane_1, v_1_plane_2, v_2_plane_2, offset_plane_2, v_1_plane_3, v_2_plane_3, offset_plane_3):
# FROM: ChatGPT3.5
# TODO: Make an actual plane class for an equivalent representation
#: Calculating normal vectors of the planes
normal_plane_1 = np.cross(v_1_plane_1, v_2_plane_1)
normal_plane_2 = np.cross(v_1_plane_2, v_2_plane_2)
normal_plane_3 = np.cross(v_1_plane_3, v_2_plane_3)
#: Adjusting D constants for the planes based on offsets
D_plane_1 = -np.dot(normal_plane_1, offset_plane_1)
D_plane_2 = -np.dot(normal_plane_2, offset_plane_2)
D_plane_3 = -np.dot(normal_plane_3, offset_plane_3)
#: Constructing coefficient matrix
coeff_matrix = np.array([normal_plane_1, normal_plane_2, normal_plane_3])
#: Constructing constant vector
const_vector = np.array([D_plane_1, D_plane_2, D_plane_3])
#: Solving the system of equations
try:
intersection_point = np.linalg.solve(coeff_matrix, const_vector)
except np.linalg.LinAlgError: # There is no intersection point
intersection_point = None
#: Return the result
return intersection_point
def get_intersection_line_plane(v_1_plane, v_2_plane, offset_plane, line, offset_line):
# FROM: ChatGPT3.5
#: Define the plane by three points
p_1_plane = offset_plane
#: Calculate the normal vector of the plane
normal_plane = np.cross(v_1_plane, v_2_plane)
#: Define the line by a point and its direction
point_line = offset_line
direction_line = line
#: Calculate the parameter t for the intersection point
t = np.dot(normal_plane, p_1_plane - point_line) / np.dot(normal_plane, direction_line)
#: Calculate the intersection point
intersection_point = point_line + t * direction_line
#: Return the results
return intersection_point
def get_intersection_bounding_box_3d(v_1, v_2, v_offset, x_bounds):
#: Set the dimension of the state-space
n_x = 3
#: Create a list
list_planes = ['xmin', 'xmax', 'ymin', 'ymax', 'zmin', 'zmax']
#: Create the inequality plot
F_ineq, b_ineq = np.vstack((np.eye(n_x), -np.eye(n_x))), np.array([x_bounds[0][1], x_bounds[1][1], x_bounds[2][1], -x_bounds[0][0], -x_bounds[1][0], -x_bounds[2][0]])
#: Create a dictionary
dict_planes = get_dict_planes(x_bounds)
#: Calculate the intersection points
if v_2 is not None: # Intersection with a plane
#: List all the pairs
list_pairs = [(list_planes[p1], list_planes[p2]) for p1 in range(len(list_planes)) for p2 in range(p1+1, len(list_planes))]
#: Create a list of intersection points
intersection_list = []
#: Loop over all elements in the list
for pair in list_pairs:
#: Extract the planes data
v_1_plane_1, v_2_plane_1, offset_plane_1 = dict_planes[pair[0]]
v_1_plane_2, v_2_plane_2, offset_plane_2 = dict_planes[pair[1]]
#: Find the intersection
points_intersection = get_intersection_three_planes(v_1_plane_1, v_2_plane_1, offset_plane_1, v_1_plane_2, v_2_plane_2, offset_plane_2, v_1, v_2, v_offset)
#: FIXME: For some reason there is an imaginary part?
points_intersection = points_intersection.real if points_intersection is not None else None
#: Check if the element is within the plotting area
if points_intersection is not None and np.all(F_ineq @ points_intersection <= 1.1 * b_ineq):
#: Add the vertex to the collection
intersection_list.append(points_intersection)
else: # Intersection with a line
#: Create a list of intersection points
intersection_list = []
#: Loop over all elements in the list
for plane in list_planes:
#: Extract the plane data
v_1_plane, v_2_plane, offset_plane = dict_planes[plane]
#: Find the intersection
points_intersection = get_intersection_line_plane(v_1_plane, v_2_plane, offset_plane, v_1, np.zeros(n_x))
#: Check if the element is within the plotting area
if points_intersection is not None and np.all(F_ineq @ points_intersection <= b_ineq):
#: Add the vertex to the collection
intersection_list.append(points_intersection)
#: Return the result
return intersection_list
def get_sorted_list_of_vertices(intersection_list, normal):
#: Loop over all indices
for idx in range(len(intersection_list) - 1):
#: Compute the angle between the current vertex
angle_to_current = np.array([vg.signed_angle(intersection_list[idx], vert, look=normal, units='rad').real for vert in intersection_list])
#: Create the mask
mask = np.zeros(len(intersection_list))
mask[idx] = 1
mask[angle_to_current < 0] = 1
#: Compute the smallest index
smallest_index = np.argmin(np.ma.array(angle_to_current, mask=mask))
#: Reorder the list
intersection_list[idx + 1], intersection_list[smallest_index] = intersection_list[smallest_index], intersection_list[idx + 1]
#: Return the results
return intersection_list
def get_dict_planes(box_bounds):
#: Create the direction vectors
xmin_1, xmin_2 = np.array([0, 1, 0]), np.array([0, 0, 1])
xmax_1, xmax_2 = np.array([0, 1, 0]), np.array([0, 0, 1])
ymin_1, ymin_2 = np.array([1, 0, 0]), np.array([0, 0, 1])
ymax_1, ymax_2 = np.array([1, 0, 0]), np.array([0, 0, 1])
zmin_1, zmin_2 = np.array([1, 0, 0]), np.array([0, 1, 0])
zmax_1, zmax_2 = np.array([1, 0, 0]), np.array([0, 1, 0])
#: Create the offset vectors
xmin_offset = np.array([box_bounds[0][0], 0, 0])
xmax_offset = np.array([box_bounds[0][1], 0, 0])
ymin_offset = np.array([0, box_bounds[1][0], 0])
ymax_offset = np.array([0, box_bounds[1][1], 0])
zmin_offset = np.array([0, 0, box_bounds[2][0]])
zmax_offset = np.array([0, 0, box_bounds[2][1]])
#: Create the dictionary
dict_planes = {'xmin': (xmin_1, xmin_2, xmin_offset), 'xmax': (xmax_1, xmax_2, xmax_offset), 'ymin': (ymin_1, ymin_2, ymin_offset), 'ymax': (ymax_1, ymax_2, ymax_offset), 'zmin': (zmin_1, zmin_2, zmin_offset), 'zmax': (zmax_1, zmax_2, zmax_offset)}
#: return the results
return dict_planes
# ------------ SCRIPT ------------
# Set the axis bounds
bounds_unsafe = ((-5, 5), (-3, 3), (-2, 2))
# Create some sample vectors
v_1 = np.array([1, -1, 0])
v_2 = np.array([1, 1, 1])
v_3 = np.array([3, 3, 0])
# Create for subset
origin = np.array([])
line = (v_1 + v_2)[:, np.newaxis]
plane = np.hstack((v_1[:, np.newaxis], v_2[:, np.newaxis]))
box = np.hstack((v_1[:, np.newaxis], v_2[:, np.newaxis], v_3[:, np.newaxis]))
# Plot the subspaces
_, _ = get_plot_subspace(origin, x_bounds=bounds_unsafe)
_, _ = get_plot_subspace(line, x_bounds=bounds_unsafe)
_, _ = get_plot_subspace(plane, x_bounds=bounds_unsafe)
_, _ = get_plot_subspace(box, x_bounds=bounds_unsafe)
# Show the figures
plt.show()
