def WriteVTSXMLVolumeFile(self):
     if (self.OutputFileName == ''):
         self.PrintError('Error: no OutputFileName.')
     self.PrintLog('Writing VTS XML grid file.')
     if self.ApplyTransform == 0:
         origin = self.Image.GetOrigin()
         spacing = self.Image.GetSpacing()
         matrix = vtk.vtkMatrix4x4()
         matrix.DeepCopy((1/spacing[0], 0, 0, - origin[0]/spacing[0], 
                      0, 1/spacing[1], 0, - origin[1]/spacing[1],
                      0, 0, 1/spacing[2], - origin[2]/spacing[2],
                      0, 0, 0, 1)) #LPI convention with correct origin and spacing 
     else:
         if self.RasToIjkMatrixCoefficients == None:
             self.PrintError('Error: no RasToIjkMatrixCoefficients.')
         matrix = vtk.vtkMatrix4x4()
         matrix.DeepCopy(self.RasToIjkMatrixCoefficients)
     trans = vtk.vtkTransform()
     trans.SetMatrix(matrix)
     trans_filt = vtk.vtkTransformFilter()
     trans_filt.SetTransform(trans)
     trans_filt.SetInputData(self.Image)
     trans_filt.Update()
     writer = vtk.vtkXMLStructuredGridWriter()
     writer.SetInputConnection(trans_filt.GetOutputPort())
     writer.SetFileName(self.OutputFileName)
     writer.Write()

  def _update_camera_params(self):
    P = vtk.vtkMatrix4x4() # projection matrix with clipping planes
    P.Zero()
    P.SetElement(0, 0, self._fx)
    P.SetElement(1, 1, self._fy)
    P.SetElement(0, 2, self._cx)
    P.SetElement(1, 2, self._cy)
    P.SetElement(2, 2, -self.near_clipping - self.far_clipping)
    P.SetElement(2, 3, -self.near_clipping * self.far_clipping)
    P.SetElement(3, 2, -1.)

    # first, reset the user transformation matrix
    cameraTransform = vtk.vtkPerspectiveTransform()
    self._camera.SetUserTransform(cameraTransform)

    # current projection matrix for the VTK camera
    Minv = self._camera.GetProjectionTransformMatrix(
        self.width / self.height, self.near_clipping, self.far_clipping)
    Minv.Invert()

    # desired user transform matrix U: UM = P
    U = vtk.vtkMatrix4x4()
    vtk.vtkMatrix4x4.Multiply4x4(P, Minv, U)

    # and finally update the transform
    cameraTransform.SetMatrix(U)
    self._camera.SetUserTransform(cameraTransform)

    self.render_window.SetSize(self.width, self.height)
    self.update()

  def calculateDistance(self):
    tipPoint = [0.0,0.0,0.0]
    targetPoint = [0.0, 0.0, 0.0]

    m = vtk.vtkMatrix4x4()
    self.toolTipToTool.GetMatrixTransformToWorld(m)
    tipPoint[0] = m.GetElement(0, 3)
    tipPoint[1] = m.GetElement(1, 3)
    tipPoint[2] = m.GetElement(2, 3)

    self.targetFiducial.GetNthFiducialPosition (1, targetPoint)
    # print(targetPoint)

    # Prueba David
    needlePoint = [0.0, 0.0, 0.0]
    v = vtk.vtkMatrix4x4()
    self.toolToReference.GetMatrixTransformToWorld(v)
    needlePoint[0] = v.GetElement(0, 3)
    needlePoint[1] = v.GetElement(1, 3)
    needlePoint[2] = v.GetElement(2, 3)
    # print(needlePoint)

    distance = math.sqrt(math.pow(tipPoint[0]-targetPoint[0], 2) + math.pow(tipPoint[1]-targetPoint[1], 2) + math.pow(tipPoint[2]-targetPoint[2], 2))
    
    self.outputDistanceLabel.setText('%.1f' % distance)

    self.drawLineBetweenPoints(tipPoint, targetPoint)

  def calculateDistance(self):

    tipPoint = [0.0,0.0,0.0]
    targetPoint = [0.0, 0.0, 0.0]
    self.targetFiducial.GetNthFiducialPosition (0, targetPoint)
    targetPoint = targetPoint + [1.0]


    if self.targetCreatedTransformed:

      ssr = vtk.vtkMatrix4x4()
      self.patientToReference.GetMatrixTransformToWorld(ssr) 

      self.targetPointTransformed = [0.0,0.0,0.0,1.0]
      ssr.MultiplyPoint(targetPoint, self.targetPointTransformed)
      self.targetPointTransformed = [self.targetPointTransformed[i] for i in (0,1,2)]
      tpoint = self.targetPointTransformed

    else:
      tpoint = [targetPoint[i] for i in (0,1,2)]
      

     
    m = vtk.vtkMatrix4x4()
    self.toolTipToTool.GetMatrixTransformToWorld(m)
    tipPoint[0] = m.GetElement(0, 3)
    tipPoint[1] = m.GetElement(1, 3)
    tipPoint[2] = m.GetElement(2, 3)
        # print(targetPoint)

    distance = math.sqrt(math.pow(tipPoint[0]-tpoint[0], 2) + math.pow(tipPoint[1]-tpoint[1], 2) + math.pow(tipPoint[2]-tpoint[2], 2))
    
    self.outputDistanceLabel.setText('%.1f' % distance)

    self.drawLineBetweenPoints(tipPoint, tpoint)

  def calculateAngle(self):

    baseConePoint = [0.0, 0.0, 0.0]
    self.targetFiducial.GetNthFiducialPosition (1, baseConePoint)
    baseConePoint = baseConePoint + [1.0]

    if self.targetCreatedTransformed:

      ssr = vtk.vtkMatrix4x4()
      self.patientToReference.GetMatrixTransformToWorld(ssr) 

      self.baseConePointTransformed = [0.0,0.0,0.0,1.0]
      ssr.MultiplyPoint(baseConePoint, self.baseConePointTransformed)
      self.baseConePointTransformed = [self.baseConePointTransformed[i] for i in (0,1,2)]
      baseConePoint = self.baseConePointTransformed
    
    tipPoint = [0.0,0.0,0.0]
    m = vtk.vtkMatrix4x4()
    self.toolTipToTool.GetMatrixTransformToWorld(m)
    tipPoint[0] = m.GetElement(0, 3)
    tipPoint[1] = m.GetElement(1, 3)
    tipPoint[2] = m.GetElement(2, 3)

    tipPoint 
    
    pos1Pos = np.subtract(baseConePoint, self.targetPointTransformed) # Normaliza los vectores
    pos1Pos2 = np.subtract(tipPoint, self.targetPointTransformed) # Normaliza los vectores

    cosine_angle = np.dot(pos1Pos, pos1Pos2) / (np.linalg.norm(pos1Pos) * np.linalg.norm(pos1Pos2))

    angle = np.arccos(cosine_angle)

    return np.degrees(angle)

  def calculateNeedleVector(self):
    
    needle = slicer.util.getNode('NeedleModel')
    polydataNeedle = needle.GetPolyData()
    center = polydataNeedle.GetCenter()
    center = [center[i] for i in (0,1,2)]
    center = center + [1.0]

    ssr = vtk.vtkMatrix4x4()
    transform= slicer.util.getNode('needleModelToNeedleTip')
    transform.GetMatrixTransformToWorld(ssr)

    centerTransformed = [0.0,0.0,0.0,1.0]
    ssr.MultiplyPoint(center, centerTransformed)
    centerTransformed = [centerTransformed[i] for i in (0,1,2)]

    m = vtk.vtkMatrix4x4()
    nttn = slicer.util.getNode('needleModelToNeedleTip')
    nttn.GetMatrixTransformToWorld(m)
    tipPoint = [0.0,0.0,0.0]
    tipPoint[0] = m.GetElement(0, 3)
    tipPoint[1] = m.GetElement(1, 3)
    tipPoint[2] = m.GetElement(2, 3)

    needleVector = np.subtract(centerTransformed,tipPoint)

    return needleVector

  def __init__(self): 
    # Variable Definition
    self.m = vtk.vtkMatrix4x4() # 4x4 VTK matrix to save the transformation matrix sent through Plus.
    self.transform=numpy.zeros((3,3), dtype=numpy.float64) # Numpy matrix used as input for the transformation decomposition.
    self.observedNode = None
    self.outputObserverTag = -1
    self.record = False
    self.timerActive=False
    self.recordingTime_mode1=10   # 5 seconds to record initial position
    self.recordingTime_mode2=10   # 5 seconds to record initial position
    self.mode=0
    self.myTimer=Timer()
    self.recordedDataBuffer = [] 
    self.processedRotation =[]
    self.name = '' # To save data
    self.prename = '' #To save data
    self.alarmCounter = 0
    self.rotationMatrix=list()
    self.rotationMatrices=list()

    # Variables to control the repetition number of each movement.
    self.repIDflexoext=0
    self.repIDflexlat=0
    self.repIDrotation=0

    # Arrays to save measured data.
    self.timeStamp=numpy.array([])

    # Defines Alarm file path and mode
    self.sound = qt.QSound('G:/ModulesSlicer/src/AlgiaCervical/Resources/Alarm/alarm.wav')
    self.sound.setLoops(1)

    # Labels for Data Visualization
    self.outputROMLabel = None
    self.outputMaxVariabilityLabel = None
    self.outputMinVariabilityLabel = None

    import Viewpoint # Viewpoint Module must have been added to Slicer 
    self.viewpointLogic = Viewpoint.ViewpointLogic()

    # Transform matrix of viewpoint in order to center head model in 3D view
    self.headCameraToHead = slicer.util.getNode('headCameraToHead')
    if not self.headCameraToHead:
      self.headCameraToHead=slicer.vtkMRMLLinearTransformNode()
      self.headCameraToHead.SetName("headCameraToHead")
      matrixHeadCamera = vtk.vtkMatrix4x4()
      matrixHeadCamera.SetElement( 0, 0, 1.0 ) # Row 1
      matrixHeadCamera.SetElement( 0, 1, 0.05 )
      matrixHeadCamera.SetElement( 0, 2, 0.01 )
      matrixHeadCamera.SetElement( 0, 3, 9.42 )      
      matrixHeadCamera.SetElement( 1, 0, 0.0 )  # Row 2
      matrixHeadCamera.SetElement( 1, 1, 0.28 )
      matrixHeadCamera.SetElement( 1, 2, -0.96 )
      matrixHeadCamera.SetElement( 1, 3, -252.69 )       
      matrixHeadCamera.SetElement( 2, 0, -0.05 )  # Row 3
      matrixHeadCamera.SetElement( 2, 1, 0.96 )
      matrixHeadCamera.SetElement( 2, 2, 0.28 )
      matrixHeadCamera.SetElement( 2, 3, 122.39 )
      self.headCameraToHead.SetMatrixTransformToParent(matrixHeadCamera)
      slicer.mrmlScene.AddNode(self.headCameraToHead)

 def __init__(self):
     '''
     Constructor
     '''
     self.__OrientationMatrix = vtk.vtkMatrix4x4()
     self.__OrientationMatrix.Identity()
     self.__InvertedOrientationMatrix = vtk.vtkMatrix4x4()
     self.__InvertedOrientationMatrix.Identity()

