def __init__(self,num_of_disks,num_of_rods):
     self.max_disk_radius = 1.0
     self.disk_thickness = 0.2
     self.rod_height = self.disk_thickness * (num_of_disks + 1)
     self.disks = [visual.cylinder(radius=self.max_disk_radius*(i+2)/(num_of_disks+1),
                                   length=self.disk_thickness,
                                   axis=(0,0,1),
                                   color=(0.0,0.5,1.0),
                                   material=visual.materials.wood) \
                   for i in range(num_of_disks)]
     self.rods  = [visual.cylinder(radius=self.max_disk_radius*1.0/(num_of_disks+1),
                                   material=visual.materials.plastic,
                                   color=(1.0,0.5,0.3),
                                   length=self.rod_height,
                                   axis=(0,0,1)) for i in range(num_of_rods)]
     for i in range(num_of_rods):
         self.rods[i].pos.x = self.max_disk_radius*2*(i-(num_of_rods-1)*0.5)
     for i in range(num_of_disks):
         self.set_disk_pos(disk=i,rod=0,z_order=num_of_disks-i-1)
     self.base = visual.box(
         pos=(0,0,-self.disk_thickness*0.5),
         length=(num_of_rods+0.5)*self.max_disk_radius*2,
         width=self.disk_thickness,
         height=self.max_disk_radius*2.5,
         color=(0.2,1.0,0.2),
         material=visual.materials.wood)

 def frame(self, sign):
     self.glass = curve(
         pos=[
             (sign * self.time, -self.time * 1.1, 0),
             (sign * self.time * 0.05, -self.time * 0.1, 0),
             (sign * self.time * 0.05, self.time * 0.1, 0),
             (sign * self.time, self.time * 1.1, 0),
             (sign * self.time, self.time * 1.15, 0),
         ],
         radius=self.time * 0.025,
     )
     self.base = cylinder(
         pos=(0, sign * self.time * 1.15, 0),
         axis=(0, 1, 0),
         length=self.time * 0.1,
         radius=self.time * 1.2,
         color=(0.66, 0.46, 0.13),
     )
     self.pole = cylinder(
         pos=(sign * self.time, -self.time * 1.1, 0),
         axis=(0, 1, 0),
         length=self.time * 2.3,
         radius=self.time * 0.06,
         color=(0.66, 0.46, 0.13),
     )

