def calculate(x, y, z, vx, vy, vz, dt, m, g, B2, S0, omega):
    """ Calculate the trajectory of a baseball including air resistance and spin by
repeatedly calling the do_time_step function.  Also draw the trajectory using visual python. """
    t = 0.0
    # Establish lists with initial position and velocity components and time.
    x_list = [x]
    y_list = [y]
    z_list = [z]
    vx_list = [vx]
    vy_list = [vy]
    vz_list = [vz]
    t_list = [t]

    # Set up visual elements.
    mound = visual.box(pos=(0,0,0), length=0.1, width=0.5, height=0.03, color=visual.color.white)
    plate = visual.box(pos=(18,0,0), length=0.5, width=0.5, height=0.03, color=visual.color.white)
    ball = visual.sphere(pos=(x,y,z), radius=0.05, color=visual.color.white)
    ball.trail = visual.curve(color=ball.color)

    while y >= 0.0:
        visual.rate(100) # Limit to no more than 100 iterations per second.
        t, x, y, z, vx, vy, vz = do_time_step(t, dt, x, y, z, vx, vy, vz, m, B2, g, S0, omega)
        x_list.append(x)
        y_list.append(y)
        z_list.append(z)
        vx_list.append(vx)
        vy_list.append(vy)
        vz_list.append(vz)
        t_list.append(t)
        ball.pos = (x,y,z)
        ball.trail.append(pos=ball.pos)

    return t_list, x_list, y_list, z_list, vx_list, vy_list, vz_list

def PlotSpheres3(f):
  data = json.loads(open(f, "r").read())

  scene = vs.display(title='3D representation',
    x=0, y=0, width=1920, height=1080,
    autocenter=True,background=(0,0,0))

  vs.box(pos=(
    (data["SystemSize"][1][0]+data["SystemSize"][0][0])*0.5,
    (data["SystemSize"][1][1]+data["SystemSize"][0][1])*0.5,
    (data["SystemSize"][1][2]+data["SystemSize"][0][2])*0.5
  ),
  length=data["SystemSize"][1][0]-data["SystemSize"][0][0],
  height=data["SystemSize"][1][1]-data["SystemSize"][0][1],
  width= data["SystemSize"][1][2]-data["SystemSize"][0][2],
  opacity=0.2,
  color=vs.color.red)

  spheres = [vs.sphere(radius=data["SphereSize"],pos=(data["Data"][0][i], data["Data"][1][i], data["Data"][2][i])) for i in range(data["SpheresNumber"])]

  vs.arrow(pos=data["SystemSize"][0], axis=(1,0,0), shaftwidth=0.1, color=vs.color.red)
  vs.arrow(pos=data["SystemSize"][0], axis=(0,1,0), shaftwidth=0.1, color=vs.color.green)
  vs.arrow(pos=data["SystemSize"][0], axis=(0,0,1), shaftwidth=0.1, color=vs.color.blue)

  while True:
    vs.rate(60)

    def cubic(self, latticeparameters=(1, 1, 1)):
        cube = visual.frame()

        visual.box(frame=cube
                   , pos=(0, 0, 0)
                   , size=latticeparameters
                   , color=(1, 0.7, 0.2))

        visual.arrow(frame=cube
                     , pos=(latticeparameters[0] / 2, 0, 0)
                     , axis=(1, 0, 0)
                     , shaftwidth=0.1
                     , color=(1, 0, 0))

        visual.arrow(frame=cube
                     , pos=(0, latticeparameters[0] / 2, 0)
                     , axis=(0, 1, 0)
                     , shaftwidth=0.1
                     , color=(0, 1, 0))

        visual.arrow(frame=cube
                     , pos=(0, 0, latticeparameters[0] / 2)
                     , axis=(0, 0, 1)
                     , shaftwidth=0.1
                     , color=(0, 0, 1))

        return cube

 def makeBoard(self):
     for i in range(8):
         for j in range(8):
             if (i+j) % 2 == 1:
                 sColor = vis.color.blue
             else: sColor = vis.color.white
             vis.box(frame = self.frame, pos=(i,-0.1,j),length=1,height=0.1,width=1,color=sColor)

 def run(self):
     '''
     Run the simulation with racers that had been previously added to the world
     by add_racer method.
     '''
     # create the scene with the plane at the top
     visual.scene.center = visual.vector(0,-25,0)
     visual.box(pos=(0,0,0), size=(12,0.2,12), color=visual.color.green)
     # create the visual objects that represent the racers (balls)
     balls = [ visual.sphere(pos=(index,0,0), radius=0.5) for index in xrange(len(self.racers))]
     for ball, racer in zip(balls, self.racers):
         color = visual.color.blue
         try:
             # try to set the color given by a string
             color = getattr(visual.color, racer.color)
         except AttributeError:
             pass
         ball.trail  = visual.curve(color=color)
     while not reduce(lambda x, y: x and y, [racer.result for racer in self.racers]):
         # slow down the looping - allow only self.time_calibration
         # number of loop entries for a second
         visual.rate(self.time_calibration)
         # move the racers
         for racer in self.racers:
             self.__move_racer(racer) 
             
         for ball, racer in zip(balls, self.racers):
             ball.pos.y = -racer.positions[-1][1]
             ball.pos.x = racer.positions[-1][0]
             ball.trail.append(pos=ball.pos)
             
         self.current_time += self.interval
         self.timeline.append(self.current_time)

	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 PlotSphereEvolution3(f):
  data = json.loads(open(f, "r").read())

  center = (
    (data["SystemSize"][1][0]+data["SystemSize"][0][0])*0.5,
    (data["SystemSize"][1][1]+data["SystemSize"][0][1])*0.5,
    (data["SystemSize"][1][2]+data["SystemSize"][0][2])*0.5
  )

  scene = vs.display(title='3D representation',
    x=0, y=0, width=1920, height=1080,
    center=center,background=(0,0,0)
  )

  vs.box(pos=center,
  length=data["SystemSize"][1][0]-data["SystemSize"][0][0],
  height=data["SystemSize"][1][1]-data["SystemSize"][0][1],
  width= data["SystemSize"][1][2]-data["SystemSize"][0][2],
  opacity=0.2,
  color=vs.color.red)

  spheres = [vs.sphere(radius=data["SphereSize"],pos=(data["Data"][0][0][i], data["Data"][1][0][i], data["Data"][2][0][i])) for i in range(data["SpheresNumber"])]

  nt = 0
  while True:
    vs.rate(60)
    for i in range(data["SpheresNumber"]):
      spheres[i].pos = (data["Data"][0][nt][i], data["Data"][1][nt][i], data["Data"][2][nt][i])
    if nt + 1 >= data["SavedSteps"]:
      nt = 0
    else:
      nt += 1

