def quaternion_between_vectors(v1: np.ndarray, v2: np.ndarray) -> quaternion:
"""
Construct rotation quaternion to rotate direction given by v1 towards v2
:param v1: "source" direction
:param v2: "destination" direction
:return: Quaternion of rotation, such that ret.rotate(v1) == v2
"""
v1 = vector_normalize(v1)
v2 = vector_normalize(v2)
if np.allclose(v1, - v2):
return quaternion_from_scalar_and_vector(scalar=0, vector=vector_normalize(orthogonal(v1)))
half = vector_normalize(v1 + v2)
# noinspection PyTypeChecker
return quaternion_from_scalar_and_vector(scalar=np.dot(v1, half), vector=np.cross(v1, half))
def look_at(eyePosition3D, center3D, upVector3D):
"""
Reimplementation of lookat from opengl. Very likely broken very much.
"""
matrix = np.zeros(16)
forward = vector_normalize(center3D - eyePosition3D)
# Side = forward x up
side = np.zeros(3)
side[0] = (forward[1] * upVector3D[2]) - (forward[2] * upVector3D[1])
side[1] = (forward[2] * upVector3D[0]) - (forward[0] * upVector3D[2])
side[2] = (forward[0] * upVector3D[1]) - (forward[1] * upVector3D[0])
side = vector_normalize(side)
# Recompute up as: up = side x forward
up = np.zeros(3)
up[0] = (side[1] * forward[2]) - (side[2] * forward[1])
up[1] = (side[2] * forward[0]) - (side[0] * forward[2])
up[2] = (side[0] * forward[1]) - (side[1] * forward[0])
#print(side, up, forward)
matrix[0] = side[0]
matrix[4] = side[1]
matrix[8] = side[2]
# matrix[12] = 0.0
# --------------------
matrix[1] = up[0]
matrix[5] = up[1]
matrix[9] = up[2]
# matrix[13] = 0.0
# --------------------
matrix[2] = -forward[0]
matrix[6] = -forward[1]
matrix[10] = -forward[2]
# matrix[14] = 0.0
# --------------------
# matrix[3] = matrix[7] = matrix[11] = 0.0
matrix[15] = 1.0
# --------------------
res = np.reshape(matrix, (4, 4))
tm = translation_matrix(-eyePosition3D)
return res @ tm
def quaternion_slerp_old(quat0: np.ndarray, quat1: np.ndarray, fraction, spin=0, shortestpath=True):
"""Return spherical linear interpolation between two quaternions.
>>> _q0 = random_quaternion()
>>> _q1 = random_quaternion()
>>> q = quaternion_slerp(_q0, _q1, 0)
>>> np.allclose(q, _q0)
True
>>> q = quaternion_slerp(_q0, _q1, 1, 1)
>>> np.allclose(q, _q1)
True
>>> q = quaternion_slerp(_q0, _q1, 0.5)
>>> ang = acos(np.dot(_q0, q))
>>> np.allclose(2, acos(np.dot(_q0, _q1)) / ang) or np.allclose(2, acos(-np.dot(_q0, _q1)) / ang)
True
"""
q0 = vector_normalize(quat0)
q1 = vector_normalize(quat1)
if fraction == 0.0:
return q0
elif fraction == 1.0:
return q1
d = np.dot(q0, q1)
if abs(abs(d) - 1.0) < EPS:
return q0
if shortestpath and d < 0.0:
# invert rotation
d = -d
np.negative(q1, q1)
angle = acos(d) + spin * pi
if abs(angle) < EPS:
return q0
isin = 1.0 / sin(angle)
q0 *= sin((1.0 - fraction) * angle) * isin
q1 *= sin(fraction * angle) * isin
q0 += q1
return q0