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 setQuaternion(self, Quaternion):
        """Set the orientation of the body in world coordinates (the display object is oriented to match)."""
        odelib.BaseBody.setQuaternion(self, Quaternion)

        # now set the orientation of the display frame
        # Rotating a point is q * (0,v) * q-1
        # q-1 is w, -x, -y, -z assuming that q is a unit quaternion
        w1, x1, y1, z1 = Quaternion
        v1 = visual.vector(x1, y1, z1)
        w3 = w1
        v3 = -v1

        # First do the axis vector
        w2 = 0.0
        v2 = visual.vector((1.0, 0.0, 0.0))

        # This code is equivalent to a quaternion multiply: qR = q1 * q2 * q3
        w12 = (w1 * w2) - visual.dot(v1, v2)
        v12 = (w1 * v2) + (w2 * v1) + visual.cross(v1, v2)
        wR = (w12 * w3) - visual.dot(v12, v3)
        vR = (w12 * v3) + (w3 * v12) + visual.cross(v12, v3)

        self._myFrame.axis = vR

        # Do it again for the up vector
        w2 = 0.0
        v2 = visual.vector((0.0, 1.0, 0.0))

        # This code is equivalent to a quaternion multiply: qR = q1 * q2 * q3
        w12 = (w1 * w2) - visual.dot(v1, v2)
        v12 = (w1 * v2) + (w2 * v1) + visual.cross(v1, v2)
        wR = (w12 * w3) - visual.dot(v12, v3)
        vR = (w12 * v3) + (w3 * v12) + visual.cross(v12, v3)

        self._myFrame.up = vR

 def __init__( self, NCellsPerDimension, Length, E0 ):
     self.n   = NCellsPerDimension
     self.L   = Length
     self.V   = self.L**3
     self.N   = 4 * self.n**3
     self.a   = self.L / self.n
     self.E0  = E0
     self.E   = E0
     self.U   = 0
     self.dU  = 0
     self.ddU = 0
     self.particles = []
     self.index = 0
     self.dt    = 1e-2
     self.dt_2  = self.dt * 0.5
     self.dt2_2 = self.dt * self.dt_2
 
     self.scene = visual.display( title = 'System',
                                  x=800, y=0,
                                  width=800, height=800,
                                  center = visual.vector(0.5,0.5,0.5) * self.L,
                                  autoscale = False,
                                  range = 1.5 * self.L)
 
     self.box   = visual.box( pos     = visual.vector(0.5,0.5,0.5) * self.L,
                              length  = self.L,
                              height  = self.L,
                              width   = self.L,
                              color   = visual.color.green,
                              opacity = 0.2 )

    def update(self, t, quantity=1):
        try:

            threshold = self.interval * self.curMarker
            # Display new arrow if t has broken new threshold
            if t >= threshold:

                # Increment marker count
                self.curMarker += 1
                
                # Display marker!
                if self.markerType == "arrow":
                    arrow(pos=self.obj.pos+self.arrowOffset, 
                        axis=self.markerScale*quantity, color=self.markerColor)
                elif self.markerType == "breadcrumbs":
                    points(pos=self.obj.pos, 
                        size=10*self.markerScale*quantity, color=self.markerColor)

                #Also display timestamp if requested
                if self.dropTime is not False:
                    epsilon = vector(0,self.markerScale*.5,0)+self.timeOffset
                    droptimeText = label(pos=self.obj.pos+epsilon, text='t='+str(t)+'s', height=10, box=False, color=self.labelColor)

                # Same with order label
                if self.labelMarkerOrder is not False:
                    label(pos=self.obj.pos-vector(0,self.markerScale*.5,0)+self.labelMarkerOffset, text=str(self.curMarker), height=10, box=False, color=self.labelColor)
                
        except TypeError as err:
            print("**********TYPE ERROR**********")
            print("Please check that you are not passing in a variable of the wrong type (e.g. a scalar as a vector, or vice-versa)!")
            print("******************************")
            print(err)
            raise err

    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 reorient(self, axis=None, startPos=None, length=None, labels=None, labelOrientation=None):
        try:
            # Determine which, if any, parameters are being modified
            self.axis = axis if axis is not None else self.axis
            self.startPos = startPos if startPos is not None else self.startPos
            self.length = length if length is not None else self.length
            self.labelText = labels if labels is not None else self.labels

            # Re-do label orientation as well, if it has been set
            if labelOrientation == "down":
                self.labelShift = vector(0,-0.05*self.length,0)
            elif labelOrientation == "up":
                self.labelShift = vector(0,0.05*self.length,0)
            elif labelOrientation == "left":
                self.labelShift = vector(-0.1*self.length,0,0)
            elif labelOrientation == "right":
                self.labelShift =  vector(0.1*self.length,0,0)

            self.__reorient()
        except TypeError as err:
            print("**********TYPE ERROR**********")
            print("Please check that you are not passing in a variable of the wrong type (e.g. a scalar as a vector, or vice-versa)!")
            print("******************************")
            print(err)
            raise err

    def test_update(self):
        startMarkerPos = vector(
            self.physAxis.intervalMarkers[-1].pos.x,
            self.physAxis.intervalMarkers[-1].pos.y,
            self.physAxis.intervalMarkers[-1].pos.z,
        )
        startLabelPos = vector(
            self.physAxis.intervalLabels[-1].pos.x,
            self.physAxis.intervalLabels[-1].pos.y,
            self.physAxis.intervalLabels[-1].pos.z,
        )
        startCurvePos = vector(
            self.physAxis.axisCurve.pos[0].x, self.physAxis.axisCurve.pos[0].y, self.physAxis.axisCurve.pos[0].z
        )

        self.physAxis.update()

        self.assertEqual(startMarkerPos, self.physAxis.intervalMarkers[-1].pos)
        self.assertEqual(startLabelPos, self.physAxis.intervalLabels[-1].pos)
        self.assertEqual(startCurvePos, self.physAxis.axisCurve.pos[0])

        self.physAxis.obj.pos = self.physAxis.obj.pos + vector(1, 1, 1)
        self.physAxis.update()

        self.assertNotEqual(startMarkerPos, self.physAxis.intervalMarkers[-1].pos)
        self.assertNotEqual(startLabelPos, self.physAxis.intervalLabels[-1].pos)
        self.assertNotEqual(startCurvePos, self.physAxis.axisCurve.pos[0])

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 init_visual(self, only_wiiobj=False):
        if not only_wiiobj:
            # vpython
            visual.scene.width = self.param["width"]
            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.autoscale = 0
            visual.scene.x = 0
            visual.scene.y = 30

            self.visual["axis"][0] = visual.arrow(
                color=visual.color.red, axis=(3, 0, 0), headwidth=1, shaftwidth=0.5, fixedwidth=1
            )
            self.visual["axis"][1] = visual.arrow(
                color=visual.color.green, axis=(0, 3, 0), headwidth=1, shaftwidth=0.5, fixedwidth=1
            )
            self.visual["axis"][2] = visual.arrow(
                color=visual.color.blue, axis=(0, 0, 3), headwidth=1, shaftwidth=0.5, fixedwidth=1
            )
        self.visual["wiiobj"] = visual.box(
            pos=(0, 0, 0), length=2, height=6, width=1, color=visual.color.white, axis=(2, 0, 0)
        )

        return

