def add_widgets(self):
        # axes
        axes = vtk.vtkAxesActor()
        self.marker_widget = vtk.vtkOrientationMarkerWidget()
        self.marker_widget.SetInteractor(self.iren)
        self.marker_widget.SetOrientationMarker(axes)
        self.marker_widget.SetViewport(0.0, 0.0, 0.25, 0.25)

        # scalar bar
        self.scalarbar_actor = vtk.vtkScalarBarActor()
        self.scalarbar_actor.SetLookupTable(self.lut)
        self.scalarbar_widget = vtk.vtkScalarBarWidget()
        self.scalarbar_widget.SetInteractor(self.iren)
        self.scalarbar_widget.SetScalarBarActor(self.scalarbar_actor)

        # contour slider
        self.slider_rep = vtk.vtkSliderRepresentation2D()
        self.slider_rep.SetTitleText("contour")
        self.slider_rep.GetPoint1Coordinate().SetCoordinateSystemToNormalizedViewport()
        self.slider_rep.GetPoint1Coordinate().SetValue(0.65, 0.1)
        self.slider_rep.GetPoint2Coordinate().SetCoordinateSystemToNormalizedViewport()
        self.slider_rep.GetPoint2Coordinate().SetValue(0.95, 0.1)

        self.slider_widget = vtk.vtkSliderWidget()
        self.slider_widget.SetInteractor(self.iren)
        self.slider_widget.SetRepresentation(self.slider_rep)
        self.slider_widget.SetAnimationModeToAnimate()
        self.slider_widget.AddObserver(vtk.vtkCommand.InteractionEvent,
            self.update_contour);

    def SetObjective(self, ApplicatorObjetiveInOpticalCoordinates):

        if self.calibrated:
            # Save the Transformation Goal
            self.ApplicatorObjetiveAxis = ApplicatorObjetiveInOpticalCoordinates.copy()

            self.ApplicatorObjetiveAxis = numpy.dot(self.TWorldToOptical,self.ApplicatorObjetiveAxis)

            self.ApplicatorObjetive = vtk.vtkTransform()
            self.ApplicatorObjetive.SetMatrix([self.ApplicatorObjetiveAxis[0,0],self.ApplicatorObjetiveAxis[0,1],self.ApplicatorObjetiveAxis[0,2],self.ApplicatorObjetiveAxis[0,3],
                            self.ApplicatorObjetiveAxis[1,0],self.ApplicatorObjetiveAxis[1,1],self.ApplicatorObjetiveAxis[1,2],self.ApplicatorObjetiveAxis[1,3],
                            self.ApplicatorObjetiveAxis[2,0],self.ApplicatorObjetiveAxis[2,1],self.ApplicatorObjetiveAxis[2,2],self.ApplicatorObjetiveAxis[2,3],
                            self.ApplicatorObjetiveAxis[3,0],self.ApplicatorObjetiveAxis[3,1],self.ApplicatorObjetiveAxis[3,2],self.ApplicatorObjetiveAxis[3,3]])
            self.ApplicatorObjetiveaxes = vtk.vtkAxesActor()
            self.ApplicatorObjetiveaxes.SetUserTransform(self.ApplicatorObjetive)
            self.ApplicatorObjetiveaxes.SetXAxisLabelText('X App')
            self.ApplicatorObjetiveaxes.SetYAxisLabelText('Y App')
            self.ApplicatorObjetiveaxes.SetZAxisLabelText('Z App')
            self.ApplicatorObjetiveaxes.SetTotalLength(0.2,0.2,0.2)

            self.STLReaderApplicator = vtk.vtkSTLReader()
            self.STLReaderApplicator.SetFileName(self.pathToModelApplicator)
            self.STLReaderApplicator.Update()

            self.mapperApplicator = vtk.vtkPolyDataMapper()
            self.mapperApplicator.SetInputConnection(self.STLReaderApplicator.GetOutputPort())

            self.modelActorApplicator = vtk.vtkActor()
            self.modelActorApplicator.SetMapper(self.mapperApplicator)

            self.modelActorApplicator.SetUserTransform(self.ApplicatorObjetive)
            self.modelActorApplicator.GetProperty().SetColor(0,1,0)
            self.modelActorApplicator.GetProperty().SetOpacity(0.5)

            self.objetiveFixed = True

    def dataShape(self, shape):
        if shape == self._dataShape:
            return
        
        self._dataShape = shape
        
        if self.planes:
            self.planes.RemoveObserver(self.coordEventObserver)
            self.qvtk.renderer.RemoveViewProp(self.planes)
            del self.planes

        self.planes = SlicingPlanesWidget(shape, self.qvtk.GetInteractor())
        #self.planes.SetInteractor(self.qvtk.GetInteractor())
        self.coordEventObserver = self.planes.AddObserver("CoordinatesEvent", self.slicingCallback)
        self.planes.SetCoordinate([0,0,0])
        self.planes.SetPickable(False)
        
        ## Add RGB arrow axes
        if self.axes:
            self.qvtk.renderer.RemoveActor(self.axes)
            del self.axes
        self.axes = vtkAxesActor();
        self.axes.AxisLabelsOff()
        self.axes.SetTotalLength(0.5*shape[0], 0.5*shape[1], 0.5*shape[2])
        self.axes.SetShaftTypeToCylinder()
        self.qvtk.renderer.AddActor(self.axes)
        
        self.qvtk.renderer.AddActor(self.planes)
        self.qvtk.renderer.ResetCamera() 
        #for some reason, we have to do this afterwards!
        self.planes.togglePlanesOn()

 def __init__(self,ext_actors=None): #ext_actors is a list of any external vtkActors.
     #initializations:
     self.renderer  = vtk.vtkRenderer()
     self.window    = vtk.vtkRenderWindow()
     self.window.SetSize(1000,1000)
     self.mapper    = vtk.vtkPolyDataMapper()
     self.points    = vtk.vtkPoints()
     self.poly_data = vtk.vtkPolyData()
     self.glyph3d   = vtk.vtkGlyph3D()
     self.actor     = vtk.vtkActor()
     self.point_s   = vtk.vtkPointSource()
     self.sphere    = vtk.vtkSphereSource() 
     self.interactor= vtk.vtkRenderWindowInteractor()
     self.inter_sty = PdbInteractorStyle()
     self.axes_actor= vtk.vtkAxesActor()
     #configurations:
     self.point_s.SetNumberOfPoints(1)
     self.sphere.SetRadius(1.0)
     self.interactor.SetInteractorStyle(self.inter_sty)
     self.interactor.SetRenderWindow(self.window)
     self.axes_actor.SetTotalLength(100,100,100)
     if ext_actors:
         self.ex_actors = ext_actors
     else:
         self.ex_actors=[]

    def __init__(self, threadLock, actorWrapper, axis=True,):
        super(VTKVisualisation, self).__init__()

        self.threadLock = threadLock

        self.ren = vtk.vtkRenderer()
        self.ren.AddActor(actorWrapper.actor)

        self.axesActor = vtk.vtkAxesActor()
        self.axesActor.AxisLabelsOff()
        self.axesActor.SetTotalLength(1, 1, 1)
        self.ren.AddActor(self.axesActor)

        self.renWin = vtk.vtkRenderWindow()
        self.renWin.AddRenderer(self.ren)

        ## IREN
        self.iren = vtk.vtkRenderWindowInteractor()
        self.iren.SetRenderWindow(self.renWin)
        self.iren.Initialize()

        self.style = vtk.vtkInteractorStyleTrackballCamera()
        self.iren.SetInteractorStyle(self.style)

        self.iren.AddObserver("TimerEvent", self.update_visualisation)
        dt = 30 # ms
        timer_id = self.iren.CreateRepeatingTimer(dt)

 def adicionaeixos(self):
     axes = vtk.vtkAxesActor()
     self.marker = vtk.vtkOrientationMarkerWidget()
     self.marker.SetInteractor( self.Interactor )
     self.marker.SetOrientationMarker( axes )
     self.marker.SetViewport(0.75,0,1,0.25)
     self.marker.SetEnabled(1)

    def visualize(self):
        
        # a renderer and render window
        renderer = vtk.vtkRenderer()
        renderWindow = vtk.vtkRenderWindow()
        renderWindow.AddRenderer(renderer)
        
        # an interactor
        renderWindowInteractor = vtk.vtkRenderWindowInteractor() 
        renderWindowInteractor.SetRenderWindow(renderWindow)
        
        #add the actors to the scene
        renderer.AddActor(self.vtkActor)
        renderer.SetBackground(255, 255, 255) # Background white
