<s> '''<STR_LIT>''' <EOL> import math <EOL> from . externals . six . moves import reduce <EOL> import numpy as np <EOL> _FLOAT_EPS_4 = np . finfo ( float ) . eps * <NUM_LIT> <EOL> def euler2mat ( z = <NUM_LIT:0> , y = <NUM_LIT:0> , x = <NUM_LIT:0> ) : <EOL> '''<STR_LIT>''' <EOL> Ms = [ ] <EOL> if z : <EOL> cosz = math . cos ( z ) <EOL> sinz = math . sin ( z ) <EOL> Ms . append ( np . array ( [ [ cosz , - sinz , <NUM_LIT:0> ] , <EOL> [ sinz , cosz , <NUM_LIT:0> ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ] ) ) <EOL> if y : <EOL> cosy = math . cos ( y ) <EOL> siny = math . sin ( y ) <EOL> Ms . append ( np . array ( [ [ cosy , <NUM_LIT:0> , siny ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , <EOL> [ - siny , <NUM_LIT:0> , cosy ] ] ) ) <EOL> if x : <EOL> cosx = math . cos ( x ) <EOL> sinx = math . sin ( x ) <EOL> Ms . append ( np . array ( [ [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , <EOL> [ <NUM_LIT:0> , cosx , - sinx ] , <EOL> [ <NUM_LIT:0> , sinx , cosx ] ] ) ) <EOL> if Ms : <EOL> return reduce ( np . dot , Ms [ : : - <NUM_LIT:1> ] ) <EOL> return np . eye ( <NUM_LIT:3> ) <EOL> def mat2euler ( M , cy_thresh = None ) : <EOL> '''<STR_LIT>''' <EOL> M = np . asarray ( M ) <EOL> if cy_thresh is None : <EOL> try : <EOL> cy_thresh = np . finfo ( M . dtype ) . eps * <NUM_LIT:4> <EOL> except ValueError : <EOL> cy_thresh = _FLOAT_EPS_4 <EOL> r11 , r12 , r13 , r21 , r22 , r23 , r31 , r32 , r33 = M . flat <EOL> cy = math . sqrt ( r33 * r33 + r23 * r23 ) <EOL> if cy > cy_thresh : <EOL> z = math . atan2 ( - r12 , r11 ) <EOL> y = math . atan2 ( r13 , cy ) <EOL> x = math . atan2 ( - r23 , r33 ) <EOL> else : <EOL> z = math . atan2 ( r21 , r22 ) <EOL> y = math . atan2 ( r13 , cy ) <EOL> x = <NUM_LIT:0.0> <EOL> return z , y , x <EOL> def euler2quat ( z = <NUM_LIT:0> , y = <NUM_LIT:0> , x = <NUM_LIT:0> ) : <EOL> '''<STR_LIT>''' <EOL> z = z / <NUM_LIT> <EOL> y = y / <NUM_LIT> <EOL> x = x / <NUM_LIT> <EOL> cz = math . cos ( z ) <EOL> sz = math . sin ( z ) <EOL> cy = math . cos ( y ) <EOL> sy = math . sin ( y ) <EOL> cx = math . cos ( x ) <EOL> sx = math . sin ( x ) <EOL> return np . array ( [ cx * cy * cz - sx * sy * sz , <EOL> cx * sy * sz + cy * cz * sx , <EOL> cx * cz * sy - sx * cy * sz , <EOL> cx * cy * sz + sx * cz * sy ] ) <EOL> def quat2euler ( q ) : <EOL> '''<STR_LIT>''' <EOL> from . import quaternions as nq <EOL> return mat2euler ( nq . quat2mat ( q ) ) <EOL> def euler2angle_axis ( z = <NUM_LIT:0> , y = <NUM_LIT:0> , x = <NUM_LIT:0> ) : <EOL> '''<STR_LIT>''' <EOL> from . import quaternions as nq <EOL> return nq . quat2angle_axis ( euler2quat ( z , y , x ) ) <EOL> def angle_axis2euler ( theta , vector , is_normalized = False ) : <EOL> '''<STR_LIT>''' <EOL> from . import quaternions as nq <EOL> M = nq . angle_axis2mat ( theta , vector , is_normalized ) <EOL> return mat2euler ( M ) </s>
