<s> from director . thirdparty import transformations <EOL> import vtkAll as vtk <EOL> import numpy as np <EOL> try : <EOL> from director import botpy <EOL> except ImportError : <EOL> pass <EOL> def getTransformFromNumpy ( mat ) : <EOL> '''<STR_LIT>''' <EOL> assert mat . shape == ( <NUM_LIT:4> , <NUM_LIT:4> ) <EOL> t = vtk . vtkTransform ( ) <EOL> t . SetMatrix ( mat . flatten ( ) ) <EOL> return t <EOL> def getNumpyFromTransform ( transform ) : <EOL> '''<STR_LIT>''' <EOL> mat = transform . GetMatrix ( ) <EOL> a = np . zeros ( ( <NUM_LIT:4> , <NUM_LIT:4> ) ) <EOL> for r in xrange ( <NUM_LIT:4> ) : <EOL> for c in xrange ( <NUM_LIT:4> ) : <EOL> a [ r ] [ c ] = mat . GetElement ( r , c ) <EOL> return a <EOL> def getTransformFromAxes ( xaxis , yaxis , zaxis ) : <EOL> t = vtk . vtkTransform ( ) <EOL> m = vtk . vtkMatrix4x4 ( ) <EOL> axes = np . array ( [ xaxis , yaxis , zaxis ] ) . transpose ( ) . copy ( ) <EOL> vtk . vtkMath . Orthogonalize3x3 ( axes , axes ) <EOL> for r in xrange ( <NUM_LIT:3> ) : <EOL> for c in xrange ( <NUM_LIT:3> ) : <EOL> m . SetElement ( r , c , axes [ r ] [ c ] ) <EOL> t . SetMatrix ( m ) <EOL> return t <EOL> def getTransformFromAxesAndOrigin ( xaxis , yaxis , zaxis , origin ) : <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> return t <EOL> def getAxesFromTransform ( t ) : <EOL> xaxis = np . array ( t . TransformNormal ( <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ) ) <EOL> yaxis = np . array ( t . TransformNormal ( <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ) ) <EOL> zaxis = np . array ( t . TransformNormal ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ) ) <EOL> return xaxis , yaxis , zaxis <EOL> def getLookAtTransform ( lookAtPosition , lookFromPosition , viewUp = [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) : <EOL> xaxis = np . array ( lookAtPosition ) - np . array ( lookFromPosition ) <EOL> if np . linalg . norm ( xaxis ) < <NUM_LIT> : <EOL> xaxis = [ <NUM_LIT:1.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] <EOL> zaxis = np . array ( viewUp ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> zaxis /= np . linalg . norm ( zaxis ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> return getTransformFromAxesAndOrigin ( xaxis , yaxis , zaxis , lookFromPosition ) <EOL> def concatenateTransforms ( transformList ) : <EOL> '''<STR_LIT>''' <EOL> result = vtk . vtkTransform ( ) <EOL> result . PostMultiply ( ) <EOL> for t in transformList : <EOL> result . Concatenate ( t ) <EOL> return result <EOL> def findTransformAxis ( transform , referenceVector ) : <EOL> '''<STR_LIT>''' <EOL> refAxis = referenceVector / np . linalg . norm ( referenceVector ) <EOL> axes = getAxesFromTransform ( transform ) <EOL> axisProjections = np . array ( [ np . abs ( np . dot ( axis , refAxis ) ) for axis in axes ] ) <EOL> matchIndex = axisProjections . argmax ( ) <EOL> matchAxis = axes [ matchIndex ] <EOL> matchSign = np . sign ( np . dot ( matchAxis , refAxis ) ) <EOL> return matchIndex , matchAxis , matchSign <EOL> def getTransformFromOriginAndNormal ( origin , normal , normalAxis = <NUM_LIT:2> ) : <EOL> normal = np . array ( normal ) <EOL> normal /= np . linalg . norm ( normal ) <EOL> axes = [ [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , <EOL> [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ] <EOL> axes [ normalAxis ] = normal <EOL> vtk . vtkMath . Perpendiculars ( axes [ normalAxis ] , axes [ ( normalAxis + <NUM_LIT:1> ) % <NUM_LIT:3> ] , axes [ ( normalAxis + <NUM_LIT:2> ) % <NUM_LIT:3> ] , <NUM_LIT:0> ) <EOL> t = getTransformFromAxes ( * axes ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> return t <EOL> def orientationFromNormal ( normal ) : <EOL> '''<STR_LIT>''' <EOL> zaxis = normal <EOL> xaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> yaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> vtk . vtkMath . Perpendiculars ( zaxis , xaxis , yaxis , <NUM_LIT:0> ) <EOL> return orientationFromAxes ( xaxis , yaxis , zaxis ) <EOL> def orientationFromAxes ( xaxis , yaxis , zaxis ) : <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> return rollPitchYawFromTransform ( t ) <EOL> def rollPitchYawFromTransform ( t ) : <EOL> pos , quat = poseFromTransform ( t ) <EOL> return quaternionToRollPitchYaw ( quat ) <EOL> def frameInterpolate ( trans_a , trans_b , weight_b ) : <EOL> '''<STR_LIT>''' <EOL> [ pos_a , quat_a ] = poseFromTransform ( trans_a ) <EOL> [ pos_b , quat_b ] = poseFromTransform ( trans_b ) <EOL> pos_c = pos_a * ( <NUM_LIT:1> - weight_b ) + pos_b * weight_b ; <EOL> quat_c = transformations . quaternion_slerp ( quat_a , quat_b , weight_b ) <EOL> return transformFromPose ( pos_c , quat_c ) <EOL> def transformFromPose ( position , quaternion ) : <EOL> '''<STR_LIT>''' <EOL> mat = transformations . quaternion_matrix ( quaternion ) <EOL> mat [ : <NUM_LIT:3> , <NUM_LIT:3> ] = position <EOL> return getTransformFromNumpy ( mat ) <EOL> def poseFromTransform ( transform ) : <EOL> '''<STR_LIT>''' <EOL> mat = getNumpyFromTransform ( transform ) <EOL> return np . array ( mat [ : <NUM_LIT:3> , <NUM_LIT:3> ] ) , transformations . quaternion_from_matrix ( mat , isprecise = True ) <EOL> def frameFromPositionAndRPY ( position , rpy ) : <EOL> '''<STR_LIT>''' <EOL> rpy = np . radians ( rpy ) <EOL> mat = transformations . euler_matrix ( rpy [ <NUM_LIT:0> ] , rpy [ <NUM_LIT:1> ] , rpy [ <NUM_LIT:2> ] ) <EOL> mat [ : <NUM_LIT:3> , <NUM_LIT:3> ] = position <EOL> return getTransformFromNumpy ( mat ) <EOL> def rollPitchYawToQuaternion ( rpy ) : <EOL> return transformations . quaternion_from_euler ( rpy [ <NUM_LIT:0> ] , rpy [ <NUM_LIT:1> ] , rpy [ <NUM_LIT:2> ] ) <EOL> def quaternionToRollPitchYaw ( quat ) : <EOL> return transformations . euler_from_quaternion ( quat ) <EOL> def copyFrame ( transform ) : <EOL> t = vtk . vtkTransform ( ) <EOL> t . PostMultiply ( ) <EOL> t . SetMatrix ( transform . GetMatrix ( ) ) <EOL> return t </s>
