def showPath(self):
        px, py, pz, dirx,diry,dirz = [],[],[],[],[],[]

        print self.path
        print len(self.z)

        for node in self.path:
            px.append(node[0])
            py.append(node[1])
            pz.append(self.z[node[0]][node[1]])
            dirx.append(cos(radians(node[2])))
            diry.append(sin(radians(node[2])))
            dirz.append(0)

        px = np.array(px)
        py = np.array(py)
        pz = np.array(pz)
        dirx = np.array(dirx)
        diry = np.array(diry)
        dirz = np.array(dirz)
        mlab.quiver3d(self.x,self.y,self.z,self.gx,self.gy,self.Gt,color=(1,0,0),scale_factor=1)
        mlab.quiver3d(px,py,pz,dirx,diry,dirz,color=(0,0,0),scale_factor=1)

        mlab.surf(self.x, self.y, self.z, representation='wireframe')
        mlab.show()

    def surface_orientation(surface, normals="triangles", name=None):
        """
        """
        fig = mlab.figure(figure=name, fgcolor=(0.5, 0.5, 0.5))
        surf_mesh = mlab.triangular_mesh(surface.vertices[:, 0],
                                         surface.vertices[:, 1],
                                         surface.vertices[:, 2],
                                         surface.triangles,
                                         color=(0.7, 0.67, 0.67),
                                         figure=fig)
        if normals == "triangles":
            surf_orient = mlab.quiver3d(surface.triangle_centres[:, 0],
                                        surface.triangle_centres[:, 1],
                                        surface.triangle_centres[:, 2],
                                        surface.triangle_normals[:, 0],
                                        surface.triangle_normals[:, 1],
                                        surface.triangle_normals[:, 2])
        elif normals == "vertices":
            surf_orient = mlab.quiver3d(surface.vertices[:, 0],
                                        surface.vertices[:, 1],
                                        surface.vertices[:, 2],
                                        surface.vertex_normals[:, 0],
                                        surface.vertex_normals[:, 1],
                                        surface.vertex_normals[:, 2])
        else:
            LOG.error("normals must be either 'triangles' or 'vertices'")

        return (surf_mesh, surf_orient)

def test_surface_normals(plot=False, skip_asserts=False,
                         write_reference=False):
    "Test the surface normals of a horseshoe mesh"
    sim = openmodes.Simulation()
    mesh = sim.load_mesh(osp.join(mesh_dir, 'horseshoe_rect.msh'))
    part = sim.place_part(mesh)
    basis = sim.basis_container[part]

    r, rho = basis.integration_points(mesh.nodes, triangle_centres)
    normals = mesh.surface_normals
    r = r.reshape((-1, 3))

    if write_reference:
        write_2d_real(osp.join(reference_dir, 'surface_r.txt'), r)
        write_2d_real(osp.join(reference_dir, 'surface_normals.txt'), normals)

    r_ref = read_2d_real(osp.join(reference_dir, 'surface_r.txt'))
    normals_ref = read_2d_real(osp.join(reference_dir, 'surface_normals.txt'))

    if not skip_asserts:
        assert_allclose(r, r_ref)
        assert_allclose(normals, normals_ref)

    if plot:
        from mayavi import mlab
        mlab.figure()
        mlab.quiver3d(r[:, 0], r[:, 1], r[:, 2],
                      normals[:, 0], normals[:, 1], normals[:, 2],
                      mode='cone')
        mlab.view(distance='auto')
        mlab.show()

    def render(self, scale_factor=1.0, text_scale=1.0, **kwargs):
        import mayavi.mlab as mlab
        # disabling the rendering greatly speeds up this for loop
        self.figure.scene.disable_render = True
        positions = []
        for label in self.lmark_group:
            p = self.lmark_group[label]
            for i, p in enumerate(p.points):
                positions.append(p)
                l = '%s_%d' % (label, i)
                # TODO: This is due to a bug in mayavi that won't allow
                # rendering text to an empty figure
                mlab.points3d(p[0], p[1], p[2], scale_factor=scale_factor)
                mlab.text3d(p[0], p[1], p[2], l, figure=self.figure,
                            scale=text_scale)
        positions = np.array(positions)
        os = np.zeros_like(positions)
        os[:, 2] = 1
        mlab.quiver3d(positions[:, 0], positions[:, 1], positions[:, 2],
                      os[:, 0], os[:, 1], os[:, 2], figure=self.figure)
        self.figure.scene.disable_render = False

        # Ensure everything fits inside the camera viewport
        mlab.get_engine().current_scene.scene.reset_zoom()

        return self