#         renderer.SetBackground(0, 0, 0)
          
        transform = vtk.vtkTransform()
        transform.Translate(0, 0, 0)
         
        axes = vtk.vtkAxesActor()
        axes.SetTotalLength(50, 50, 50)
        axes.GetXAxisCaptionActor2D().GetCaptionTextProperty().SetFontSize(1)
        axes.GetYAxisCaptionActor2D().GetCaptionTextProperty().SetFontSize(1)
        axes.GetZAxisCaptionActor2D().GetCaptionTextProperty().SetFontSize(1)
        #  The axes are positioned with a user transform
        axes.SetUserTransform(transform)
        
#         renderer.AddActor(axes)
 
        renderer.ResetCamera()
        
        # Begin Interaction
        renderWindow.SetSize(400, 400)
        renderWindow.Render()
        renderWindowInteractor.Start()

def Visualize_Orientation(model, out_file):
    # create coordinate actor
    axes_actor = vtk.vtkAxesActor()
    axes_actor.SetTotalLength(500.0, 500.0, 500.0)

    # create model actor
    model_mapper = vtk.vtkPolyDataMapper()
    model_mapper.SetInputData(model)
    model_actor = vtk.vtkActor()
    model_actor.SetMapper(model_mapper)

    global renderer
    global interactor

    renderer = vtk.vtkRenderer()
    renderer.AddActor(model_actor)
    renderer.AddActor(axes_actor)
    win = vtk.vtkRenderWindow()
    win.AddRenderer(renderer)
    interactor = vtk.vtkRenderWindowInteractor()

    interactor_style = QIN_Style()
    interactor_style.SetRenderer(renderer)
    interactor_style.SetActor(model_actor)
    interactor_style.SetPolyData(model)
    interactor_style.SetOutName(out_file)
    interactor.SetInteractorStyle(interactor_style)

    win.SetInteractor(interactor)
    win.Render()
    interactor.Start()
    pass

    def __init__(self, cell):

        assert isinstance(cell, vtkUnitCellModule)
        self.cell = cell

        l0 = self.cell.get_characteristic_length()

        # Create VTK axes actor (not really a VTK actor though)
        self.vtk_ax = vtkAxesActor()
        self.vtk_ax.SetTipTypeToCone()
        self.vtk_ax.SetConeRadius(5e-2*l0)
        self.vtk_ax.SetShaftTypeToCylinder()
        self.vtk_ax.SetCylinderRadius(5e-3*l0)

        # Create VTK two-dimensional property
        p2d = vtkProperty2D()
        p2d.SetDisplayLocationToBackground()

        vtkModule.__init__(self, self.vtk_ax, p2d)

        # Create VTK text property and apply to axes
        vtk_textproperty = vtkTextProperty()
        vtk_textproperty.SetFontSize(14)
        vtk_textproperty.SetBold(True)
        vtk_textproperty.SetItalic(True)
        vtk_textproperty.SetShadow(True)
        vtk_textproperty.SetJustificationToRight()
        vtk_textproperty.SetVerticalJustificationToCentered()

        self.set_text_property(vtk_textproperty)

