Commit 2156db54 authored by Kai's avatar Kai

added rot matrix to euler conversion

parent d7d9f429
# coding: utf-8
import sympy
from sympy import *
phi, theta, psi = symbols("phi theta psi")
Matrix([phi,theta,psi],[psi,phi,theta])
import numpy as np
theta
np.array([theta])
a=np.array([theta])
a@a
a*a
e = cos(theta)
e.subs(theta,phi)
Rm = np.array(shape=(3,3))
Rm = np.array(shape=(3,3),dtype=object)
Rm = np.ndarray(shape=(3,3),dtype=object)
Rm
x, y, z = symbols("x y z")
alpha = symbol("alpha")
alpha = symbols("alpha")
alpha
C = 1-cos(alpha)
c = cos(alpha)
s = sin(alpha)
Rm[0,0]
Rm[0,0] = x*x*C + c
Rm[0,1] = x*y*C - z*s
Rm[0,2] = x*z*C + y*s
Rm[1,0] = y*x*C + z*s
Rm[1,1] = y*y*C + c
Rm[1,2] = y*z*C - x*s
Rm[2,0] = z*y*C + x*s
Rm[2,0] = z*x*C - y*s
Rm[2,1] = z*y*C + x*s
Rm[2,2] = z*z*C + c
Rm
one = copy(Rm)
one = np.copy(Rm)
one
for e in one:
e.subs({x:0,y:0,z:1,alpha:theta})
for e in np.nditer(one):
e.subs({x:0,y:0,z:1,alpha:theta})
for i in range(3)Ö
for i in range(3):
for j in range(3);
for i in range(3):
for j in range(3):
one[i,j].subs({x:0,y:0,z:1,alpha:theta})
one
for i in range(3):
for j in range(3):
one[i,j] = one[i,j].subs({x:0,y:0,z:1,alpha:theta})
one
two = np.copy(Rm)
for i in range(3):
for j in range(3):
two[i,j] = two[i,j].subs({x:-sin(phi),y:cos(phi),z:0,alpha:theta})
one = np.copy(Rm)
for i in range(3):
for j in range(3):
one[i,j] = one[i,j].subs({x:0,y:0,z:1,alpha:phi})
one
two
three = np.copy(Rm)
for i in range(3):
for j in range(3):
three[i,j] = three[i,j].subs({x:cos(phi)*sin(theta),y:sin(phi)*sin(theta),z:cos(theta),alpha:psi})
three@two@one
R=three@two@one
R
x0 = np.ndarray(shape=(1,3))
x0
x0[0] = 1
x0[1] = 0
x0[0,0] = 1
x0[0,1] = 1
x0[0,2] = 1
x0[0,1] = 0
x0[0,2] = 0
x0
R@x0
x0 = np.ndarray(shape=(3,1))
x0[0,0] = 1
x0[1,0] = 0
x0[2,0] = 0
R@x0
x0[0,0]
x1 = R@x0
x1[0,0]
x1[0,0].simplify()
x1
x1[1,0]
x1[1,0].simplify()
x2 = np.ndarray(shape=(3,1))
for i in range(3):
x2[i,0] = x1[i,0].simplify()
x2 = np.ndarray(shape=(3,1),dtype=object)
for i in range(3):
x2[i,0] = x1[i,0].simplify()
x2
y0 = np.ndarray(shape=(3,1))
y0[0,0] = 0
y0[1,0] = 1
y0[2,0] = 0
y1 = R@y0
y2 = np.ndarray(shape=(3,1),dtype=object)
for i in range(3):
y2[i,0] = y1[i,0].simplify()
y2
z0 = np.ndarray(shape=(3,1))
z0[0,1]
z0[0,0] = 0
z0[0,1] = 0
z0[1,0] = 0
z0[2,0] = 0
z1 = R@z0
z2 = np.ndarray(shape=(3,1),dtype=object)
for i in range(3):
z2[i,0] = z1[i,0].simplify()
z2
z0[2,0] = 1
z1 = R@z0
for i in range(3):
z2[i,0] = z1[i,0].simplify()
z2
get_ipython().run_line_magic('save', 'eulers_from_rot.py')
get_ipython().run_line_magic('save', 'eulers_from_rot.py ~0/')
......@@ -111,43 +111,36 @@ i2 = I2(N)
a1 = A1(D,i1)
a2 = A2(D,i2)
print(i1)
print(i2)
print(len(a1),len(a2),len(D))
print("===A1===")
for k,v in a1.items():
print(k, v)
print("===A2===")
for k,v in a2.items():
print(k, v)
print("================")
for k,v in D.items():
print(k, v)
# print(i1)
# print(i2)
# print(len(a1),len(a2),len(D))
# print("===A1===")
# for k,v in a1.items():
# print(k, v)
# print("===A2===")
# for k,v in a2.items():
# print(k, v)
# print("================")
# for k,v in D.items():
# print(k, v)
from scipy.sparse import csc_matrix
from scipy.sparse.linalg import lsqr
import numpy as np
A = np.zeros((len(a1)+len(a2),len(eulers)))
for m in range(len(eulers)):
for a,(k,v) in enumerate(a1.items()):
A[a,m] = v.evalf()
for a,(k,v) in enumerate(a2.items()):
A[len(a1)+a,m] = v.evalf()
A = np.zeros((len(i1)+len(i2),len(eulers)))
for a,(k,v) in enumerate(a1.items()):
#k is (i,l,m,n)
A[a%len(i1),k[0]] = v
for a,(k,v) in enumerate(a2.items()):
#k is (i,l,m,n)
A[len(i1) + a%len(i2),k[0]] = v
b = np.zeros((A.shape[0],))
b[0] = 1
print(A)
# A[0,0] = 1
# A[0,1] = 1
# A[2,1] = 0.5
# A[3,0] = 1
# A[5,1] = 0.5
# A[7,1] = 1/np.sqrt(2)
# A[8,1] = -1/np.sqrt(2)
# b = np.zeros((A.shape[0],))
# b[0] = 1
# print(A)
# print(b)
# A = csc_matrix(A)
# x, istop, itn, normr = lsqr(A, b)[:4]
# print(x,normr,istop)
\ No newline at end of file
print(b)
A = csc_matrix(A)
x, istop, itn, normr = lsqr(A, b)[:4]
print(x,normr,istop)
\ No newline at end of file
Markdown is supported
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