def drawCameraFrame():  # create frame and draw its contents
    global  cam_box, cent_plane,  cam_lab, cam_tri, range_lab, linelen, fwd_line
    global fwd_arrow, mouse_line, mouse_arrow, mouse_lab, fov, range_x, cam_dist, cam_frame
    global ray
    cam_frame = vs.frame( pos = vs.vector(0,2,2,),  axis = (0,0,1))
               # NB: contents are rel to this frame.  start with camera looking "forward"
               # origin is at simulated scene.center
    fov = vs.pi/3.0  # 60 deg 
    range_x = 6  # simulates scene.range.x  
    cam_dist = range_x / vs.tan(fov/2.0)  # distance between camera and center. 
    ray = vs.vector(-20.0, 2.5, 3.0).norm()  # (unit) direction of ray vector (arbitrary)
                                         #  REL TO CAMERA FRAME
    cam_box = vs.box(frame=cam_frame, length=1.5, height=1, width=1.0, color=clr.blue,
                                                   pos=(cam_dist,0,0)) # camera-box
    cent_plane = vs.box(frame=cam_frame, length=0.01, height=range_x*1.3, width=range_x*2,
                                                    pos=(0,0,0), opacity=0.5 )  # central plane
    cam_lab = vs.label(frame=cam_frame, text= 'U', pos= (cam_dist,0,0), height= 9, xoffset= 6)
    cam_tri = vs.faces( frame=cam_frame, pos=[(0,0,0), (0,0,-range_x), (cam_dist,0,0)])
    cam_tri.make_normals()
    cam_tri.make_twosided()
    range_lab = vs.label(frame=cam_frame, text= 'R', pos= (0, 0, -range_x), height= 9, xoffset= 6)
    linelen = scene_size + vs.mag( cam_frame.axis.norm()*cam_dist + cam_frame.pos)
                                                                   # len of lines from camera
    fwd_line = drawLine( vs.vector(cam_dist,0,0), linelen, vs.vector(-1,0,0))
    fwd_arrow = vs.arrow(frame=cam_frame, axis=(-2,0,0), pos=(cam_dist, 0, 0), shaftwidth=0.08,
                                                                            color=clr.yellow)
    vs.label(frame=cam_frame, text='C', pos=(0,0,0), height=9, xoffset=6, color=clr.yellow)
    mouse_line = drawLine ( vs.vector(cam_dist,0,0), linelen, ray ) 
    mouse_arrow = vs.arrow(frame=cam_frame, axis=ray*2, pos=(cam_dist,0,0), shaftwidth=0.08,
                                                                                   color=clr.red)
    mouse_lab = vs.label(frame=cam_frame, text= 'M', height= 9, xoffset= 10, color=clr.red, 
                                pos=  -ray*(cam_dist/vs.dot(ray,(1,0,0))) + (cam_dist,0,0))

    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 show_ladybug(self, H=None):
        '''Show Ladybug in Visual Python at origin or at the translated position
        if parameter is given.

        TODO: Implement an additional translation H and show at new position.'''