def initialize():

    # Create a rendering window and renderer
    transform = vtk.vtkTransform()
    transform.Scale(10.0, 10.0, 10.0)
     
    axes = vtk.vtkAxesActor()
    axes.SetUserTransform(transform)

    transform.Translate(3.0, -2.0, 0.0)
    axes.SetUserTransform(transform)

    ren = vtk.vtkRenderer()
    ren.AddActor(axes)
    ren.ResetCamera()

    renWin = vtk.vtkRenderWindow()
    renWin.AddRenderer(ren)

    # Create a RenderWindowInteractor to permit manipulating the camera
    iren = vtk.vtkRenderWindowInteractor()
    iren.SetRenderWindow(renWin)
    style = vtk.vtkInteractorStyleTrackballCamera()
    iren.SetInteractorStyle(style)

    # enable user interface interactor
    iren.Initialize()
    renWin.Render()
    iren.Start()

    return ren, iren, renWin

 def __init__(self, renderers):
     '''
     Initialize the axes - not the parent version, we're going to assign a vtkAxesActor to it and add it ourselves.
     '''
     # Skip the parent constructor
     #super(Axes,self).__init__(renderer)
     
     # Ref: http://vtk.org/gitweb?p=VTK.git;a=blob;f=Examples/GUI/Tcl/ProbeWithSplineWidget.tcl
     self.vtkActor = vtk.vtkAxesActor()
     self.vtkActor.SetShaftTypeToCylinder()
     self.vtkActor.SetCylinderRadius(0.05)
     self.vtkActor.SetTotalLength(2.5, 2.5, 2.5)
     # Change the font size to something reasonable
     # Ref: http://vtk.1045678.n5.nabble.com/VtkAxesActor-Problem-td4311250.html
     self.vtkActor.GetXAxisCaptionActor2D().GetTextActor().SetTextScaleMode(vtk.vtkTextActor.TEXT_SCALE_MODE_NONE)
     self.vtkActor.GetXAxisCaptionActor2D().GetTextActor().GetTextProperty().SetFontSize(25);
     self.vtkActor.GetYAxisCaptionActor2D().GetTextActor().SetTextScaleMode(vtk.vtkTextActor.TEXT_SCALE_MODE_NONE)
     self.vtkActor.GetYAxisCaptionActor2D().GetTextActor().GetTextProperty().SetFontSize(25);
     self.vtkActor.GetZAxisCaptionActor2D().GetTextActor().SetTextScaleMode(vtk.vtkTextActor.TEXT_SCALE_MODE_NONE)
     self.vtkActor.GetZAxisCaptionActor2D().GetTextActor().GetTextProperty().SetFontSize(25);         
     
     # Add the actor.
     for item in renderers:
         item.AddActor(self.vtkActor) 
     #renderer.AddActor(self.vtkActor)    

	def createAnnotations(self):
		# Add axes.
		self.axesActor = vtk.vtkAxesActor()
		self.axesActor.SetTotalLength(30,30,30)
		self.axesActor.SetShaftTypeToCylinder()
		self.axesActor.SetCylinderRadius(.05)
		self.axesActor.GetXAxisCaptionActor2D().GetTextActor().SetTextScaleModeToNone()
		self.axesActor.GetXAxisCaptionActor2D().GetCaptionTextProperty().SetFontSize(12)
		self.axesActor.GetXAxisCaptionActor2D().GetCaptionTextProperty().ItalicOff()
		self.axesActor.GetXAxisCaptionActor2D().GetCaptionTextProperty().BoldOff()
		self.axesActor.GetXAxisCaptionActor2D().GetCaptionTextProperty().ShadowOff()
		self.axesActor.GetYAxisCaptionActor2D().GetTextActor().SetTextScaleModeToNone()
		self.axesActor.GetYAxisCaptionActor2D().GetCaptionTextProperty().SetFontSize(12)
		self.axesActor.GetYAxisCaptionActor2D().GetCaptionTextProperty().ItalicOff()
		self.axesActor.GetYAxisCaptionActor2D().GetCaptionTextProperty().BoldOff()
		self.axesActor.GetYAxisCaptionActor2D().GetCaptionTextProperty().ShadowOff()
		self.axesActor.GetZAxisCaptionActor2D().GetTextActor().SetTextScaleModeToNone()
		self.axesActor.GetZAxisCaptionActor2D().GetCaptionTextProperty().SetFontSize(12)
		self.axesActor.GetZAxisCaptionActor2D().GetCaptionTextProperty().ItalicOff()
		self.axesActor.GetZAxisCaptionActor2D().GetCaptionTextProperty().BoldOff()
		self.axesActor.GetZAxisCaptionActor2D().GetCaptionTextProperty().ShadowOff()
		self.addActor(self.axesActor)
		
		# Add handling info.
		self.infoText = vtk.vtkTextActor()
		self.infoText.SetInput("Rotate:  Left mouse button\nPan:       Middle mouse button\nZoom:    Right mouse button")
		self.infoText.GetTextProperty()
		self.infoText.GetTextProperty().SetFontFamilyToArial()
		self.infoText.GetTextProperty().SetFontSize(11)
		self.infoText.GetTextProperty().SetColor(.6,.6,.6)
		self.infoText.SetDisplayPosition(20,30)
		self.addActor(self.infoText)

    def ShowOrientationCube(self):
        cube = vtk.vtkAnnotatedCubeActor()
        cube.GetXMinusFaceProperty().SetColor(1,0,0)
        cube.GetXPlusFaceProperty().SetColor(1,0,0)
        cube.GetYMinusFaceProperty().SetColor(0,1,0)
        cube.GetYPlusFaceProperty().SetColor(0,1,0)
        cube.GetZMinusFaceProperty().SetColor(0,0,1)
        cube.GetZPlusFaceProperty().SetColor(0,0,1)
        cube.GetTextEdgesProperty().SetColor(0,0,0)

        # anatomic labelling
        cube.SetXPlusFaceText ("A")
        cube.SetXMinusFaceText("P")
        cube.SetYPlusFaceText ("L")
        cube.SetYMinusFaceText("R")
        cube.SetZPlusFaceText ("S")
        cube.SetZMinusFaceText("I")

        axes = vtk.vtkAxesActor()
        axes.SetShaftTypeToCylinder()
        axes.SetTipTypeToCone()
        axes.SetXAxisLabelText("X")
        axes.SetYAxisLabelText("Y")
        axes.SetZAxisLabelText("Z")
        #axes.SetNormalizedLabelPosition(.5, .5, .5)

        orientation_widget = vtk.vtkOrientationMarkerWidget()
        orientation_widget.SetOrientationMarker(cube)
        orientation_widget.SetViewport(0.85,0.85,1.0,1.0)
        #orientation_widget.SetOrientationMarker(axes)
        orientation_widget.SetInteractor(self.interactor)
        orientation_widget.SetEnabled(1)
        orientation_widget.On()
        orientation_widget.InteractiveOff()

  def setupAxes(self):
    '''Add an vtkAxesActor to the renderer.'''
    bnd= self.gridRecord.getBND()
    offsetVector= bnd.diagonal*0.1
    offset= offsetVector.getModulo()
    axesPosition= bnd.pMin-offsetVector
    transform = vtk.vtkTransform()
    transform.Translate(axesPosition.x, axesPosition.y, axesPosition.z)
    axes= vtk.vtkAxesActor()
    #  The axes are positioned with a user transform
    axes.SetUserTransform(transform)

    length= offset
    axes.SetTotalLength(length,length,length)

    textSize= int(3*offset)
    axes.GetXAxisCaptionActor2D().GetTextActor().SetTextScaleMode(False)
    axes.GetXAxisCaptionActor2D().GetTextActor().GetTextProperty().SetFontSize(textSize)
    axes.GetYAxisCaptionActor2D().GetTextActor().SetTextScaleMode(False)
    axes.GetYAxisCaptionActor2D().GetTextActor().GetTextProperty().SetFontSize(textSize)
    axes.GetZAxisCaptionActor2D().GetTextActor().SetTextScaleMode(False)
    axes.GetZAxisCaptionActor2D().GetTextActor().GetTextProperty().SetFontSize(textSize)

    # properties of the axes labels can be set as follows
    # this sets the x axis label to red
    # axes.GetXAxisCaptionActor2D().GetCaptionTextProperty().SetColor(1,0,0)
 
    # the actual text of the axis label can be changed:
    # axes.SetXAxisLabelText("test")
    self.renderer.AddActor(axes)

    def __init__(self):

        # Skip the parent constructor
        # super(Axes, self).__init__(renderer)

        text_property_x = vtk.vtkTextProperty()
        text_property_x.ItalicOn()
        text_property_x.ShadowOn()
        text_property_x.BoldOn()
        text_property_x.SetFontFamilyToTimes()
        text_property_x.SetColor(1, 0, 0)

        text_property_y = vtk.vtkTextProperty()
        text_property_y.ShallowCopy(text_property_x)
        text_property_y.SetColor(0, 1, 0)

        text_property_z = vtk.vtkTextProperty()
        text_property_z.ShallowCopy(text_property_x)
        text_property_z.SetColor(0, 0, 1)

        self.axes = vtk.vtkAxesActor()
        self.axes.SetShaftTypeToCylinder()
        self.axes.SetCylinderRadius(0.05)
        self.axes.SetTotalLength(3 * [1.5])

        self.axes.GetXAxisCaptionActor2D().SetCaptionTextProperty(
            text_property_x)
        self.axes.GetYAxisCaptionActor2D().SetCaptionTextProperty(
            text_property_y)
        self.axes.GetZAxisCaptionActor2D().SetCaptionTextProperty(
            text_property_z)

        def renderthis(self):
            # open a window and create a renderer
            self.ren = vtk.vtkRenderer()
            self.widget.GetRenderWindow().AddRenderer(self.ren)
            
            # Background colour lightgrey
            self.ren.SetBackground(0.9,0.9,0.9)
            
            #create a X,Y,Z axes to show 3d position:
            # create axes variable and load vtk axes actor
            self.axes = vtk.vtkAxesActor()
            self.marker = vtk.vtkOrientationMarkerWidget()
            
            # set the interactor. self.widget._Iren is inbuilt python mechanism for current renderer window.
            self.marker.SetInteractor(self.widget._Iren )
            self.marker.SetOrientationMarker(self.axes )
            
            # set size and position of window (Xmin,Ymin,Xmax,Ymax)
            self.marker.SetViewport(0.75,0,1,0.25)
            
            #Allow user input
            self.marker.SetEnabled(1)

            # #settings for renderer window 
            self.ren.ResetCamera()
            self.ren.ResetCameraClippingRange()         
            self.isplotted = True
            self.p=0

    def renderthis(self):
            # open a window and create a renderer
            ren = vtk.vtkRenderer()
            self.widget.GetRenderWindow().AddRenderer(ren)
   
            # o take the polygonal data from the vtkConeSource and
            # create a rendering for the renderer.
            mapper = vtk.vtkPolyDataMapper()
            mapper.SetInputConnection(self.reader.GetOutputPort())

			#SetInputConnection(reader.GetOutputPort())
 
            # create an actor for our scene
            actor = vtk.vtkActor()
            actor.SetMapper(mapper)
            # Add actor
            ren.AddActor(actor)
 
            axes = vtk.vtkAxesActor()
            self.marker = vtk.vtkOrientationMarkerWidget()
            self.marker.SetInteractor( self.widget._Iren )
            self.marker.SetOrientationMarker( axes )
            self.marker.SetViewport(0.75,0,1,0.25)
            self.marker.SetEnabled(1)
 
            ren.ResetCamera()
            ren.ResetCameraClippingRange()
            cam = ren.GetActiveCamera()
            cam.Elevation(10)
            cam.Azimuth(70)
            self.isploted = True

