def Transform_3D_to_2D(polygon1, normal):
# Create V1 and V2 vectors of polygon1 normal and [0,0,1] z-axis
V1 = normal
V2 = [0, 0, 1] # z-axis
V1 = np.array(V1)
V2 = np.array(V2)
# create a rotation axis to rotate V1 to V2
# and compute angle in degrees of rotation, obtain quaternion for this
axis = np.cross(V1, V2)
angle = acos(np.inner(V1, V2) / (np.linalg.norm(V1) * np.linalg.norm(V2)))
degrees = angle * 180 / pi
q = QG.rotation_quaternion(degrees, axis[0], axis[1], axis[2])
# compute rotation matrix R for quaternion q
epsilon = 1e-8
R = q.rotation_matrix()
# Now transform polygon1 to polygon2 so it
# has normal [0,0,1] in z-axis using rotation R
polygon2 = []
pt0 = list(np.einsum('ij,j->i', R, polygon1[0]))
z = pt0[2] # z-component of first point
for pt in polygon1:
pt2 = list(np.einsum('ij,j->i', R, pt))
pt2[2] -= z # translate pt2 by [0,0,-z]
polygon2.append(pt2)
C = np.array([0, 0, 0])
for pt in polygon2:
C = C + np.array(pt)
C = C / (1.0 * len(polygon2))
for i in range(len(polygon2)):
pt = polygon2[i]
pt2 = list(np.array(pt) - C)
polygon2[i] = pt2
return polygon2
def theta2q(theta):
a = np.linalg.norm(theta)
degrees = a * 180 / pi
epsilon = 1e-8
if abs(a) > epsilon:
axis = np.array(theta) / a
else:
axis = np.array([0, 0, 1])
degrees = 0
q = HH.rotation_quaternion(degrees, axis[0], axis[1], axis[2])
return q
def AimAxis(axis1, axis2):
# Create V1 and V2 vectors of polygon1 normal and [0,0,1] z-axis
V1 = axis1
V2 = axis2 # z-axis
V1 = np.array(V1)
V2 = np.array(V2)
# create a rotation axis to rotate V1 to V2
# and compute angle in degrees of rotation, obtain quaternion for this
axis = np.cross(V1, V2)
angle = acos(np.inner(V1, V2) / (np.linalg.norm(V1) * np.linalg.norm(V2)))
degrees = angle * 180 / pi
q = QG.rotation_quaternion(degrees, axis[0], axis[1], axis[2])
return q
def q2R(q):
q = HH.Quaternion(q)
R3 = q.rotation_matrix()
R = np.identity(4)
R[:3, :3] = R3
return R