def plot_pos_ori(pos, ori, color=(0., 0., 0.)):
    mlab.points3d(pos[:, 0], pos[:, 1], pos[:, 2], scale_factor=0.005,
                  color=color)
    mlab.quiver3d(pos[:, 0], pos[:, 1], pos[:, 2],
                  ori[:, 0], ori[:, 1], ori[:, 2],
                  scale_factor=0.03,
                  color=color)

 def plot_channel(self,fn_hist,fn_corrvec,fn_arrows):
     list_channel = self.list_channel
     import matplotlib.pyplot as plt
     # plot vecs
     plt.figure()
     #vecs = np.array((3,len(list_channel)))
     for i in range(3):
         vecs = [chan.vec[i] for chan in list_channel]
         plt.hist(vecs,bins=100)
     plt.savefig(fn_hist)
     # plot corr vecs
     plt.figure()
     v0 = [chan.vec[0] for chan in list_channel]
     v1 = [chan.vec[1] for chan in list_channel]
     v2 = [chan.vec[2] for chan in list_channel]
     plt.plot(v0,v1,"o")
     plt.plot(v0,v2,"o")
     plt.plot(v1,v2,"o")
     plt.savefig(fn_corrvec)
     # cluster
     import matplotlib.pyplot as plt
     import mayavi.mlab as mlab
     #from mlab import quiver3d
     mlab.options.backend = 'envisage'         # one way to save visualization
     #f = mlab.figure()
     mlab.figure()
     x = [np.zeros(v.shape) for v in v0]
     mlab.quiver3d(x,x,x,v0,v1,v2)
     mlab.savefig(fn_arrows)

	def __init__(self, system):
		"""Constructor.
		**Arguments**

		:system: instance of espressomd.System
		"""
		self.system = system

		# objects drawn
		self.points = mlab.quiver3d([],[],[], [],[],[], scalars=[], mode="sphere", scale_factor=1, name="Particles")
		self.points.glyph.color_mode = 'color_by_scalar'
		self.points.glyph.glyph_source.glyph_source.center = [0, 0, 0]
		self.box = mlab.outline(extent=(0,0,0,0,0,0), color=(1,1,1), name="Box")
		self.arrows = mlab.quiver3d([],[],[], [],[],[], scalars=[], mode="2ddash", scale_factor=1, name="Bonds")
		self.arrows.glyph.color_mode = 'color_by_scalar'

		# state
		self.data = None
		self.last_N = 1
		self.last_Nbonds = 1
		self.last_boxl = [0,0,0]
		self.running = False
		self.last_T = None

		# GUI window
		self.gui = GUI()
		self.timers = [Timer(100, self._draw)]

def plotvfonsph3D(theta_rad, phi_rad, E_th, E_ph, freq=0.0,
                     vcoord='sph', projection='equirectangular'):
    PLOT3DTYPE = "quiver"
    (x, y, z) = sph2crtISO(theta_rad, phi_rad)
    from mayavi import mlab
    
    mlab.figure(1, bgcolor=(1, 1, 1), fgcolor=(0, 0, 0), size=(400, 300))
    mlab.clf()
    if PLOT3DTYPE == "MESH_RADIAL" :
        r_Et = numpy.abs(E_th)
        r_Etmx = numpy.amax(r_Et)
        mlab.mesh(r_Et*(x)-1*r_Etmx, r_Et*y, r_Et*z, scalars=r_Et)
        r_Ep = numpy.abs(E_ph)
        r_Epmx = numpy.amax(r_Ep)
        mlab.mesh(r_Ep*(x)+1*r_Epmx , r_Ep*y, r_Ep*z, scalars=r_Ep)
    elif PLOT3DTYPE == "quiver":
        ##Implement quiver plot
        s2cmat = getSph2CartTransfMatT(numpy.array([x,y,z]))
        E_r = numpy.zeros(E_th.shape)
        E_fldsph = numpy.rollaxis(numpy.array([E_r, E_ph, E_th]), 0, 3)[...,numpy.newaxis]
        E_fldcrt = numpy.rollaxis(numpy.matmul(s2cmat, E_fldsph).squeeze(), 2, 0)
        #print E_fldcrt.shape
        mlab.quiver3d(x+1.5, y, z,
                      numpy.real(E_fldcrt[0]),
                      numpy.real(E_fldcrt[1]),
                      numpy.real(E_fldcrt[2]))
        mlab.quiver3d(x-1.5, y, z,
                      numpy.imag(E_fldcrt[0]),
                      numpy.imag(E_fldcrt[1]),
                      numpy.imag(E_fldcrt[2]))              
    mlab.show()

