<s> import numpy as np <EOL> class Quaternion ( object ) : <EOL> """<STR_LIT>""" <EOL> def __init__ ( self , w = <NUM_LIT:1> , x = <NUM_LIT:0> , y = <NUM_LIT:0> , z = <NUM_LIT:0> , normalize = True ) : <EOL> self . w = float ( w ) <EOL> self . x , self . y , self . z = float ( x ) , float ( y ) , float ( z ) <EOL> if normalize : <EOL> self . _normalize ( ) <EOL> def __repr__ ( self ) : <EOL> return "<STR_LIT>" % ( <EOL> self . w , self . x , self . y , self . z ) <EOL> def copy ( self ) : <EOL> """<STR_LIT>""" <EOL> return Quaternion ( self . w , self . x , self . y , self . z , False ) <EOL> def norm ( self ) : <EOL> """<STR_LIT>""" <EOL> tmp = self . w ** <NUM_LIT:2> + self . x ** <NUM_LIT:2> + self . y ** <NUM_LIT:2> + self . z ** <NUM_LIT:2> <EOL> return tmp ** <NUM_LIT:0.5> <EOL> def _normalize ( self ) : <EOL> """<STR_LIT>""" <EOL> L = self . norm ( ) <EOL> if not L : <EOL> raise ValueError ( '<STR_LIT>' ) <EOL> self . w /= L <EOL> self . x /= L <EOL> self . y /= L <EOL> self . z /= L <EOL> def normalize ( self ) : <EOL> """<STR_LIT>""" <EOL> new = self . copy ( ) <EOL> new . _normalize ( ) <EOL> return new <EOL> def conjugate ( self ) : <EOL> """<STR_LIT>""" <EOL> new = self . copy ( ) <EOL> new . x *= - <NUM_LIT:1> <EOL> new . y *= - <NUM_LIT:1> <EOL> new . z *= - <NUM_LIT:1> <EOL> return new <EOL> def inverse ( self ) : <EOL> """<STR_LIT>""" <EOL> new = self . conjugate ( ) <EOL> tmp = self . norm ( ) ** <NUM_LIT:2> <EOL> new . w /= tmp <EOL> new . x /= tmp <EOL> new . y /= tmp <EOL> new . z /= tmp <EOL> return new <EOL> def exp ( self ) : <EOL> """<STR_LIT>""" <EOL> vecNorm = self . x ** <NUM_LIT:2> + self . y ** <NUM_LIT:2> + self . z ** <NUM_LIT:2> <EOL> wPart = np . exp ( self . w ) <EOL> q = Quaternion ( ) <EOL> q . w = wPart * np . cos ( vecNorm ) <EOL> q . x = wPart * self . x * np . sin ( vecNorm ) / vecNorm <EOL> q . y = wPart * self . y * np . sin ( vecNorm ) / vecNorm <EOL> q . z = wPart * self . z * np . sin ( vecNorm ) / vecNorm <EOL> return q <EOL> def log ( self ) : <EOL> """<STR_LIT>""" <EOL> norm = self . norm ( ) <EOL> vecNorm = self . x ** <NUM_LIT:2> + self . y ** <NUM_LIT:2> + self . z ** <NUM_LIT:2> <EOL> tmp = self . w / norm <EOL> q = Quaternion ( ) <EOL> q . w = np . log ( norm ) <EOL> q . x = np . log ( norm ) * self . x * np . arccos ( tmp ) / vecNorm <EOL> q . y = np . log ( norm ) * self . y * np . arccos ( tmp ) / vecNorm <EOL> q . z = np . log ( norm ) * self . z * np . arccos ( tmp ) / vecNorm <EOL> return q <EOL> def __add__ ( self , q ) : <EOL> """<STR_LIT>""" <EOL> new = self . copy ( ) <EOL> new . w += q . w <EOL> new . x += q . x <EOL> new . y += q . y <EOL> new . z += q . z <EOL> return new <EOL> def __sub__ ( self , q ) : <EOL> """<STR_LIT>""" <EOL> new = self . copy ( ) <EOL> new . w -= q . w <EOL> new . x -= q . x <EOL> new . y -= q . y <EOL> new . z -= q . z <EOL> return new <EOL> def __mul__ ( self , q2 ) : <EOL> """<STR_LIT>""" <EOL> new = Quaternion ( ) <EOL> q1 = self <EOL> new . w = q1 . w * q2 . w - q1 . x * q2 . x - q1 . y * q2 . y - q1 . z * q2 . z <EOL> new . x = q1 . w * q2 . x + q1 . x * q2 . w + q1 . y * q2 . z - q1 . z * q2 . y <EOL> new . y = q1 . w * q2 . y + q1 . y * q2 . w + q1 . z * q2 . x - q1 . x * q2 . z <EOL> new . z = q1 . w * q2 . z + q1 . z * q2 . w + q1 . x * q2 . y - q1 . y * q2 . x <EOL> return new <EOL> def rotate_point ( self , p ) : <EOL> """<STR_LIT>""" <EOL> p = Quaternion ( <NUM_LIT:0> , p [ <NUM_LIT:0> ] , p [ <NUM_LIT:1> ] , p [ <NUM_LIT:2> ] , False ) <EOL> q1 = self . normalize ( ) <EOL> q2 = self . inverse ( ) <EOL> r = ( q1 * p ) * q2 <EOL> return r . x , r . y , r . z <EOL> def get_matrix ( self ) : <EOL> """<STR_LIT>""" <EOL> a = np . zeros ( ( <NUM_LIT:4> , <NUM_LIT:4> ) , dtype = np . float32 ) <EOL> w , x , y , z = self . w , self . x , self . y , self . z <EOL> a [ <NUM_LIT:0> , <NUM_LIT:0> ] = - <NUM_LIT> * ( y * y + z * z ) + <NUM_LIT:1.0> <EOL> a [ <NUM_LIT:1> , <NUM_LIT:0> ] = + <NUM_LIT> * ( x * y + z * w ) <EOL> a [ <NUM_LIT:2> , <NUM_LIT:0> ] = + <NUM_LIT> * ( x * z - y * w ) <EOL> a [ <NUM_LIT:3> , <NUM_LIT:0> ] = <NUM_LIT:0.0> <EOL> a [ <NUM_LIT:0> , <NUM_LIT:1> ] = + <NUM_LIT> * ( x * y - z * w ) <EOL> a [ <NUM_LIT:1> , <NUM_LIT:1> ] = - <NUM_LIT> * ( x * x + z * z ) + <NUM_LIT:1.0> <EOL> a [ <NUM_LIT:2> , <NUM_LIT:1> ] = + <NUM_LIT> * ( z * y + x * w ) <EOL> a [ <NUM_LIT:3> , <NUM_LIT:1> ] = <NUM_LIT:0.0> <EOL> a [ <NUM_LIT:0> , <NUM_LIT:2> ] = + <NUM_LIT> * ( x * z + y * w ) <EOL> a [ <NUM_LIT:1> , <NUM_LIT:2> ] = + <NUM_LIT> * ( y * z - x * w ) <EOL> a [ <NUM_LIT:2> , <NUM_LIT:2> ] = - <NUM_LIT> * ( x * x + y * y ) + <NUM_LIT:1.0> <EOL> a [ <NUM_LIT:3> , <NUM_LIT:2> ] = <NUM_LIT:0.0> <EOL> a [ <NUM_LIT:0> , <NUM_LIT:3> ] = <NUM_LIT:0.0> <EOL> a [ <NUM_LIT:1> , <NUM_LIT:3> ] = <NUM_LIT:0.0> <EOL> a [ <NUM_LIT:2> , <NUM_LIT:3> ] = <NUM_LIT:0.0> <EOL> a [ <NUM_LIT:3> , <NUM_LIT:3> ] = <NUM_LIT:1.0> <EOL> return a <EOL> def get_axis_angle ( self ) : <EOL> """<STR_LIT>""" <EOL> angle = <NUM_LIT:2> * np . arccos ( max ( min ( self . w , <NUM_LIT:1.> ) , - <NUM_LIT:1.> ) ) <EOL> scale = ( self . x ** <NUM_LIT:2> + self . y ** <NUM_LIT:2> + self . z ** <NUM_LIT:2> ) ** <NUM_LIT:0.5> <EOL> if scale : <EOL> ax = self . x / scale <EOL> ay = self . y / scale <EOL> az = self . z / scale <EOL> else : <EOL> ax , ay , az = <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> <EOL> return angle , ax , ay , az <EOL> @ classmethod <EOL> def create_from_axis_angle ( cls , angle , ax , ay , az , degrees = False ) : <EOL> """<STR_LIT>""" <EOL> if degrees : <EOL> angle = np . radians ( angle ) <EOL> while angle < <NUM_LIT:0> : <EOL> angle += np . pi * <NUM_LIT:2> <EOL> angle2 = angle / <NUM_LIT> <EOL> sinang2 = np . sin ( angle2 ) <EOL> return Quaternion ( np . cos ( angle2 ) , ax * sinang2 , ay * sinang2 , az * sinang2 ) <EOL> @ classmethod <EOL> def create_from_euler_angles ( cls , rx , ry , rz , degrees = False ) : <EOL> """<STR_LIT>""" <EOL> if degrees : <EOL> rx , ry , rz = np . radians ( [ rx , ry , rz ] ) <EOL> qx = Quaternion ( np . cos ( rx / <NUM_LIT:2> ) , <NUM_LIT:0> , <NUM_LIT:0> , np . sin ( rx / <NUM_LIT:2> ) ) <EOL> qy = Quaternion ( np . cos ( ry / <NUM_LIT:2> ) , <NUM_LIT:0> , np . sin ( ry / <NUM_LIT:2> ) , <NUM_LIT:0> ) <EOL> qz = Quaternion ( np . cos ( rz / <NUM_LIT:2> ) , np . sin ( rz / <NUM_LIT:2> ) , <NUM_LIT:0> , <NUM_LIT:0> ) <EOL> return qx * qy * qz </s>