def visualizeKvectors(SsfList):
    scene = visual.scene
    #visual.scene = visual.display()
    #scene = visual.scene
    #scene.exit = 0
    #scene.fov = .5 * scene.fov
    #scene.range = 7
    #scene.width, scene.height = 800, 800
    import saiga12.util
    avg = saiga12.util.Averager()
    for kmag in SsfList.kmags():
        Ssf = SsfList.SsfDict[kmag]
        for i, (kvec, Sk) in enumerate(
              zip(Ssf.kvecsOrig, Ssf.SkArraysByKvec())):
            avg.add(Sk)
            #print kvec, Sk

            widthfactor = .2
            col = (Sk-.25)*.25
            #col = Sk
            col = min(1., max(.1,  col))
            col = (col, col, col)
            radius = max(.05, (Sk)*widthfactor)
            #visual.arrow(pos=(0,0,0), axis=kvec,
            #             shaftwidth=Sk*widthfactor,
            #             fixedwidth=1)
            visual.cylinder(pos=(0,0,0), axis=kvec, color=col,
                            radius=radius)
    print "mean:", avg.mean
    return scene

    def __init__(self, joint, num, length, height=LEGS_HEIGHT, width=LEGS_WIDTH):
        """
        """
        super(Coxa3D, self).__init__(joint, num, length, height, width, (0, 1, 0))

        visual.cylinder(frame=self, pos=(0, -(height+5)/2, 0), radius=(height+1)/2, axis=(0, 1, 0), length=height+5, color=visual.color.cyan)
        visual.box(frame=self, pos=(length/2, 0, 0), length=length, height=height, width=width , color=visual.color.red)

 def setMap(self, board):        
     # clean the board
     for stone in self.stones: stone.visible = False
     self.stones = []
     
     for x in range(19):
         for y in range(19):
             if board[y + 1][x + 1] == 1:
                 stone = visual.cylinder(pos = (x, y, 0),
                                         axis = (0, 0, .1),
                                         radius = .4,
                                         color = visual.color.black,
                                         frame = self)
             elif board[y + 1][x + 1] == 2:
                 stone = visual.cylinder(pos = (x, y, 0),
                                         axis = (0, 0, .1),
                                         radius = .4,
                                         color = visual.color.white,
                                         frame = self)
             else: # 0
                 if(x + 1, y + 1) not in self.star:
                     stone = visual.cylinder(pos = (x, y, 0),
                                           axis = (0, 0, 0),
                                           radius = 0,
                                           color = visual.color.black,
                                           frame = self)
                 else:
                     stone = visual.cylinder(pos = (x, y, 0),
                                           axis = (0, 0, .01),
                                           radius = .2,
                                           color = (.5, .5, .5),
                                           frame = self)
             self.stones.append(stone)

    def addcar(self, pos, color=v.color.green, name="v"):
        # Creating car 
        car = v.frame()
        car.start = pos
        car.pos = car.start
        car.vector = v.vector(0.05, 0, 0) 
        car.color = color
        car.colorori = car.color

        body = v.box(frame = car, pos = (0,0,0), size = (2.4*self.thk, 0.6*self.thk, 1.4*self.thk), color = car.colorori) 
        wheel1 = v.cylinder(frame=car, pos=(0.8*self.thk,-0.2*self.thk,0.8*self.thk), axis=(0,0,-1.6*self.thk), radius=0.25*self.thk, color=(0.6,0.6,0.6)) 
        wheel2 = v.cylinder(frame=car, pos=(-0.8*self.thk,-0.2*self.thk,0.8*self.thk), axis=(0,0,-1.6*self.thk), radius=0.25*self.thk, color=(0.6,0.6,0.6)) 
        head = v.convex(frame=car, color=car.colorori)
        head.append(pos=v.vector(0.6, 0.3, -0.7)*self.thk)
        head.append(pos=v.vector(0.6, 0.3, 0.7)*self.thk)
        head.append(pos=v.vector(-1.2, 0.3, -0.7)*self.thk)
        head.append(pos=v.vector(-1.2, 0.3, 0.7)*self.thk)
        head.append(pos=v.vector(0.4, 0.7, -0.6)*self.thk)
        head.append(pos=v.vector(0.4, 0.7, 0.6)*self.thk)
        head.append(pos=v.vector(-0.8, 0.7, -0.6)*self.thk)
        head.append(pos=v.vector(-0.8, 0.7, 0.6)*self.thk)

        # Creating Label
        car.vlabel = v.label(justify='center', pos=car.pos, xoffset=3*self.thk, yoffset=39*self.thk, space=3*self.thk, text=name,height=15, line=0,border=3)

        self.cars[name] = car
        self.labels[name] = car.vlabel

	def __init__(self, room_=1, beam_axis_=0, target_pos_=(0,0,0)):
		self.labScene = visual.display(title="7Be(d,n)8B Experiment", width=800, height=600, background=GetRGBcode(153,204,255))
		axisx = visual.box(pos=(0,0,0), axis=(10.0,0,0), width=0.05, height=0.05, color=visual.color.red)
		axisy = visual.box(pos=(0,0,0), axis=(0,10.0,0), width=0.05, height=0.05, color=visual.color.blue)
		axisz = visual.box(pos=(0,0,0), axis=(0,0,10.0), width=0.05, height=0.05, color=visual.color.green)
		labelx = visual.label(pos=(5.0,0,0), text="Z-Axis")
		labely = visual.label(pos=(0,5.0,0), text="Y-Axis")
		labelz = visual.label(pos=(0,0,5.0), text="X-Axis")
		self.labScene.center = target_pos_
		self.labScene.autoscale = False
		
		self.room = room_
		self.beam_axis = beam_axis_
		self.target_pos = target_pos_
		
		self.Floors = []
		self.Walls = []
		self.Columns = []
		self.Others = []		

		self.BuildRoom()

		if(self.room == 1 or self.room == 2):
			chamber_radius = 0.25
			self.Beamline1 = visual.cylinder(pos=Translate(self.target_pos,GetCartesianCoords(chamber_radius, math.pi/2.0, DegToRad(180+self.beam_axis))), axis=ConvIM3(71.75,0,-71.75*math.tan(DegToRad(180-self.beam_axis))), radius=ConvIM(1.75), color=visual.color.blue) # East beamline
			self.Beamline2 = visual.cylinder(pos=Translate(self.target_pos,GetCartesianCoords(chamber_radius, math.pi/2.0, DegToRad(self.beam_axis))), axis=ConvIM3(-217.5,0,217.5*math.tan(DegToRad(180-self.beam_axis))), radius=ConvIM(1.75), color=visual.color.blue) # West beamline
			self.OneMeterChamber = visual.cylinder(pos=self.target_pos, axis=(0,chamber_radius*2,0), radius=chamber_radius, color=visual.color.blue)
			self.OneMeterChamber.pos[1] = -0.5

  def draw(self):
    import visual

    visual.scene.background = (0.6, 0.6, 0.6)
    """visual.cylinder(pos=(0, 0, 0),
             axis=(1000, 0, 0),
             radius=2,
             color=visual.color.red)
    visual.cylinder(pos=(0, 0, 0),
             axis=(0, 1000, 0),
             radius=2,
             color=visual.color.green)
    visual.cylinder(pos=(0, 0, 0),
             axis=(0, 0, 1000),
             radius=2,
             color=visual.color.blue)"""
    visual.cylinder(pos=(0, 0, -0.5 * self.cell_height),
                    axis=(0, 0, self.cell_height),
                    radius=self.cell_radius,
                    color=visual.color.blue,
                    opacity=0.25)
    #visual.sphere(pos=(0, 0, 0), radius=50, color=visual.color.green)
    for tube in self.__tubes:
      for i in range(len(tube) - 1):
        p1, p2 = [ (p[2], p[1], p[0]) for p in tube[i:i+2] ]
        direction = [p2[i] - p1[i] for i in range(3)]
        visual.cylinder(pos=p1,
                        axis=direction,
                        radius=self.tube_radius,
                        color=visual.color.red)

 def addLine(self, start, stop, colour=None):
     if not Visualiser.VISUALISER_ON:
         return
     if colour == None:
         colour = visual.color.white
     axis = np.array(stop) - np.array(start)
     visual.cylinder(pos=start, axis=axis, radius=0.0001, color=geo.norm(colour))

 def addRay(self, ray, colour=None):
     if not Visualiser.VISUALISER_ON:
         return
     if isinstance(ray, geo.Ray):
         if colour == None:
             colour = visual.color.white
         pos = ray.position
         axis = ray.direction * 5
         visual.cylinder(pos=pos, axis=axis, radius=0.0001, color=geo.norm(colour))