def vector_field(x, y, z, u, v, w, name, constant_color):
    figure = mlab.figure(
        name,
        fgcolor=(0, 0, 0),
        bgcolor=(1, 1, 1)
    )

    mlab.quiver3d(x, y, z, u, v, w, line_width=3, scale_factor=1, figure=figure, color=constant_color)

def drawVector(origin,vector,scale=1,color=(1,0,0)):
    mlab.quiver3d(  
                origin[0], 
                origin[1], 
                origin[2], 
                vector[0], 
                vector[1], 
                vector[2], 
                scale_factor=scale, color=color)

 def draw_frame(self, pose, scale=10, label=''):
     R, t = pose
     scale = self.scale * scale
     clr = [RED, GREEN, BLUE]
     vecs = R[:, 0], R[:, 1], R[:, 2]
     for k in range(3):
         mlab.quiver3d(t[0], t[1], t[2], vecs[k][0], vecs[k][1], vecs[k][2],
                       color=clr[k], mode='arrow', scale_factor=scale)
     mlab.text3d(t[0], t[1], t[2], label, scale=scale/5)

 def draw_tools(self, points, frames, scale=10):
     scale = self.scale * scale
     vectors = []
     for frame in frames:
         vectors.append(frame[:, 2])
     vectors = np.array(vectors)
     points = points - scale * vectors
     mlab.quiver3d(points[:, 0], points[:, 1], points[:, 2],
                   vectors[:, 0], vectors[:, 1], vectors[:, 2],
                   color=(0, 0, 1), mode='arrow', scale_factor=scale)

 def render(self, colour='r', line_width=2, marker_style='2darrow',
            marker_resolution=8, marker_size=None, step=None, alpha=1.0):
     from mayavi import mlab
     marker_size = _parse_marker_size(marker_size, self.points)
     colour = _parse_colour(colour)
     mlab.quiver3d(self.points[:, 0], self.points[:, 1], self.points[:, 2],
                   self.vectors[:, 0], self.vectors[:, 1], self.vectors[:, 2],
                   figure=self.figure, color=colour, mask_points=step,
                   line_width=line_width, mode=marker_style,
                   resolution=marker_resolution, opacity=alpha,
                   scale_factor=marker_size)
     return self

    def visualize(self, obj, arrow_len=0.01, line_width=20.0):
        """ Display point grasp as arrows on the contact points of the mesh """
        contacts_found, contacts = self.close_fingers(obj)

        if contacts_found:
            c1_world = contacts[0].point
            c2_world = contacts[1].point
            v = c2_world - c1_world
            v = arrow_len * v / np.linalg.norm(v)
            mv.quiver3d(c1_world[0] - v[0], c1_world[1] - v[1], c1_world[2] - v[2], v[0], v[1], v[2], scale_factor=1.0,
                        mode='arrow', line_width=line_width)
            mv.quiver3d(c2_world[0] + v[0], c2_world[1] + v[1], c2_world[2] + v[2], -v[0], -v[1], -v[2], scale_factor=1.0,
                        mode='arrow', line_width=line_width)

def _plot_axes(lattice, color=(1, 0, 0)):
    lat = np.transpose([x/np.linalg.norm(x) for x in lattice.T])
    mlab.quiver3d([0, 0, 0],
                  [0, 0, 0],
                  [0, 0, 0],
                  lat[0],
                  lat[1],
                  lat[2],
                  color=color,
                  line_width=3,
                  scale_factor=1)

    for c, v in zip(('a','b','c'), (lat * 1.3).T):
        mlab.text3d(v[0]+0.15, v[1], v[2], c, color=color, scale=0.3)

 def showPc(s,figm=None,showNormals=False,algo='sobel',color=(1,0,0)):
   s.getPc()
   if figm is None:
     figm = mlab.figure(bgcolor=(1,1,1))
   mlab.points3d(s.pc[s.mask,0],s.pc[s.mask,1],s.pc[s.mask,2],
       s.gray[s.mask],colormap='gray',scale_factor=0.01,
       figure=figm,mode='point',mask_points=1)
   if showNormals:
     s.getNormals(algo)
     mlab.quiver3d(s.pc[s.mask,0],s.pc[s.mask,1],s.pc[s.mask,2],
         s.n[s.mask,0],s.n[s.mask,1],s.n[s.mask,2], 
         figure=figm, mode='2darrow',line_width=1.0, 
         color=color, scale_factor=0.1,mask_points=50)
   return figm

 def draw_path(self, path, color=(0.7, 0.5, 0.3)):
     points, vectors, processes = [], [], []
     for k in range(len(path)-1):
         points.append(path[k][0])
         vectors.append(path[k+1][0] - path[k][0])
         processes.append(path[k][2])
     points, vectors = np.array(points), np.array(vectors)
     processes = np.array(processes)
     pnts, vctrs = points[processes], vectors[processes]
     mlab.quiver3d(pnts[:, 0], pnts[:, 1], pnts[:, 2],
                   vctrs[:, 0], vctrs[:, 1], vctrs[:, 2],
                   color=color, mode='2ddash',
                   scale_factor=1, line_width=5.0)
     mlab.quiver3d(pnts[:, 0], pnts[:, 1], pnts[:, 2],
                   vctrs[:, 0], vctrs[:, 1], vctrs[:, 2],
                   color=color, mode='arrow',
                   scale_factor=3, scale_mode='scalar', line_width=5.0)
     pnts = points[np.bitwise_not(processes)]
     vctrs = vectors[np.bitwise_not(processes)]
     mlab.quiver3d(pnts[:, 0], pnts[:, 1], pnts[:, 2],
                   vctrs[:, 0], vctrs[:, 1], vctrs[:, 2],
                   color=(0.6, 0.6, 0.6), mode='2ddash',
                   scale_factor=1, line_width=2.0)
     mlab.quiver3d(pnts[:, 0], pnts[:, 1], pnts[:, 2],
                   vctrs[:, 0], vctrs[:, 1], vctrs[:, 2],
                   color=(0.6, 0.6, 0.6), mode='arrow',
                   scale_factor=2, scale_mode='scalar', line_width=2.0)