def PlotGrids_wFEM(grids):
    """ Plots CFD structured grids with a single FEM """

    N = len(grids)
    HSV_tuples = [(x*1.0/N, 0.5, 0.5) for x in range(N)]
    RGB_tuples = map(lambda x: colorsys.hsv_to_rgb(*x), HSV_tuples)
    
    actors = []
    for i in range(len(grids)):
        
        # Create mapper
        mapper = vtk.vtkDataSetMapper()
        if vtk.vtkVersion().GetVTKMajorVersion() >5:
            mapper.SetInput(grids[i])
        else:
            mapper.SetInputData(grids[i])
        
        # Create actor
        actor = vtk.vtkActor()
        actor.SetMapper(mapper)
        actor.GetProperty().EdgeVisibilityOn()
        if i != 0:
            actor.GetProperty().SetRepresentationToWireframe()
            actor.GetProperty().SetColor(RGB_tuples[i])
        actor.GetProperty().LightingOff()
        actors.append(actor)
        
    # Add FEM Actor to renderer window
    ren = vtk.vtkRenderer()
#    ren.SetBackground(0.3, 0.3, 0.3)
    ren.SetBackground(0.8, 0.8, 0.8)
    renWin = vtk.vtkRenderWindow()
    renWin.AddRenderer(ren)
    iren = vtk.vtkRenderWindowInteractor()
    iren.SetRenderWindow(renWin)
    
    # Allow user to interact
    istyle = vtk.vtkInteractorStyleTrackballCamera()
    iren.SetInteractorStyle(istyle)
    

    
    # Add actor
    for actor in actors:
        ren.AddActor(actor)
    
    axes = vtk.vtkAxesActor()
    widget = vtk.vtkOrientationMarkerWidget()