def unitcell_init(a,scale):
	radius = 0.01*scale
	#initialise variable to return
	components = []
	#draw the cylinders (one for each vector at the origin, two of each other vector from the tip of each vector, one of each vector from the sum of each pair of vectors)
	for i in range(3):
		components.append(v.cylinder(pos=(0,0,0), axis=(a[i][0],a[i][1],a[i][2]), radius=radius))
		components.append(v.cylinder(pos=(a[(i+1)%3][0],a[(i+1)%3][1],a[(i+1)%3][2]), axis=(a[i][0],a[i][1],a[i][2]), radius=radius))
		components.append(v.cylinder(pos=(a[(i+2)%3][0],a[(i+2)%3][1],a[(i+2)%3][2]), axis=(a[i][0],a[i][1],a[i][2]), radius=radius))
		components.append(v.cylinder(pos=(a[(i+1)%3][0]+a[(i+2)%3][0],a[(i+1)%3][1]+a[(i+2)%3][1],a[(i+1)%3][2]+a[(i+2)%3][2]), axis=(a[i][0],a[i][1],a[i][2]), radius=radius))
	return components

 def addCylinder(self, cylinder, colour=None):
     if not Visualiser.VISUALISER_ON:
         return
     if colour == None:
         colour = visual.color.blue
     #angle, direction, point = tf.rotation_from_matrix(cylinder.transform)
     #axis = direction * cylinder.length
     position = geo.transform_point([0,0,0], cylinder.transform)
     axis = geo.transform_direction([0,0,1], cylinder.transform)
     print cylinder.transform, "Cylinder:transform"
     print position, "Cylinder:position"
     print axis, "Cylinder:axis"
     print colour, "Cylinder:colour"
     print cylinder.radius, "Cylinder:radius"
     visual.cylinder(pos=position, axis=axis, color=colour, radius=cylinder.radius, opacity=0.5, length = cylinder.length)