def convert_transform_to_vtk(transform, scaled=False, mode=[1,1,1,1]):
    """ Produce an output vtkTransform corresponding to the
    registration results. Input is a 15-component
    transform vector."""

    if scaled:
        # transform_scaling (must be same as defined above in class)
        #transform = numpy.divide(transform, numpy.array([1, 1, 1, .5, .5, .5, 300, 300, 300, 1, 1, 1, 1, 1, 1]))
        transform = numpy.divide(transform, numpy.array([1, 1, 1, .5, .5, .5, 200, 200, 200, 1, 1, 1, 1, 1, 1]))
                                 
    vtktrans = vtk.vtkTransform()

    if mode[0]:
        # translate first so optimizer starts there
        vtktrans.Translate(transform[0],
                        transform[1], transform[2])

    if mode[1]:
        # degrees
        vtktrans.RotateX(transform[3])
        vtktrans.RotateY(transform[4])
        vtktrans.RotateZ(transform[5])

    if mode[2]:
        vtktrans.Scale(transform[6],
                    transform[7], transform[8])

    if mode[3]:
        #// Update affine transformation: Add shearing
        #vtkMatrix4x4 *skewx= vtkMatrix4x4::New();  skewx->Identity();
        #skewx->SetElement(2, 1,tan(_szy*(pi/180.0)));   skewx->SetElement(1, 2,tan(_syz*(pi/180.0))); 
        #vtkMatrix4x4 *skewy= vtkMatrix4x4::New();   skewy->Identity();
        #skewy->SetElement(2, 0,tan(_szx*(pi/180.0)));   skewy->SetElement(0, 2,tan(_sxz*(pi/180.0)));
        #vtkMatrix4x4 *skewz= vtkMatrix4x4::New();   skewz->Identity();
        #skewz->SetElement(1, 0,tan(_sxy*(pi/180.0)));    skewz->SetElement(0, 1,tan(_syx*(pi/180.0))); 
        #tr->Concatenate(skewx);   tr->Concatenate(skewy);   tr->Concatenate(skewz);
        sxy = transform[9] * numpy.pi/180.0
        sxz = transform[10] * numpy.pi/180.0
        syx = transform[11] * numpy.pi/180.0
        syz = transform[12] * numpy.pi/180.0
        szx = transform[13] * numpy.pi/180.0
        szy = transform[14] * numpy.pi/180.0
        skewx = vtk.vtkMatrix4x4()
        skewy = vtk.vtkMatrix4x4()
        skewz = vtk.vtkMatrix4x4()
        skewx.SetElement(2, 1, numpy.tan(szy))
        skewx.SetElement(1, 2, numpy.tan(syz))
        skewy.SetElement(2, 0, numpy.tan(szx))
        skewy.SetElement(0, 2, numpy.tan(sxz))
        skewz.SetElement(1, 0, numpy.tan(sxy))
        skewz.SetElement(0, 1, numpy.tan(syx))
        vtktrans.Concatenate(skewx)
        vtktrans.Concatenate(skewy)
        vtktrans.Concatenate(skewz)

    return vtktrans

    def convert_transform_to_vtk(self, transform=None):
        """ Produce an output vtkTransform corresponding to the
        registration results. Optionally can input a 9-component
        transform vector."""
        
        if transform is None:
            transform = self.transform
        
        vtktrans = vtk.vtkTransform()

        vtktrans.RotateX(transform[0] * (180 / numpy.pi))
        vtktrans.RotateY(transform[1] * (180 / numpy.pi))
        vtktrans.RotateZ(transform[2] * (180 / numpy.pi))

        vtktrans.Translate(transform[3],
                           transform[4], transform[5])

        vtktrans.Scale(transform[6],
                       transform[7], transform[8])

        #// Update affine transformation: Add shearing
        #vtkMatrix4x4 *skewx= vtkMatrix4x4::New();  skewx->Identity();
        #skewx->SetElement(2, 1,tan(_szy*(pi/180.0)));   skewx->SetElement(1, 2,tan(_syz*(pi/180.0))); 
        #vtkMatrix4x4 *skewy= vtkMatrix4x4::New();   skewy->Identity();
        #skewy->SetElement(2, 0,tan(_szx*(pi/180.0)));   skewy->SetElement(0, 2,tan(_sxz*(pi/180.0)));
        #vtkMatrix4x4 *skewz= vtkMatrix4x4::New();   skewz->Identity();
        #skewz->SetElement(1, 0,tan(_sxy*(pi/180.0)));    skewz->SetElement(0, 1,tan(_syx*(pi/180.0))); 
        #tr->Concatenate(skewx);   tr->Concatenate(skewy);   tr->Concatenate(skewz);
        sxy = transform[9]
        sxz = transform[10]
        syx = transform[11]
        syz = transform[12]
        szx = transform[13]
        szy = transform[14]
        skewx = vtk.vtkMatrix4x4()
        skewy = vtk.vtkMatrix4x4()
        skewz = vtk.vtkMatrix4x4()
        skewx.SetElement(2, 1, numpy.tan(szy))
        skewx.SetElement(1, 2, numpy.tan(syz))
        skewy.SetElement(2, 0, numpy.tan(szx))
        skewy.SetElement(0, 2, numpy.tan(sxz))
        skewz.SetElement(1, 0, numpy.tan(sxy))
        skewz.SetElement(0, 1, numpy.tan(syx))
        vtktrans.Concatenate(skewx)
        vtktrans.Concatenate(skewy)
        vtktrans.Concatenate(skewz)
        #del skewx
        #del skewy
        #del skewz
        return vtktrans

 def AddTimestamp( self, time, matrix, point, role ):  
   for i in range( self.targets.GetNumberOfFiducials() ):    
     # Find the centre of the fiducial
     currTargetPosition = [ 0, 0, 0 ]
     self.targets.GetNthFiducialPosition( i, currTargetPosition )
     currTargetPosition_RAS = [ currTargetPosition[ 0 ], currTargetPosition[ 1 ], currTargetPosition[ 2 ], 1 ]
     
     # Assume the matrix is ImageToRAS
     # We know the center of mass of the structure in the RAS coordinate system
     # Transform the center of mass into the image coordinate system
     RASToImageMatrix = vtk.vtkMatrix4x4()
     RASToImageMatrix.DeepCopy( matrix )
     RASToImageMatrix.Invert()
   
     currTargetPosition_Image = [ 0, 0, 0, 1 ]
     RASToImageMatrix.MultiplyPoint( currTargetPosition_RAS, currTargetPosition_Image )
   
     # Assumption is the imaging plane is in the Image coordinate system's XY plane    
     if ( currTargetPosition_Image[0] < self.imageMinX or currTargetPosition_Image[0] > self.imageMaxX ):
       return
     
     if ( currTargetPosition_Image[1] < self.imageMinY or currTargetPosition_Image[1] > self.imageMaxY ):
       return
   
     # Note: This only works for similarity matrix (i.e. uniform scale factor)
     scaleFactor = math.pow( matrix.Determinant(), 1.0 / 3.0 )
   
     # Now check if the z-coordinate of the point in the image coordinate system is below some threshold value (i.e. 2mm)
     if ( abs( currTargetPosition_Image[2] ) < TargetsScanned.IMAGE_PLANE_THRESHOLD / scaleFactor ):
       self.hitTargets[ i ] = 1

  def __init__(self, sliceWidget):
    super(PaintEffectTool,self).__init__(sliceWidget)
    # create a logic instance to do the non-gui work
    self.logic = PaintEffectLogic(self.sliceWidget.sliceLogic())

    # configuration variables
    self.delayedPaint = True
    self.parameterNode = EditUtil.getParameterNode()
    self.sphere = not (0 == int(self.parameterNode.GetParameter("PaintEffect,sphere")))
    self.smudge = not (0 == int(self.parameterNode.GetParameter("PaintEffect,smudge")))
    self.pixelMode = not (0 == int(self.parameterNode.GetParameter("PaintEffect,pixelMode")))
    self.radius = float(self.parameterNode.GetParameter("PaintEffect,radius"))

    # interaction state variables
    self.position = [0, 0, 0]
    self.paintCoordinates = []
    self.feedbackActors = []
    self.lastRadius = 0

    # scratch variables
    self.rasToXY = vtk.vtkMatrix4x4()

    # initialization
    self.brush = vtk.vtkPolyData()
    self.createGlyph(self.brush)
    self.mapper = vtk.vtkPolyDataMapper2D()
    self.actor = vtk.vtkActor2D()
    self.mapper.SetInputData(self.brush)
    self.actor.SetMapper(self.mapper)
    self.actor.VisibilityOff()

    self.renderer.AddActor2D(self.actor)
    self.actors.append(self.actor)

    self.processEvent()

 def AddTimestamp( self, time, matrix, point, role ):
   if ( self.trajectory is None ):
     return
     
   # Build up the matrix
   newRow = [ 0 ] * self.trajectory.GetNumberOfDataNodes()
   self.distanceMatrix.append( newRow )
   i = len( self.distanceMatrix ) - 1
   
   # Use dynamic programming to compute the next row
   for j in range( self.trajectory.GetNumberOfDataNodes() ):
     currTrajectoryNode = self.trajectory.GetNthDataNode( j )
     currTrajectoryMatrix = vtk.vtkMatrix4x4()
     currTrajectoryNode.GetMatrixTransformToWorld( currTrajectoryMatrix )
     currTrajectoryPoint = [ currTrajectoryMatrix.GetElement( 0, 3 ), currTrajectoryMatrix.GetElement( 1, 3 ), currTrajectoryMatrix.GetElement( 2, 3 ) ]
        
     currDistance = vtk.vtkMath.Distance2BetweenPoints( point[ 0:3 ], currTrajectoryPoint )
     if ( i == 0 ):
       if ( j == 0 ):
         self.distanceMatrix[ i ][ j ] = currDistance
       else:
         self.distanceMatrix[ i ][ j ] = max( self.distanceMatrix[ i ][ j - 1 ], currDistance )
     else:
       if ( j == 0 ):
         self.distanceMatrix[ i ][ j ] = max( self.distanceMatrix[ i - 1 ][ j ], currDistance )
       else:
         self.distanceMatrix[ i ][ j ] = max( min( self.distanceMatrix[ i ][ j - 1 ], self.distanceMatrix[ i - 1 ][ j ], self.distanceMatrix[ i - 1 ][ j - 1 ] ), currDistance )
         
   self.frechetDistance = math.sqrt( self.distanceMatrix[ i ][ self.trajectory.GetNumberOfDataNodes() - 1 ] )

  def onApply(self):
    try:
      # Get master volume image data
      import vtkSegmentationCorePython as vtkSegmentationCore
      masterImageData = self.scriptedEffect.masterVolumeImageData()
      # Get modifier labelmap
      modifierLabelmap = self.scriptedEffect.defaultModifierLabelmap()
      originalImageToWorldMatrix = vtk.vtkMatrix4x4()
      modifierLabelmap.GetImageToWorldMatrix(originalImageToWorldMatrix)
      # Get parameters
      min = self.scriptedEffect.doubleParameter("MinimumThreshold")
      max = self.scriptedEffect.doubleParameter("MaximumThreshold")

      self.scriptedEffect.saveStateForUndo()

      # Perform thresholding
      thresh = vtk.vtkImageThreshold()
      thresh.SetInputData(masterImageData)
      thresh.ThresholdBetween(min, max)
      thresh.SetInValue(1)
      thresh.SetOutValue(0)
      thresh.SetOutputScalarType(modifierLabelmap.GetScalarType())
      thresh.Update()
      modifierLabelmap.DeepCopy(thresh.GetOutput())
    except IndexError:
      logging.error('apply: Failed to threshold master volume!')
      pass

    # Apply changes
    self.scriptedEffect.modifySelectedSegmentByLabelmap(modifierLabelmap, slicer.qSlicerSegmentEditorAbstractEffect.ModificationModeSet)

    # De-select effect
    self.scriptedEffect.selectEffect("")

    def probeVolume(self, volumeNode, rulerNode):

        # get ruler ednpoints coordinates in RAS
        p0ras = rulerNode.GetPolyData().GetPoint(0) + (1,)
        p1ras = rulerNode.GetPolyData().GetPoint(1) + (1,)

        # RAS --> IJK
        ras2ijk = vtk.vtkMatrix4x4()
        volumeNode.GetRASToIJKMatrix(ras2ijk)
        p0ijk = [int(round(c)) for c in ras2ijk.MultiplyPoint(p0ras)[:3]]
        p1ijk = [int(round(c)) for c in ras2ijk.MultiplyPoint(p1ras)[:3]]

        # Create VTK line that will be used for sampling
        line = vtk.vtkLineSource()
        line.SetResolution(100)
        line.SetPoint1(p0ijk)
        line.SetPoint2(p1ijk)

        # Create VTK probe filter and sample the image
        probe = vtk.vtkProbeFilter()
        probe .SetInputConnection(line.GetOutputPort())
        probe.SetSourceData(volumeNode.GetImageData())
        probe.Update()

        # Return VTK array
        return probe.GetOutput().GetPointData().GetArray('ImageScalars')