def axes( frame, colour, sz, posn ): # Make axes visible (of world or frame).
                                     # Use None for world.   
    directions = [vs.vector(sz,0,0), vs.vector(0,sz,0), vs.vector(0,0,sz)]
    texts = ["X","Y","Z"]
    posn = vs.vector(posn)
    for i in range (3): # EACH DIRECTION
       vs.curve( frame = frame, color = colour, pos= [ posn, posn+directions[i]])
       vs.label( frame = frame,color = colour,  text = texts[i], pos = posn+ directions[i],
                                                                    opacity = 0, box = False )

    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,s,k,m):
     '''
     Abstract Robot constructor
     '''
     self.sensor=s
     self.kinematic=k
     self.map=m
     self.pos=vector(0.0,0.0)
     self.orientation=0.0
     self.v=vector(0.0,0.0,0.0)
     self.w=vector(0.0,0.0,0.0)

 def test_init(self):
     self.assertEqual(self.obj, self.map.obj)
     self.assertEqual(self.dt, self.map.dt)
     self.assertEqual(self.numSteps, self.map.numSteps)
     self.assertEqual("arrow", self.map.markerType)
     self.assertEqual(self.markerScale, self.map.markerScale)
     self.assertEqual(color.green, self.map.markerColor)
     self.assertEqual(vector(1,1,1), self.map.timeOffset)
     self.assertEqual(True, self.map.dropTime)
     self.assertEqual(self.map.curMarker, 0)
     self.assertEqual(vector(1,1,1), self.map.arrowOffset)

    def test_plot(self):
        self.assertRaises(Exception, self.physGraph.plot, vector(0, 0, 0), vector(1, 1, 1))

        self.physGraph.plot(
            vector(0, 0, 0), vector(1, 1, 1), vector(2, 2, 2), vector(3, 3, 3), vector(4, 4, 4), vector(5, 5, 5)
        )
        self.assertEqual(len(self.physGraph.graphs[-1].plots), 5)
        self.assertEqual(self.physGraph.graphs[-1].plots[0], (vector(0, 0, 0), vector(1, 1, 1)))

    def __init__(
        self,
        obj,
        dt,
        numSteps,
        markerType="arrow",
        markerScale=1,
        markerColor=color.red,
        labelMarkerOrder=True,
        labelMarkerOffset=vector(0, 0, 0),
        dropTime=False,
        timeOffset=vector(0, 0, 0),
        arrowOffset=vector(0, 0, 0),
        labelColor=color.white,
    ):
        # MotionMapN
        # obj - object to track in mapping / placing markers
        # dt - time between steps
        # numSteps - number of steps between markers
        # markerType - determines type of motionmap; options are "arrow" or "breadcrumbs"
        # markerScale - replaces pSize / quantscale from motionmodule.py depending on type
        # markerColor - color of markers
        # labelMarkerOrder - drop numbers of markers?
        # labelMarkerOffset - amount to offset numbering by
        # dropTime - boolean determining whether a timestamp should be placed along with the marker
        # timeOffset - if dropTime is True, determines the offset, if any, of the label from the markers
        # arrowOffset - shift an arrow by an amount (x,y,z), useful for two arrows views

        self.obj = obj
        self.dt = dt
        self.numSteps = numSteps
        self.markerType = markerType
        self.markerScale = markerScale
        self.markerColor = markerColor
        self.labelMarkerOrder = labelMarkerOrder
        self.labelMarkerOffset = labelMarkerOffset
        self.timeOffset = timeOffset
        self.dropTime = dropTime
        self.arrowOffset = arrowOffset
        self.labelColor = labelColor

        # Calculate size of interval for each step, set initial step index
        try:
            self.interval = self.dt * self.numSteps
        except TypeError as err:
            print("**********TYPE ERROR**********")
            print(
                "Please check that you are not passing in a variable of the wrong type (e.g. a scalar as a vector, or vice-versa)!"
            )
            print("******************************")
            print(err)
            raise err
        self.curMarker = 0