def form_bonds(atoms, dis):
    bonds = []
    for a in atoms:
        for b in atoms:
            if abs(a.position - b.position)<dis:
                bonds.append(visual.cylinder(pos=a.position,axis=(b.position-a.position),radius=0.05,color=(1,0.7,0.2),opacity=0.45))
    return bonds

 def sand(self):
     self.top = cone(pos=(0, self.time * 1.1, 0), axis=(0, -1, 0), color=(1, 1, 0))
     self.bottom = cone(pos=(0, -self.time * 1.1, 0), axis=(0, 1, 0), color=(0.5, 0.5, 0))
     self.falling = cylinder(
         pos=(0, self.time * 0.125, 0), axis=(0, -1, 0), color=(0.5, 0.5, 0), radius=self.time * 0.035
     )
     self.countdown = sphere(pos=(0, self.time * 1.5, 0), radius=0)

 def draw_edges(self):
     self.rods = []
     self.line_handle = []
     if not self.connect==None:
         self.I, self.J = np.nonzero(self.connect)
         for i, j in zip(self.I, self.J):
             pos, axis = self.pos_axis(i, j)
             self.rods.append(visual.cylinder(pos=pos, axis=axis, radius=self.radius))

def main():

    # Init some 3D stuff
    scene = visual.display(width=settings.SCENE_WIDTH, height=settings.SCENE_HEIGHT)
    ground = visual.box(axis=(0, 1, 0), pos=(0, 0, 0), length=0.1, height=500, width=500, color=visual.color.gray(0.75), opacity=0.5)

    def addGait(gaitClass, gaitName):
        gait = gaitClass(gaitName, settings.GAIT_LEGS_GROUPS[gaitName], settings.GAIT_PARAMS[gaitName])
        GaitManager().add(gait)

    addGait(GaitTripod, "tripod")
    addGait(GaitTetrapod, "tetrapod")
    addGait(GaitRiple, "riple")
    addGait(GaitWave, "metachronal")
    addGait(GaitWave, "wave")
    #GaitManager().select("riple")

    robot = Hexapod()

    # Init some more 3D stuff (opt.)
    CoordinatesSystem3D()
    for legIndex, footNeutralP in robot.feetNeutralP.items():
        visual.sphere(pos=(footNeutralP[0], footNeutralP[2], -footNeutralP[1]), radius=5)
        visual.cylinder(pos=(footNeutralP[0], footNeutralP[2], -footNeutralP[1]), axis=(0, 0, -1), radius=1, length=50)
        visual.cylinder(pos=(footNeutralP[0], footNeutralP[2], -footNeutralP[1]), axis=(0, 0, 1), radius=1, length=50)
        if legIndex == 'RR':
            CoordinatesSystem3D(pos=(footNeutralP[0], footNeutralP[2], -footNeutralP[1]))

    GaitSequencer().start()

    try:
        gamepad = Gamepad(robot)
        gamepad.start()
    except OSError:
        Logger().exception("main()", debug=True)
        keyboard = Keyboard(robot)
        keyboard.start()

    robot.setBodyPosition(z=40)

    robot.mainLoop()

    remote.stop()
    remote.join()
    GaitSequencer().stop()
    GaitSequencer().join()

    def addCylinder(self, cylinder, colour=None, opacity=1., material=None):
        if not VISUAL_INSTALLED:
            return
        if colour is None:
            colour = visual.color.blue

        # angle, direction, point = tf.rotation_from_matrix(cylinder.transform)
        # axis = direction * cylinder.length
        position = Geo.transform_point([0, 0, 0], cylinder.transform)
        axis = Geo.transform_direction([0, 0, 1], cylinder.transform)
        # print cylinder.transform, "Cylinder:transform"
        # print position, "Cylinder:position"
        # print axis, "Cylinder:axis"
        # print colour, "Cylinder:colour"
        # print cylinder.radius, "Cylinder:radius"
        visual.cylinder(pos=position, axis=axis, color=colour, radius=cylinder.radius, opacity=opacity,
                        length=cylinder.length, material=material)

