da Wikipedia |
--------------------------------------------------------------
import math
import numpy
# yaw = alfa
# pitch = beta
# roll = gamma
def ruota3D(alfa,beta,gamma,x,y,z):
alfa = math.radians(alfa)
beta = math.radians(beta)
gamma = math.radians(gamma)
ca = math.cos(alfa)
sa = math.sin(alfa)
cb = math.cos(beta)
sb = math.sin(beta)
cg = math.cos(gamma)
sg = math.sin(gamma)
ma = numpy.empty((3,3))
ma [0][0] = ca*cb
ma [0][1] = ((ca*sb*sg)-(sa*cg))
ma [0][2] = ((ca*sb*cg)+(sa*sg))
ma [1][0] = sa*cb
ma [1][1] = ((sa*sb*sg)+ (ca*cg))
ma [1][2] = ((sa*sb*cg)-(ca*sg))
ma [2][0] = -sb
ma [2][1] = cb*sg
ma [2][2] = cb*cg
nx1 = ma[0][0] * x
nx2 = ma[0][1] * y
nx3 = ma[0][2] * z
nx = nx1+nx2+nx3
ny1 = ma[1][0] * x
ny2 = ma[1][1] * y
ny3 = ma[1][2] * z
ny = ny1+ny2+ny3
nz1 = ma[2][0] * x
nz2 = ma[2][1] * y
nz3 = ma[2][2] * z
nz = nz1+nz2+nz3
return [nx,ny,nz]
xx = ruota3D(0,0,90,1,1,1)
print xx
Nessun commento:
Posta un commento