def transform_polydata_from_disk(in_filename, transform_filename, out_filename):
    # Read it in.
    print "<io.py> Transforming ", in_filename, "->", out_filename, "..."

    # Read the transform from disk because we cannot pickle it
    (root, ext) = os.path.splitext(transform_filename)
    print root, ext
    if ext == '.xfm':
        reader = vtk.vtkMNITransformReader()
        reader.SetFileName(transform_filename)
        reader.Update()
        transform = reader.GetTransform()
    elif ext == '.img':
        reader = vtk.vtkImageReader()
        reader.SetFileName(transform_filename)
        reader.Update()
        coeffs = reader.GetOutput()
        transform = vtk.vtkBSplineTransform()
        transform.SetCoefficients(coeffs)
        print coeffs
        print transform
    else:
        f = open(transform_filename, 'r')
        transform = vtk.vtkTransform()
        matrix = vtk.vtkMatrix4x4()
        for i in range(0,4):
            for j in range(0,4):
                matrix_val = float(f.readline())
                matrix.SetElement(i,j, matrix_val)
        transform.SetMatrix(matrix)
        del matrix

    start_time = time.time()
    pd = read_polydata(in_filename)
    elapsed_time = time.time() - start_time
    print "READ:", elapsed_time
    # Transform it.
    start_time = time.time()
    transformer = vtk.vtkTransformPolyDataFilter()
    if (vtk.vtkVersion().GetVTKMajorVersion() >= 6.0):
        transformer.SetInputData(pd)
    else:
        transformer.SetInput(pd)
    transformer.SetTransform(transform)
    transformer.Update()
    elapsed_time = time.time() - start_time
    print "TXFORM:", elapsed_time

    # Write it out.
    start_time = time.time()
    pd2 = transformer.GetOutput()
    write_polydata(pd2, out_filename)
    elapsed_time = time.time() - start_time
    print "WRITE:", elapsed_time

    # Clean up.
    del transformer
    del pd2
    del pd
    del transform

 def onApply(self):
   inputVolume = self.inputSelector.currentNode()
   outputVolume = self.outputSelector.currentNode()
   # check for input data
   if not (inputVolume and outputVolume):
     slicer.util.errorDisplay('Input and output volumes are required for conversion', windowTitle='Luminance')
     return
   # check that data has enough components
   inputImage = inputVolume.GetImageData()
   if not inputImage or inputImage.GetNumberOfScalarComponents() < 3:
     slicer.util.errorDisplay('Input does not have enough components for conversion',
                              windowTitle='Vector to Scalar Volume')
     return
   # run the filter
   # - extract the RGB portions
   extract = vtk.vtkImageExtractComponents()
   extract.SetComponents(0,1,2)
   luminance = vtk.vtkImageLuminance()
   extract.SetInputConnection(inputVolume.GetImageDataConnection())
   luminance.SetInputConnection(extract.GetOutputPort())
   luminance.Update()
   ijkToRAS = vtk.vtkMatrix4x4()
   inputVolume.GetIJKToRASMatrix(ijkToRAS)
   outputVolume.SetIJKToRASMatrix(ijkToRAS)
   outputVolume.SetImageDataConnection(luminance.GetOutputPort())
   # make the output volume appear in all the slice views
   selectionNode = slicer.app.applicationLogic().GetSelectionNode()
   selectionNode.SetReferenceActiveVolumeID(outputVolume.GetID())
   slicer.app.applicationLogic().PropagateVolumeSelection(0)

 def save(self): # writer to new file
     logger.log('Exporting temp STL of model %s...' % self.name)
     # Extract transformations done to the actor
     matrix = vtk.vtkMatrix4x4() 
     self.getActor().GetMatrix(matrix)
     # Apply transformation
     transform = vtk.vtkTransform()
     transform.SetMatrix(matrix)
     # T
     t_filter = vtk.vtkTransformPolyDataFilter()
     t_filter.SetInput(self.getPolyData())
     t_filter.SetTransform(transform)
     # Triangle filter
     #vtkTriangleFilter
     # Clean Polydata
     #vtkcleanpolydata
     # Simplify the model
     #vtk.vtkDecimate
     # Save data to a STL file
     writer = vtk.vtkSTLWriter()
     writer.SetFileName('temp.stl')
     writer.SetInputConnection(t_filter.GetOutputPort())
     writer.SetFileTypeToBinary()
     writer.Write()
     logger.log('End exporting')

  def calculateDistance(self):

    pointerTipPoint = [0.0,0.0,0.0]
    
     
    m = vtk.vtkMatrix4x4()
    self.pointerTipToPointer.GetMatrixTransformToWorld(m)
    pointerTipPoint[0] = m.GetElement(0, 3)
    pointerTipPoint[1] = m.GetElement(1, 3)
    pointerTipPoint[2] = m.GetElement(2, 3)
    

    distance = math.sqrt(math.pow(pointerTipPoint[0]-self.pos[0], 2) + math.pow(pointerTipPoint[1]-self.pos[1], 2) + math.pow(pointerTipPoint[2]-self.pos[2], 2))
    
    # La distancia se da en mm ----> 50mm = 5cm
    # Voy a normalizar con un tope de 20cm
    normalizedDistance = [(distance - 0)/200]
    
    if self.OSC_active:
      #print ("HOLAAA")
      c.send("/dumpOSC/0/0", 0)

    self.outputDistanceLabel.setText('%.1f' % distance)
    
    self.pos = [self.pos[i] for i in (0,1,2)]
    self.drawLineBetweenPoints(pointerTipPoint, self.pos)

    if self.sendDataOK:
      self.sendData(normalizedDistance)

 def AddTimestamp( self, time, matrix, point, role ):
   if ( self.trajectory is None ):
     return
     
   # Build up the matrix
   newRow = [ 0 ] * self.trajectory.GetNumberOfDataNodes()
   self.distanceMatrix.append( newRow )
   i = len( self.distanceMatrix ) - 1
   
   # Use dynamic programming to compute the next row
   for j in range( self.trajectory.GetNumberOfDataNodes() ):
     currTrajectoryNode = self.trajectory.GetNthDataNode( j )
     currTrajectoryMatrix = vtk.vtkMatrix4x4()
     currTrajectoryNode.GetMatrixTransformToWorld( currTrajectoryMatrix )
     currTrajectoryPoint = [ currTrajectoryMatrix.GetElement( 0, 3 ), currTrajectoryMatrix.GetElement( 1, 3 ), currTrajectoryMatrix.GetElement( 2, 3 ) ]
        
     currDistance = math.sqrt( vtk.vtkMath.Distance2BetweenPoints( point[ 0:3 ], currTrajectoryPoint ) )
     if ( i == 0 ):
       if ( j == 0 ):
         self.distanceMatrix[ i ][ j ] = currDistance
       else:
         self.distanceMatrix[ i ][ j ] = currDistance + self.distanceMatrix[ i ][ j - 1 ]
     else:
       if ( j == 0 ):
         self.distanceMatrix[ i ][ j ] = currDistance + self.distanceMatrix[ i - 1 ][ j ]
       else:
         self.distanceMatrix[ i ][ j ] = currDistance + min( self.distanceMatrix[ i ][ j - 1 ], self.distanceMatrix[ i - 1 ][ j ], self.distanceMatrix[ i - 1 ][ j - 1 ] )
         
   self.dtwDistance = self.distanceMatrix[ i ][ self.trajectory.GetNumberOfDataNodes() - 1 ]
   
   # Assume the dtw path length is the sum of the number of points in each sequence minus 1
   # Note: this counts diagonal jumps as 2
   self.dtwPathLength = len( self.distanceMatrix ) + len( self.distanceMatrix[ i ] ) - 1