def my_arrow(pos, axis, proportion=True, shaftwidth=0.1, headwidth=2, headlength=3, color=[1.0, 1.0, 1.0]):
    px, py, pz = pos
    x, y, z = axis
    sw = shaftwidth
    length = math.sqrt(x * x + y * y + z * z)
    if proportion:
        sw *= length
    hl = sw * headlength / length
    l = 1.0 - hl
    lx = l * x
    ly = l * y
    lz = l * z
    cylinder_axis = [lx, ly, lz]
    cone_pos = [px + lx, py + ly, pz + lz]
    cone_axis = [hl * x, hl * y, hl * z]
    cone_radius = sw * headwidth
    visual.cylinder(pos=pos, axis=cylinder_axis, radius=sw, color=color)
    visual.cone(pos=cone_pos, axis=cone_axis, radius=cone_radius, color=color)

def display(cylinders,Lx,Ly):
    scene = visual.display(center = [Ly/2.0,.14,Lx/2.0], forward = [0,-1,0],
            up = [1,0,0],background=(1,1,1))
    surr = visual.curve(pos=[(0,0,0),(0,0,Lx),(Ly,0,Lx),(Ly,0,0),(0,0,0)],
            radius=.005*Lx,color=(0,0,0))
    for cyl in cylinders:
        x = cyl.pos[0]
        y = cyl.pos[1]
        z = 0
        rod = visual.cylinder(pos=(y,z,x), axis=(0,.001,0), radius = cyl.radius,
                color=(200,0,0))

def Draw(r, init):
	Error = ((NetEnergy(r) - Energy0)/Energy0)
	if init == 1:	#Initializes the display window
		global pend0, pend1, pend2, rod1, rod2, ErrDisplay, scene, pend, rod
		scene = display(width = 800, height = 800)
		pend = [sphere(pos=(0,0,0), radius = .1*(l_0[0] + l_0[1]))]
		rod = [cylinder(pos = (r[0,0,0],r[0,0,1],0), axis = (-r[0,0,0], r[0,0,1], 0), radius = .025*(l_0[0] + l_0[1]))]
		for i in range(shape(r)[0]):
			pend.append(sphere(pos = (r[i,0,0],r[i,0,1],0), radius = .1*(l_0[0] + l_0[1])))
		for i in range(1, shape(r)[0]):
			rod.append(cylinder(pos = (r[i,0,0],r[i,0,1],0), axis = (r[i-1,0,0]-r[i,0,0], r[i-1,0,1]-r[i,0,1],0), radius = .025*(l_0[0] + l_0[1])))
		ErrDisplay = label(pos = (0, l_0[0] + l_0[1] + 2,0), text = "Error Percent: %f " % Error)
	else:	#Redraw for steps
		for i in range(shape(r)[0]):
			pend[i+1].pos = (r[i,0,0],r[i,0,1],0)
		rod[0].axis = (-r[0,0,0], -r[0,0,1],0); rod[0].pos = (r[0,0,0], r[0,0,1], 0)
		for i in range(1, shape(r)[0]):
			rod[i].pos = (r[i,0,0], r[i,0,1], 0)
			rod[i].axis = (r[i-1,0,0]-r[i,0,0], r[i-1,0,1]-r[i,0,1],0)
		ErrDisplay.text = "Error Percent: %f " % Error