Given two vectors which intersect at a point in 3D space, I would like to determine (and display) the plane which bisects these lines.
So far I have done the following:
0) Plotted the vectors (red)
1) Computed the cross-product (blue) and the dot product angle between the two vectors.
2) Rotated one of the vector by the 1/2 the angle between them, using the cross-product vector as the axis of rotation. The result is normal to the plane (green).
3) Using the normal, solve for 'd' in the ax+by+c*z+d = 0 equation of the plane.
4) Plot the resultant plane.
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import math
def unit_vector(vector):
""" Returns the unit vector of the vector. """
return vector / np.linalg.norm(vector)
def angle_between(v1, v2):
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 rotation_matrix(axis, theta):
axis = np.asarray(axis)
axis = axis / math.sqrt(np.dot(axis, axis))
a = math.cos(theta / 2.0)
b, c, d = -axis * math.sin(theta / 2.0)
aa, bb, cc, dd = a * a, b * b, c * c, d * d
bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
return np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
[2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
[2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]])
#Path of points to plot
pathPoints = np.array ([[ 3, 0, 30],
[13, 7, 25],
[23, 12, 12]])
#Initialize flight path array
flightPath = []
lineSegment = np.array ([0,0,0,0,0,0])
for cntr, point in enumerate (pathPoints):
#skip the last point in the sequence
if pathPoints.shape[0] == cntr + 1: #shape is 1-based
break
print ('Point #%d' % cntr, point)
nextPoint = pathPoints[cntr+1]
print ('Next Point #%d' % cntr, nextPoint)
lineSegment = np.zeros(6)
lineSegment[:3] = point
diff = np.subtract (nextPoint, point)
print ('Diff #%d' % cntr, diff)
lineSegment[3:] = diff
print ('LineSeg #%d' % cntr, lineSegment)
flightPath.append (lineSegment)
print ('Flight Path array ', flightPath)
#Create the plot
plotSize = 15
plt3d = plt.figure(figsize=(plotSize,plotSize)).gca(projection='3d')
#set the plot limits
axisMin=0
axisMax=30
plt3d.set_xlim([axisMin,axisMax])
plt3d.set_ylim([axisMin,axisMax])
plt3d.set_zlim([axisMin,axisMax])
plt3d.set_xlabel('x')
plt3d.set_ylabel('y')
plt3d.set_zlabel('z')
for vector in flightPath:
v = np.array([vector[3],vector[4],vector[5]])
vlength = np.linalg.norm(v)
plt3d.quiver(vector[0],vector[1],vector[2],vector[3],vector[4],vector[5],
pivot='tail', arrow_length_ratio=2.0/vlength,color='red')
#Compute the cross product at the intersection points, and then a plane which
#bisects the intersection point.
flightPlanes = []
flightPlaneOffsets = []
for cntr, currVec in enumerate (flightPath):
#skip the last point in the sequence
if len (flightPath) == cntr + 1: #shape is 1-based
break
print ('Vector #%d' % cntr, currVec)
#Compute the cross product normal
nextVec = flightPath[cntr+1]
print ('Next Vector #%d' % cntr, nextVec)
#Compute the cross product of the differences
crossProd = np.cross (np.negative (currVec[3:]), nextVec[3:])
vlength = np.linalg.norm(crossProd)
print ('Cross Product #%d' % cntr, crossProd)
#Scale the length of the cross product in order to display on the
#plot. Determining the plane doesn't depend on length.
crossProd = np.multiply (crossProd, 15/vlength)
print ('Scaled Cross Product #%d' % cntr, crossProd)
#Recompute the scaled length
vlength = np.linalg.norm(crossProd)
plt3d.quiver(nextVec[0], nextVec[1], nextVec[2],
crossProd[0], crossProd[1], crossProd[2],
pivot='tail', arrow_length_ratio=2.0/vlength,color='blue')
#Generate the bisecting plane
#Compute the angle between the vectors
bisectAngle = angle_between (np.negative (currVec[3:]), nextVec[3:]) / 2
print ('Bisect angle between #%d %.2f deg' % (cntr, np.degrees(bisectAngle)))
#Determining normal vector
#Compute the normal vector to the plane
#See https://stackoverflow.com/questions/6802577/rotation-of-3d-vector
normalVecA = np.dot(rotation_matrix(crossProd, 2*np.pi-bisectAngle), nextVec[:3])
#Scale the length of the normal vector in order to display on the
#plot. Determining the plane doesn't depend on length.
vlength = np.linalg.norm(normalVecA)
plt3d.quiver(nextVec[0], nextVec[1], nextVec[2],
normalVecA[0], normalVecA[1], normalVecA[2],
pivot='tail', arrow_length_ratio=2.0/vlength,color='green')
print ('Normal vector A #%d' % cntr, normalVecA)
#Create a view of one of the normal vectors
normalVec = normalVecA
#Plane point is the intersection point
planePoint = np.array(nextVec[:3])
print ('Plane point #%d' % cntr, planePoint)
# A plane is a*x+b*y+c*z+d=0
# [a,b,c] is the normal. Thus, we have to calculate
# d and we're set
d = -planePoint.dot(normalVec)
print ('D offset is #%d' % cntr, d)
# create x,y
gridSize = 3
ptsSpacing = 10 / abs (normalVec[2])
xRange = np.array(np.arange(nextVec[0]-gridSize,
nextVec[0]+gridSize+1, ptsSpacing))
yRange = np.array(np.arange(nextVec[1]-gridSize,
nextVec[1]+gridSize+1, ptsSpacing))
print ('Xrange #%d' % cntr, xRange)
xx, yy = np.meshgrid(xRange, yRange)
# calculate corresponding z
z = (-normalVec[0] * xx - normalVec[1] * yy - d) * 1. /normalVec[2]
flightPlanes.append(normalVec)
flightPlaneOffsets.append(d)
# plot the surface
plt3d.plot_surface(xx, yy, z, alpha=0.3)
plt.show()
I expect the cross-product vector (blue), which is the axis of rotation, also should be on the bisecting plane (however this is not the case). The following code line is key:
normalVecA = np.dot(rotation_matrix(crossProd, 2*np.pi-bisectAngle), nextVec[:3])
I tried setting the angle to just 'bisectAngle' and '-bisectAngle' and it doesn't appear to fix the issue.