#    widget.SetOutlineColor( 0.9300, 0.5700, 0.1300 )
    widget.SetOrientationMarker( axes )
    widget.SetInteractor( iren )
    widget.SetViewport( 0.0, 0.0, 0.4, 0.4 )
    widget.SetEnabled( 1 )
    widget.InteractiveOn()
    
    # Render
    iren.Initialize()
    renWin.Render()
    iren.Start()

def vtk_visualize(point_list, view_thresh):
    point_cloud = VtkPointCloud()
    x_thresh = view_thresh[0]
    y_thresh = view_thresh[1]
    z_thresh = view_thresh[2]

    for i in range(len(point_list)):
        point_coords = point_list[i]

        if (point_coords[0] > x_thresh[0]) and (point_coords[0] < x_thresh[1]) and \
                (point_coords[1] > y_thresh[0]) and (point_coords[1] < y_thresh[1]) and \
                (point_coords[2] > z_thresh[0]) and (point_coords[2] < z_thresh[1]):
            color_num = 0.7
        else:
            color_num = -1
        point_cloud.addPoint(point_list[i], color_num)

    # Add the velodyne plane
    for x in np.linspace(-4, 4, 100):
        for y in np.linspace(0, 2, 25):
            tmp_coords = np.array([x, 0, y])
            point_cloud.addPoint(tmp_coords, 1)
    # Add the floor plane
    plane_center = (-4, -4, -0.55)
    normal = (0, 0, 1)
    point1 = ([-4, 10, -0.55])
    point2 = ([4, -4, -0.55])
    point_cloud.addPlane(plane_center, normal, point1, point2)

    # Renderer
    renderer = vtk.vtkRenderer()
    renderer.AddActor(point_cloud.point_vtkActor)
    renderer.AddActor(point_cloud.plane_vtkActor)

    renderer.SetBackground(0.0, 0.0, 0.0)
    renderer.ResetCamera()

    # Render Window
    render_window = vtk.vtkRenderWindow()
    render_window.AddRenderer(renderer)

    # Interactor
    render_window_interactor = vtk.vtkRenderWindowInteractor()
    render_window_interactor.SetInteractorStyle(vtk.vtkInteractorStyleTrackballCamera())
    render_window_interactor.SetRenderWindow(render_window)

    '''Add camera coordinates'''
    axes = vtk.vtkAxesActor()
    widget = vtk.vtkOrientationMarkerWidget()
    widget.SetOutlineColor(0.9300, 0.5700, 0.1300)
    widget.SetOrientationMarker(axes)
    widget.SetInteractor(render_window_interactor)
    widget.SetViewport(0.0, 0.0, 0.4, 0.4)
    widget.SetEnabled(1)
    widget.InteractiveOn()
    render_window.Render()
    render_window_interactor.Start()

 def __init__(self):
     # ODE initialization
     self.world = ode.World()
     self.world.setGravity(GRAVITY)
     self.world.setERP(ERP)
     self.world.setCFM(CFM)
     self.space = ode.Space()
     self.floor = ode.GeomPlane(self.space, (0.0,1.0,0.0), 0.0)
     self.jointGroup = ode.JointGroup()
     self.time = 0.0
     # VTK initialization
     self.renderer = vtk.vtkRenderer()
     self.renderer.SetBackground(102.0/255.0,204/255.0,1.0)
     self.window = vtk.vtkRenderWindow()
     self.window.SetSize(WINDOW_WIDTH,WINDOW_HEIGHT)
     self.window.AddRenderer(self.renderer)
     self.interactor = vtk.vtkRenderWindowInteractor()
     self.interactor.SetRenderWindow(self.window)
     self.axes = vtk.vtkAxesActor()
     self.axes.SetAxisLabels(0)
     transform = vtk.vtkTransform()
     transform.Scale(0.1,0.1,0.1)
     self.axes.SetUserTransform(transform)
     self.renderer.AddActor(self.axes)
     # Create ground plane visualization
     self.floorVisual = vtk.vtkPlaneSource()
     self.floorVisual.SetNormal((0.0,1.0,0.0))
     self.floorVisual.SetResolution(10,10)
     self.floorReader = vtk.vtkJPEGReader()
     self.floorReader.SetFileName(FLOOR_IMAGE)
     self.floorTexture = vtk.vtkTexture()
     transform = vtk.vtkTransform()
     transform.Scale(50.0,50.0,50.0)
     self.floorTexture.SetTransform(transform)
     self.floorTexture.SetInput(self.floorReader.GetOutput())
     self.floorMap = vtk.vtkTextureMapToPlane()
     self.floorMap.SetInput(self.floorVisual.GetOutput())
     self.floorMapper = vtk.vtkPolyDataMapper()
     self.floorMapper.SetInput(self.floorMap.GetOutput())
     self.floorActor = vtk.vtkActor()
     transform = vtk.vtkTransform()
     transform.Scale(100.0,100.0,100.0)
     self.floorActor.SetUserTransform(transform)
     self.floorActor.SetMapper(self.floorMapper)
     self.floorActor.SetTexture(self.floorTexture)
     self.renderer.AddActor(self.floorActor)
     # VTK camera setup
     self.camera = vtk.vtkCamera()
     self.renderer.SetActiveCamera(self.camera)
     self.cameraFocus = [0.0, 0.0, 0.0]
     self.cameraPos = [4.0, 2.5, 1.5]
     self.cameraOffset = [0.0,1.0,3.0]
     self.cameraRoll = 0.0
     # Keep track of the simulated bodies and robots
     self.bodies = []
     self.robots = []
     self.controllers = []