#        vis.ellipsoid(width=0.12, length=0.08, height=0.08,
#                      color=vis.color.red, opacity=0.2)
        vis.arrow(axis=(0.04, 0, 0), color=(1,0,0) )
        vis.arrow(axis=(0, 0.04, 0), color=(0,1,0) )
        vis.arrow(axis=(0, 0, 0.04), color=(0,0,1) )
        colors = [vis.color.red, vis.color.green, vis.color.blue,
                  vis.color.cyan, vis.color.yellow, vis.color.magenta]
        for P in self.LP:
            R = P[:3,:3]
            pos = dot(P[:3],r_[0,0,0,1])
            pos2 = dot(P[:3],r_[0,0,0.01,1])
            vis.sphere(pos=pos, radius=0.002, color=colors.pop(0))
            vis.box(pos=pos2, axis=dot(R, r_[0,0,1]).flatten(),
                    size=(0.001,0.07,0.09), color=vis.color.red,
                    opacity=0.1)
            vis.arrow(pos=pos,
                      axis=dot(R, r_[0.02,0,0]).flatten(),
                      color=(1,0,0), opacity=0.5 )
            vis.arrow(pos=pos,
                      axis=dot(R, r_[0,0.02,0]).flatten(),
                      color=(0,1,0), opacity=0.5 )
            vis.arrow(pos=pos,
                      axis=dot(R, r_[0,0,0.02]).flatten(),
                      color=(0,0,1), opacity=0.5 )

 def AddToQueue(self, ID, In):
     InOut = 1
     if In == True: InOut = 0
     priorSquare = 0
     if len(self.NodeList[ID][InOut]) > 0:
         priorSquare = self.NodeList[ID][InOut][-1].pos[1]
     newY = priorSquare + 1
     if In==1: box = v.box(pos=(ID*2+InOut, newY, 0), Length=0.25, Width=0.25, Height=0.25, color=v.color.green)
     else: box = v.box(pos=(ID*2+InOut, newY, 0), Length=0.25, Width=0.25, Height=0.25, color=v.color.red)
     self.NodeList[ID][InOut].append(box)

    def __init__(self):
        self._value = random() * 2 * pi
        self._model = v.frame()

        W = 0.1
        v.box(frame=self._model, pos=(+W / 2, 0, 0), size=(W, W, W), color=v.color.red)
        v.box(frame=self._model, pos=(-W / 2, 0, 0), size=(W, W, W),
            color=v.color.white)

        self._model.rotate(angle=self._value, axis=(0, 0, 1))

def visualBrick(brick, opacity = 0.7, offset = (0.0, 0.0, 0.0), material = None, color = visual.color.white):
    center_x = (brick.max_x + brick.min_x) / 2 + offset[0]
    center_y = (brick.max_y + brick.min_y) / 2 + offset[1]
    center_z = (brick.max_z + brick.min_z) / 2 + offset[2]
    size_x = brick.max_x - brick.min_x
    size_y = brick.max_y - brick.min_y
    size_z = brick.max_z - brick.min_z
    visual.box(pos = (center_x, center_y, center_z),
               length = size_x, height = size_y, width = size_z,
               color = color, opacity = opacity,
               material = material
        )

 def addFinitePlane(self, plane, colour=None, opacity=0.):
     if not Visualiser.VISUALISER_ON:
         return
     if isinstance(plane, geo.FinitePlane):
         if colour == None:
             colour = visual.color.blue
         # visual doesn't support planes, so we draw a very thin box
         H = .001
         pos = (plane.length/2, plane.width/2, H/2)
         pos = geo.transform_point(pos, plane.transform)
         size = (plane.length, plane.width, H)
         axis = geo.transform_direction((0,0,1), plane.transform)
         visual.box(pos=pos, size=size, color=colour, opacity=0)

 def addCSGvoxel(self, box, colour):
     """
     16/03/10: To visualise CSG objects
     """
        
     if colour == None:         
         colour = visual.color.red
         
     org = box.origin
     ext = box.extent
         
     size = np.abs(ext - org)
         
     pos = org + 0.5*size                      
         
     visual.box(pos=pos, size=size, color=colour, opacity=0.2)

