<s> """<STR_LIT>""" <EOL> from __future__ import absolute_import , division , print_function <EOL> import numpy as np <EOL> from . import vector , vector3 , vector4 <EOL> from . utils import all_parameters_as_numpy_arrays , parameters_as_numpy_arrays <EOL> class index : <EOL> x = <NUM_LIT:0> <EOL> y = <NUM_LIT:1> <EOL> z = <NUM_LIT:2> <EOL> w = <NUM_LIT:3> <EOL> def create ( x = <NUM_LIT:0.> , y = <NUM_LIT:0.> , z = <NUM_LIT:0.> , w = <NUM_LIT:1.> , dtype = None ) : <EOL> return np . array ( [ x , y , z , w ] , dtype = dtype ) <EOL> def create_from_x_rotation ( theta , dtype = None ) : <EOL> thetaOver2 = theta * <NUM_LIT:0.5> <EOL> return np . array ( <EOL> [ <EOL> np . sin ( thetaOver2 ) , <EOL> <NUM_LIT:0.0> , <EOL> <NUM_LIT:0.0> , <EOL> np . cos ( thetaOver2 ) <EOL> ] , <EOL> dtype = dtype <EOL> ) <EOL> def create_from_y_rotation ( theta , dtype = None ) : <EOL> thetaOver2 = theta * <NUM_LIT:0.5> <EOL> return np . array ( <EOL> [ <EOL> <NUM_LIT:0.0> , <EOL> np . sin ( thetaOver2 ) , <EOL> <NUM_LIT:0.0> , <EOL> np . cos ( thetaOver2 ) <EOL> ] , <EOL> dtype = dtype <EOL> ) <EOL> def create_from_z_rotation ( theta , dtype = None ) : <EOL> thetaOver2 = theta * <NUM_LIT:0.5> <EOL> return np . array ( <EOL> [ <EOL> <NUM_LIT:0.0> , <EOL> <NUM_LIT:0.0> , <EOL> np . sin ( thetaOver2 ) , <EOL> np . cos ( thetaOver2 ) <EOL> ] , <EOL> dtype = dtype <EOL> ) <EOL> @ parameters_as_numpy_arrays ( '<STR_LIT>' ) <EOL> def create_from_axis_rotation ( axis , theta , dtype = None ) : <EOL> dtype = dtype or axis . dtype <EOL> if not np . isclose ( np . linalg . norm ( axis ) , <NUM_LIT:1.> ) : <EOL> axis = vector . normalise ( axis ) <EOL> thetaOver2 = theta * <NUM_LIT:0.5> <EOL> sinThetaOver2 = np . sin ( thetaOver2 ) <EOL> return np . array ( <EOL> [ <EOL> sinThetaOver2 * axis [ <NUM_LIT:0> ] , <EOL> sinThetaOver2 * axis [ <NUM_LIT:1> ] , <EOL> sinThetaOver2 * axis [ <NUM_LIT:2> ] , <EOL> np . cos ( thetaOver2 ) <EOL> ] , <EOL> dtype = dtype <EOL> ) <EOL> @ parameters_as_numpy_arrays ( '<STR_LIT>' ) <EOL> def create_from_matrix ( mat , dtype = None ) : <EOL> dtype = dtype or mat . dtype <EOL> quat = np . array ( <EOL> [ <EOL> np . sqrt ( np . maximum ( <NUM_LIT:0.> , <NUM_LIT:1.> + mat [ <NUM_LIT:0> ] [ <NUM_LIT:0> ] - mat [ <NUM_LIT:1> ] [ <NUM_LIT:1> ] - mat [ <NUM_LIT:2> ] [ <NUM_LIT:2> ] ) ) / <NUM_LIT> , <EOL> np . sqrt ( np . maximum ( <NUM_LIT:0.> , <NUM_LIT:1.> - mat [ <NUM_LIT:0> ] [ <NUM_LIT:0> ] + mat [ <NUM_LIT:1> ] [ <NUM_LIT:1> ] - mat [ <NUM_LIT:2> ] [ <NUM_LIT:2> ] ) ) / <NUM_LIT> , <EOL> np . sqrt ( np . maximum ( <NUM_LIT:0.> , <NUM_LIT:1.> - mat [ <NUM_LIT:0> ] [ <NUM_LIT:0> ] - mat [ <NUM_LIT:1> ] [ <NUM_LIT:1> ] + mat [ <NUM_LIT:2> ] [ <NUM_LIT:2> ] ) ) / <NUM_LIT> , <EOL> np . sqrt ( np . maximum ( <NUM_LIT:0.> , <NUM_LIT:1.> + mat [ <NUM_LIT:0> ] [ <NUM_LIT:0> ] + mat [ <NUM_LIT:1> ] [ <NUM_LIT:1> ] + mat [ <NUM_LIT:2> ] [ <NUM_LIT:2> ] ) ) / <NUM_LIT> , <EOL> ] , <EOL> dtype = dtype <EOL> ) <EOL> return quat <EOL> @ parameters_as_numpy_arrays ( '<STR_LIT>' ) <EOL> def create_from_eulers ( eulers , dtype = None ) : <EOL> """<STR_LIT>""" <EOL> dtype = dtype or eulers . dtype <EOL> pitch , yaw , roll = eulers <EOL> halfPitch = pitch * <NUM_LIT:0.5> <EOL> sP = np . sin ( halfPitch ) <EOL> cP = np . cos ( halfPitch ) <EOL> halfRoll = roll * <NUM_LIT:0.5> <EOL> sR = np . sin ( halfRoll ) <EOL> cR = np . cos ( halfRoll ) <EOL> halfYaw = yaw * <NUM_LIT:0.5> <EOL> sY = np . sin ( halfYaw ) <EOL> cY = np . cos ( halfYaw ) <EOL> return np . array ( <EOL> [ <EOL> ( - cY * sP * cR ) - ( sY * cP * sR ) , <EOL> ( cY * sP * sR ) - ( sY * cP * cR ) , <EOL> ( sY * sP * cR ) - ( cY * cP * sR ) , <EOL> ( cY * cP * cR ) + ( sY * sP * sR ) , <EOL> ] , <EOL> dtype = dtype <EOL> ) <EOL> @ parameters_as_numpy_arrays ( '<STR_LIT>' ) <EOL> def create_from_inverse_of_eulers ( eulers , dtype = None ) : <EOL> """<STR_LIT>""" <EOL> dtype = dtype or eulers . dtype <EOL> pitch , roll , yaw = euler . pitch ( eulers ) , euler . roll ( eulers ) , euler . yaw ( eulers ) <EOL> halfRoll = roll * <NUM_LIT:0.5> <EOL> sinRoll = np . sin ( halfRoll ) <EOL> cosRoll = np . cos ( halfRoll ) <EOL> halfPitch = pitch * <NUM_LIT:0.5> <EOL> sinPitch = np . sin ( halfPitch ) <EOL> cosPitch = np . cos ( halfPitch ) <EOL> halfYaw = yaw * <NUM_LIT:0.5> <EOL> sinYaw = np . sin ( halfYaw ) <EOL> cosYaw = np . cos ( halfYaw ) <EOL> return np . array ( <EOL> [ <EOL> ( cosYaw * sinPitch * cosRoll ) + ( sinYaw * cosPitch * sinRoll ) , <EOL> ( - cosYaw * sinPitch * sinRoll ) + ( sinYaw * cosPitch * cosRoll ) , <EOL> ( - sinYaw * sinPitch * cosRoll ) + ( cosYaw * cosPitch * sinRoll ) , <EOL> ( cosYaw * cosPitch * cosRoll ) + ( sinYaw * sinPitch * sinRoll ) <EOL> ] , <EOL> dtype = dtype <EOL> ) <EOL> @ all_parameters_as_numpy_arrays <EOL> def cross ( quat1 , quat2 ) : <EOL> """<STR_LIT>""" <EOL> q1x , q1y , q1z , q1w = quat1 <EOL> q2x , q2y , q2z , q2w = quat2 <EOL> return np . array ( <EOL> [ <EOL> q1x * q2w + q1y * q2z - q1z * q2y + q1w * q2x , <EOL> - q1x * q2z + q1y * q2w + q1z * q2x + q1w * q2y , <EOL> q1x * q2y - q1y * q2x + q1z * q2w + q1w * q2z , <EOL> - q1x * q2x - q1y * q2y - q1z * q2z + q1w * q2w , <EOL> ] , <EOL> dtype = quat1 . dtype <EOL> ) <EOL> def is_zero_length ( quat ) : <EOL> """<STR_LIT>""" <EOL> return quat [ <NUM_LIT:0> ] == quat [ <NUM_LIT:1> ] == quat [ <NUM_LIT:2> ] == quat [ <NUM_LIT:3> ] == <NUM_LIT:0.0> <EOL> def is_non_zero_length ( quat ) : <EOL> """<STR_LIT>""" <EOL> return not is_zero_length ( quat ) <EOL> def squared_length ( quat ) : <EOL> """<STR_LIT>""" <EOL> return vector4 . squared_length ( quat ) <EOL> def length ( quat ) : <EOL> """<STR_LIT>""" <EOL> return vector4 . length ( quat ) <EOL> def normalise ( quat ) : <EOL> """<STR_LIT>""" <EOL> return vector4 . normalise ( quat ) <EOL> def rotation_angle ( quat ) : <EOL> """<STR_LIT>""" <EOL> thetaOver2 = np . arccos ( quat [ <NUM_LIT:3> ] ) <EOL> return thetaOver2 * <NUM_LIT> <EOL> @ all_parameters_as_numpy_arrays <EOL> def rotation_axis ( quat ) : <EOL> """<STR_LIT>""" <EOL> sinThetaOver2Sq = <NUM_LIT:1.0> - ( quat [ <NUM_LIT:3> ] ** <NUM_LIT:2> ) <EOL> if sinThetaOver2Sq <= <NUM_LIT:0.0> : <EOL> return np . array ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , - <NUM_LIT:1.0> ] , dtype = quat . dtype ) <EOL> oneOverSinThetaOver2 = <NUM_LIT:1.0> / np . sqrt ( sinThetaOver2Sq ) <EOL> return np . array ( <EOL> [ <EOL> quat [ <NUM_LIT:0> ] * oneOverSinThetaOver2 , <EOL> quat [ <NUM_LIT:1> ] * oneOverSinThetaOver2 , <EOL> quat [ <NUM_LIT:2> ] * oneOverSinThetaOver2 <EOL> ] , <EOL> dtype = quat . dtype <EOL> ) <EOL> def dot ( quat1 , quat2 ) : <EOL> """<STR_LIT>""" <EOL> return vector4 . dot ( quat1 , quat2 ) <EOL> @ all_parameters_as_numpy_arrays <EOL> def conjugate ( quat ) : <EOL> """<STR_LIT>""" <EOL> return np . array ( <EOL> [ <EOL> - quat [ <NUM_LIT:0> ] , <EOL> - quat [ <NUM_LIT:1> ] , <EOL> - quat [ <NUM_LIT:2> ] , <EOL> quat [ <NUM_LIT:3> ] <EOL> ] , <EOL> dtype = quat . dtype <EOL> ) <EOL> @ parameters_as_numpy_arrays ( '<STR_LIT>' ) <EOL> def power ( quat , exponent ) : <EOL> """<STR_LIT>""" <EOL> if np . fabs ( quat [ <NUM_LIT:3> ] ) > <NUM_LIT> : <EOL> assert False <EOL> print ( "<STR_LIT>" ) <EOL> return quat <EOL> alpha = np . arccos ( quat [ <NUM_LIT:3> ] ) <EOL> newAlpha = alpha * exponent <EOL> multi = np . sin ( newAlpha ) / np . sin ( alpha ) <EOL> return np . array ( <EOL> [ <EOL> quat [ <NUM_LIT:0> ] * multi , <EOL> quat [ <NUM_LIT:1> ] * multi , <EOL> quat [ <NUM_LIT:2> ] * multi , <EOL> np . cos ( newAlpha ) <EOL> ] , <EOL> dtype = quat . dtype <EOL> ) <EOL> def inverse ( quat ) : <EOL> """<STR_LIT>""" <EOL> return conjugate ( quat ) / length ( quat ) <EOL> @ all_parameters_as_numpy_arrays <EOL> def negate ( quat ) : <EOL> """<STR_LIT>""" <EOL> return quat * - <NUM_LIT:1.0> <EOL> def is_identity ( quat ) : <EOL> return np . allclose ( quat , [ <NUM_LIT:0.> , <NUM_LIT:0.> , <NUM_LIT:0.> , <NUM_LIT:1.> ] ) <EOL> @ all_parameters_as_numpy_arrays <EOL> def apply_to_vector ( quat , vec ) : <EOL> """<STR_LIT>""" <EOL> def apply ( quat , vec4 ) : <EOL> """<STR_LIT>""" <EOL> length = vector . length ( vec4 ) <EOL> vec4 [ : ] = vector . normalise ( vec4 ) <EOL> result = cross ( quat , cross ( vec4 , conjugate ( quat ) ) ) <EOL> result *= length <EOL> return result <EOL> if vec . size == <NUM_LIT:3> : <EOL> vec = np . array ( [ vec [ <NUM_LIT:0> ] , vec [ <NUM_LIT:1> ] , vec [ <NUM_LIT:2> ] , <NUM_LIT:0.0> ] , dtype = vec . dtype ) <EOL> vec = apply ( quat , vec ) <EOL> vec = vec [ : <NUM_LIT:3> ] <EOL> return vec <EOL> elif vec . size == <NUM_LIT:4> : <EOL> vec = apply ( quat , vec ) <EOL> return vec <EOL> else : <EOL> raise ValueError ( "<STR_LIT>" ) </s>