def drawReferenceFrameFromDCM(pos,dcm,size):
    # check Visual.Mframe
    ###############################################################################
    # draw x, y z axes
    # quiver = [x,y,z, u,v,w]
    
    v1 = np.dot(dcm,np.array([1,0,0]))
    v2 = np.dot(dcm,np.array([0,1,0]))
    v3 = np.dot(dcm,np.array([0,0,1]))
    
    pos = pos.tolist()
    a = mlab.quiver3d(pos[0], pos[1], pos[2],  v1[0], v1[1], v1[2], scale_factor=size, color=(1,0,0), mask_points=5)
    b = mlab.quiver3d(pos[0], pos[1], pos[2],  v2[0], v2[1], v2[2], scale_factor=size, color=(0,1,0), mask_points=5)
    c = mlab.quiver3d(pos[0], pos[1], pos[2],  v3[0], v3[1], v3[2], scale_factor=size, color=(0,0,1), mask_points=5)
    return [a,b,c]

 def draw_frames(self, points, frames, scale=10):
     scale = self.scale * scale
     clr = [RED, GREEN, BLUE]
     vectors = [[], [], []]
     for frame in frames:
         vectors[0].append(frame[:, 0])
         vectors[1].append(frame[:, 1])
         vectors[2].append(frame[:, 2])
     vectors[0] = np.array(vectors[0])
     vectors[1] = np.array(vectors[1])
     vectors[2] = np.array(vectors[2])
     for k in range(3):
         mlab.quiver3d(points[:, 0], points[:, 1], points[:, 2],
                       vectors[k][:, 0], vectors[k][:, 1], vectors[k][:, 2],
                       color=clr[k], mode='arrow', scale_factor=scale)

  def draw(self,**kwargs):
    x = np.linspace(self._base_square_x[0] + 0.5*(self._base_square_x[1]-self._base_square_x[0])/self._Nl,
                    self._base_square_x[1] - 0.5*(self._base_square_x[1]-self._base_square_x[0])/self._Nl,
                    self._Nl)
    y = np.linspace(self._base_square_y[0] + 0.5*(self._base_square_y[1]-self._base_square_y[0])/self._Nw,
                    self._base_square_y[1] - 0.5*(self._base_square_y[1]-self._base_square_y[0])/self._Nw,
                    self._Nw)
    x,y = np.meshgrid(x,y)
    z = 0.0*x
    for trans in self._transforms:
      p = np.concatenate((x[:,:,None],
                          y[:,:,None],
                          z[:,:,None]),
                          axis=-1)
      p = trans(p)
      pflat = np.reshape(p,(-1,3))
      x,y,z = pflat[:,0],pflat[:,1],pflat[:,2]
      value = self._f(pflat,*self._f_args,**self._f_kwargs)
      u,v,w = value[:,0],value[:,1],value[:,2]
      m = mlab.quiver3d(x,y,z,u,v,w,mode='arrow',color=(1.0,1.0,1.0),scale_factor=self.scale_units,
                        resolution=20,**kwargs)
      mlab.draw()
 

      if self._plots is None:
        self._plots = (m,trans),

      else:
        self._plots += (m,trans),

    return [i[0] for i in self._plots]