def golf_ball_calc(x,y,z,vx,vy,vz,dt,m,g,B2,S0,w,w_vector):
    t = 0.0
    x_list = [x]
    y_list = [y]
    z_list = [z]
    vx_list = [vx]
    vy_list = [vy]
    vz_list = [vz]
    t_list = [t]
    v_vector = visual.vector(vx,vy,vz)

    tee = visual.box(pos=(0,0,0), length=0.05, width=0.05, height=0.5,color=visual.color.white)
    ball = visual.sphere(pos=(x,y,z), radius = 0.25, color = visual.color.white)
    ball.trail = visual.curve(color = visual.color.red)

    while y > 0.0:
        visual.rate(100)
        t,x,y,z,vx,vy,vz = golfball_step(t,x,y,z,vx,vy,vz,m,g,B2,S0,w,dt,w_vector,v_vector)
        x_list.append(x)
        y_list.append(y)
        z_list.append(z)
        vx_list.append(vx)
        vy_list.append(vy)
        vz_list.append(vz)
        t_list.append(t)
        v_vector = visual.vector(vx,vy,vz)
        ball.pos = (x,y,z)
        ball.trail.append(pos=ball.pos)

    return t_list,x_list,y_list,z_list,vx_list,vy_list,vz_list

def draw_world(world_data):
	"""
	Draws a world in 3D
	"""
	for obj in vpy.scene.objects:
		del obj

	box_width = WIDTH // len(world_data[0])
	box_length = LENGTH // len(world_data)

	for row_i, row in enumerate(world_data):
		for col_i, col in enumerate(row):
			color = terrain_key.get(row[col_i], None)
			if color is not None:
				vpy.box(pos=(col_i*box_width, 0, row_i*box_length),
					length=box_length, height=box_width, width=1, color=color)

 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 init_visual(self, only_wiiobj=False):
        visual_pos = ((0, 0, 0), (7,7,0), (14,14,0))
        if not only_wiiobj:
            #vpython
            visual.scene.width=self.param["width"] * 3
            visual.scene.height=self.param["height"]
            visual.scene.title = "3D WiiMote Simulation"
            visual.scene.forward = visual.vector(1,0,0) # not to error ?
            visual.scene.up = visual.vector(0,0,1)
            visual.scene.forward = visual.vector(-1,1,-1)
            visual.scene.center = (7,7,0)
            visual.scene.scale = (0.07, 0.07, 0.07)
            visual.scene.autoscale = 0
            visual.scene.x = 0
            visual.scene.y = 30
            
            self.visual["axis"] = [(visual.arrow(pos=visual_pos[i], color=visual.color.red, axis=(3,0,0), headwidth=1, shaftwidth=0.5, fixedwidth=1),
                                    visual.arrow(pos=visual_pos[i], color=visual.color.green, axis=(0,3,0), headwidth=1, shaftwidth=0.5, fixedwidth=1),
                                    visual.arrow(pos=visual_pos[i], color=visual.color.blue, axis=(0,0,3), headwidth=1, shaftwidth=0.5, fixedwidth=1))
                                   for i in xrange(3)]
            self.visual["text"] = [visual.label(text="accel", pos=(1, -1, -4), color=visual.color.white),
                                   visual.label(text="gyro", pos=(8, 6, -4), color=visual.color.white),
                                   visual.label(text="accel + gyro", pos=(15, 13, -4), color=visual.color.white),]
        self.visual["wiiobj"] = [visual.box(pos=visual_pos[i], length=2, height=6, width=1, color=visual.color.white, axis=(1,0,0)) #x=length, y=height, z=width
                                 for i in xrange(3)]

        return

def main():
    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 = Hexapod3D()

    remote = Gamepad(robot)

    GaitSequencer().start()
    remote.start()

    robot.setBodyPosition(z=30)

    robot.mainLoop()

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