myprojects-hg-reborn

Fork

#! /usr/bin/env python




import numpy as n
import scipy as s
import scipy.linalg as slin



#I define some global variables (sequances, tuples, etc..) I will need for 


_EPS = n.finfo(float).eps * 4.0

# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]     #BACKUP!

# _NEXT_AXIS = [2, 1, 0, 2]


_AXES2TUPLE = { # axes string -> (inner axis, parity, repetition, frame)
    "sxyz": (0, 0, 0, 0), "sxyx": (0, 0, 1, 0), "sxzy": (0, 1, 0, 0),
    "sxzx": (0, 1, 1, 0), "syzx": (1, 0, 0, 0), "syzy": (1, 0, 1, 0),
    "syxz": (1, 1, 0, 0), "syxy": (1, 1, 1, 0), "szxy": (2, 0, 0, 0),
    "szxz": (2, 0, 1, 0), "szyx": (2, 1, 0, 0), "szyz": (2, 1, 1, 0),
    "rzyx": (0, 0, 0, 1), "rxyx": (0, 0, 1, 1), "ryzx": (0, 1, 0, 1),
    "rxzx": (0, 1, 1, 1), "rxzy": (1, 0, 0, 1), "ryzy": (1, 0, 1, 1),
    "rzxy": (1, 1, 0, 1), "ryxy": (1, 1, 1, 1), "ryxz": (2, 0, 0, 1),
    "rzxz": (2, 0, 1, 1), "rxyz": (2, 1, 0, 1), "rzyz": (2, 1, 1, 1)}
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())



def euler_from_rotation_matrix(matrix, axes='sxyz'):
    """Return Euler angles from rotation matrix for specified axis sequence.

    matrix -- 3x3 or 4x4 rotation matrix
    axes -- One of 24 axis sequences as string or encoded tuple

    Note that many Euler angle triplets can describe one matrix.

    """
    try:
        firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
    except (AttributeError, KeyError):
        _TUPLE2AXES[axes]
        firstaxis, parity, repetition, frame = axes

    i = firstaxis
    j = _NEXT_AXIS[i+parity]
    k = _NEXT_AXIS[i-parity+1]

    M = n.array(matrix, dtype=n.float64)[0:3, 0:3]
    if repetition:
        sy = s.sqrt(M[i,j]*M[i,j] + M[i,k]*M[i,k])
        if sy > _EPS:
            ax = s.arctan2( M[i,j],  M[i,k])
            ay = s.arctan2( sy,      M[i,i])
            az = s.arctan2( M[j,i], -M[k,i])
        else:
            ax = s.arctan2(-M[j,k],  M[j,j])
            ay = s.arctan2( sy,      M[i,i])
            az = 0.0
    else:
        cy = s.sqrt(M[i,i]*M[i,i] + M[j,i]*M[j,i])
        if cy > _EPS:
            ax = s.arctan2( M[k,j],  M[k,k])
            ay = s.arctan2(-M[k,i],  cy)
            az = s.arctan2( M[j,i],  M[i,i])
        else:
            ax = s.arctan2(-M[j,k],  M[j,j])
            ay = s.arctan2(-M[k,i],  cy)
            az = 0.0

    if parity: ax, ay, az = -ax, -ay, -az
    if frame: ax, az = az, ax
    return ax, ay, az



def normalize(vec):
    norm_vec=vec/s.sqrt(s.dot(vec,vec.conj()))
    return norm_vec

def angles_3D_normalized(vec1,vec2):
    #calculates the angle between two NORMALIZED vectors in 3D

    alpha=s.arccos(s.dot(vec1,vec2))

    return alpha
    



#Now I generate TWO set of TWO vectors I will use for the rotation matrix

vec_ini_1=s.array([1.,0.,1.])

vec_ini_2=s.array([1.,0.,0.])

vec_ini_3=s.array([0.,1.,0.])




vec_fin_1=s.array([0.8536,-0.5,-0.1464])
vec_fin_2=s.array([0.5,-0.7071,0.5])
vec_fin_3=s.array([-0.5,-0.701,-0.5])


#Then I want to normalize them



vec_ini_1=normalize(vec_ini_1)
vec_ini_2=normalize(vec_ini_2)
vec_ini_3=normalize(vec_ini_3)




vec_fin_1=normalize(vec_fin_1)
vec_fin_2=normalize(vec_fin_2)
vec_fin_3=normalize(vec_fin_3)



def calc_euler_matrix(vec_ini_1,vec_ini_2,vec_ini_3,vec_fin_1,vec_fin_2,vec_fin_3):
    #First normalize the vectors
    vec_ini_1=normalize(vec_ini_1)
    vec_ini_2=normalize(vec_ini_2)
    vec_ini_3=normalize(vec_ini_3)

    vec_fin_1=normalize(vec_fin_1)
    vec_fin_2=normalize(vec_fin_2)
    vec_fin_3=normalize(vec_fin_3)
    X=s.zeros((3,3))

    # xtest=s.zeros((3,3))


    X[:,0]=vec_ini_1
    X[:,1]=vec_ini_2
    X[:,2]=vec_ini_3



    X_prime=s.zeros((3,3))


    X_prime[:,0]=vec_fin_1
    X_prime[:,1]=vec_fin_2
    X_prime[:,2]=vec_fin_3

    #I call B=x'x^T

    B=s.dot(X_prime,s.transpose(X))

    #print "B is", B

    #I call C=xx^T

    C=s.dot(X,s.transpose(X))

    #print "C is, ", C

    #print "the determinant of C is, ", slin.det(C)

    #Now I invert C and cal D=C^{-1}

    D=slin.inv(C)

    #print "D is, ", D


    #Now I can calculate A

    A=s.dot(B,D)

    return A











    


print "vec_ini_1(2,3) and vec_fin_1(2,3) are, ", vec_ini_1,vec_ini_2,vec_ini_3, vec_fin_1,vec_fin_2,vec_fin_3

alpha_ini_12=angles_3D_normalized(vec_ini_1,vec_ini_2)

alpha_ini_13=angles_3D_normalized(vec_ini_1,vec_ini_3)
alpha_ini_23=angles_3D_normalized(vec_ini_2,vec_ini_3)



alpha_fin_12=angles_3D_normalized(vec_fin_1,vec_fin_2)
alpha_fin_13=angles_3D_normalized(vec_fin_1,vec_fin_3)
alpha_fin_23=angles_3D_normalized(vec_fin_2,vec_fin_3)

print "alpha_ini_12(13,23) is, ", alpha_ini_12 ,alpha_ini_13,alpha_ini_23 

print "alpha_fin_12(13,23) is, ", alpha_fin_12 ,alpha_fin_13,alpha_fin_23 






A=calc_euler_matrix(vec_ini_1,vec_ini_2,vec_ini_3,vec_fin_1,vec_fin_2,vec_fin_3)

print "the rotation matrix is, ", A

my_angles=euler_from_rotation_matrix(A, ('szyx'))

print "my_angles are, ", my_angles



print "So far so good"