martedì 6 maggio 2014

Rotazione 3D in Python

Una funzione per calcolare la rotazione 3d di un punto attorno al centro degli assi mediante Python usando gli angoli di Eulero
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