def getSegments(p):
    seg=[]
    for i,pt in enumerate(p):
        if i<len(p)-1:
            x0=pt[0]
            y0=pt[2]
            x1=p[i+1][0]
            y1=p[i+1][2]
            v0=vector(x0,y0)
            v1=vector(x1,y1)
            s=Segment(v0,v1)
            seg.append(s)
    return seg

    def drawScene(self):
        # draw the frame of world coordinates
        if self.showWorldFrame:
            drawFrameAxes((0, 0, 0), Frame(vector(0, 1, 0), vector(1, 0, 0)))

        (points, frames) = computeRMF(
            self.curve, self.sampleCount, self.boundaryConditions, self.adjustFrames, self.adjustmentFunc
        )
        # display the results
        if self.showFrames:
            drawFrames(points, frames)
        if self.showSweepSurface:
            makeSweepSurface(points, frames, 0.9)

 def move(self, t0, t1):
     '''
     move the racer between t0 and t1 time quant
     '''
     sin_beta, cos_beta = self.__compute_sin_cos_beta()
     (xx, vx), (xy, vy) = self.__compute_next_position_and_velocity(t0,t1, sin_beta, cos_beta)
     self.positions.append(visual.vector(xx,xy))
     self.velocities.append(visual.vector(vx,vy))
     new_kappa, new_sign_omega = self.kappa_controller.control(position=self.positions[-1], kappa=self.kappas[-1],
                                               sign_omega=self.sign_omegas[-1],
                                               start_x=self.positions[0][0], cos_beta=cos_beta, sin_beta=sin_beta)
     self.kappas.append(new_kappa)
     self.sign_omegas.append(new_sign_omega)

 def __init__(self, x, y, useScientific=False):
     try:
         self.useScientific = useScientific
         if useScientific is False:
             self.timerLabel = label(pos=vector(x,y,0), text='00:00:00:00', box=False)
         else:
             self.timerLabel = label(pos=vector(x,y,0), text='00E01', box=False)
     except TypeError as err:
         print "**********TYPE ERROR**********"
         print "Please check that you are not passing in a variable of the wrong type (e.g. a scalar as a vector, or vice-versa)!"
         print "******************************"
         print err
         raise err

	def setPos(self, pos):
		# parameter pos is a (x,y) tuple
		x, y = pos[0], pos[1]
		# self.__pos is a CGAL.Vector_2()
		self.__pos = CGAL.Vector_2(x, y)
		# self.__vpos is a visual.vector(). Used for visual representation
		self.__vpos = visual.vector(x, y)
		# TOCHECK: CGAL.Point_2 has no setter ?!
		super(VPoint_2, self).__init__(x, y)
		# self.__repr.pos is visual.vector(x,y)
		if self.__repr is not None:
			self.__repr.pos = visual.vector(x, y)
			
		self.display.updateVPoints_2(self)