PY
py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
import math as m
import numpy as np
import matplotlib.pyplot as plt
class Quaternions:
def __init__(self,w=1,x=0,y=0,z=0):
self.x=x
self.y=y
self.z=z
self.w=w
self.lista=[w,x,y,z]
def q_conjugate(self):
return Quaternions(self.w, -self.x, -self.y, -self.z)
def q_mult(q1, q2):
w1,x1,y1,z1=q1.lista
w2,x2,y2,z2=q2.lista
w=w1*w2-x1*x2-y1*y2-z1*z2
x=w1*x2+x1*w2+y1*z2-z1*y2
y=w1*y2+y1*w2+z1*x2-x1*z2
z=w1*z2+z1*w2+x1*y2-y1*x2
return Quaternions(w,x,y,z)
def Euler2Q(phi,theta,psi):
qw=m.cos(phi/2)*m.cos(theta/2)*m.cos(psi/2)+m.sin(phi/2)*m.sin(theta/2)*m.sin(psi/2)
qx=m.sin(phi/2)*m.cos(theta/2)*m.cos(psi/2)-m.cos(phi/2)*m.sin(theta/2)*m.sin(psi/2)
qy=m.cos(phi/2)*m.sin(theta/2)*m.cos(psi/2)+m.sin(phi/2)*m.cos(theta/2)*m.sin(psi/2)
qz=m.cos(phi/2)*m.cos(theta/2)*m.sin(psi/2)-m.sin(phi/2)*m.sin(theta/2)*m.cos(psi/2)
return Quaternions(qw,qx,qy,qz)
def qv_mult(q1,v1):
q2=Quaternions(0,v1[0],v1[1],v1[2])
Enter to Rename, Shift+Enter to Preview
OUTPUT
Run