Commit 7e1b4ef0 authored by cmaffeo2's avatar cmaffeo2
Browse files

Normalized quaternions in quaternion_to_matrix

parent 2175845a
...@@ -102,6 +102,7 @@ def quaternion_to_matrix(q): ...@@ -102,6 +102,7 @@ def quaternion_to_matrix(q):
# [ 2*(q1*q2 + q3*q4), 1-2*(q1*q1 + q3*q3), 2*(q2*q3 - q1*q4)], # [ 2*(q1*q2 + q3*q4), 1-2*(q1*q1 + q3*q3), 2*(q2*q3 - q1*q4)],
# [ 2*(q1*q3 - q2*q4), 2*(q1*q4 + q2*q3), 1-2*(q2*q2 + q1*q1)]] # [ 2*(q1*q3 - q2*q4), 2*(q1*q4 + q2*q3), 1-2*(q2*q2 + q1*q1)]]
q = q / np.linalg.norm(q)
q0,q1,q2,q3 = q q0,q1,q2,q3 = q
R = [[1-2*(q2*q2 + q3*q3), 2*(q1*q2 - q3*q0), 2*(q1*q3 + q2*q0)], R = [[1-2*(q2*q2 + q3*q3), 2*(q1*q2 - q3*q0), 2*(q1*q3 + q2*q0)],
[ 2*(q1*q2 + q3*q0), 1-2*(q1*q1 + q3*q3), 2*(q2*q3 - q1*q0)], [ 2*(q1*q2 + q3*q0), 1-2*(q1*q1 + q3*q3), 2*(q2*q3 - q1*q0)],
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment