Python trackball.Trackball类代码示例

	def __init__(self, g_pool, world_camera_intrinsics , cal_ref_points_3d, eye_to_world_matrix0 , cal_gaze_points0_3d, eye_to_world_matrix1 = np.eye(4) , cal_gaze_points1_3d = [],   name = "Debug Calibration Visualizer", run_independently = False):
       # super(Visualizer, self).__init__()

		self.g_pool = g_pool
		self.image_width = 640 # right values are assigned in update
		self.focal_length = 620
		self.image_height = 480

		self.eye_to_world_matrix0 = eye_to_world_matrix0
		self.eye_to_world_matrix1 = eye_to_world_matrix1

		self.cal_ref_points_3d = cal_ref_points_3d
		self.cal_gaze_points0_3d = cal_gaze_points0_3d
		self.cal_gaze_points1_3d = cal_gaze_points1_3d

		self.world_camera_width = world_camera_intrinsics['resolution'][0]
		self.world_camera_height = world_camera_intrinsics['resolution'][1]
		self.world_camera_focal = (world_camera_intrinsics['camera_matrix'][0][0] +  world_camera_intrinsics['camera_matrix'][1][1] ) / 2.0

		# transformation matrices
		self.anthromorphic_matrix = self.get_anthropomorphic_matrix()
		self.adjusted_pixel_space_matrix = self.get_adjusted_pixel_space_matrix(1)

		self.name = name
		self.window_size = (640,480)
		self.window = None
		self.input = None
		self.run_independently = run_independently

		camera_fov = math.degrees(2.0 * math.atan( self.window_size[0] / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)
		self.trackball.distance = [0,0,-0.1]
		self.trackball.pitch = 0
		self.trackball.roll = 180

	def __init__(self, g_pool, world_camera_intrinsics , cal_ref_points_3d, cal_observed_points_3d, eye_camera_to_world_matrix0 , cal_gaze_points0_3d, eye_camera_to_world_matrix1 = np.eye(4) , cal_gaze_points1_3d = [],  run_independently = False , name = "Calibration Visualizer" ):
		super(Calibration_Visualizer, self).__init__( g_pool,name,  run_independently)

		self.image_width = 640 # right values are assigned in update
		self.focal_length = 620
		self.image_height = 480

		self.eye_camera_to_world_matrix0 = eye_camera_to_world_matrix0
		self.eye_camera_to_world_matrix1 = eye_camera_to_world_matrix1

		self.cal_ref_points_3d = cal_ref_points_3d
		self.cal_observed_points_3d = cal_observed_points_3d
		self.cal_gaze_points0_3d = cal_gaze_points0_3d
		self.cal_gaze_points1_3d = cal_gaze_points1_3d

		if world_camera_intrinsics:
			self.world_camera_width = world_camera_intrinsics['resolution'][0]
			self.world_camera_height = world_camera_intrinsics['resolution'][1]
			self.world_camera_focal = (world_camera_intrinsics['camera_matrix'][0][0] +  world_camera_intrinsics['camera_matrix'][1][1] ) / 2.0
		else:
			self.world_camera_width = 0
			self.world_camera_height = 0
			self.world_camera_focal = 0

		camera_fov = math.degrees(2.0 * math.atan( self.window_size[0] / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)
		self.trackball.distance = [0,0,-80.]
		self.trackball.pitch = 210
		self.trackball.roll = 0

	def __init__(self,g_pool , focal_length ):
		super(Eye_Visualizer, self).__init__(g_pool , "Debug Visualizer", False)

		self.focal_length = focal_length
		self.image_width = 640 # right values are assigned in update
		self.image_height = 480

		camera_fov = math.degrees(2.0 * math.atan( self.image_height / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)

	def __init__(self,focal_length, name = "Debug Visualizer", run_independently = False):
       # super(Visualizer, self).__init__()
		self.focal_length = focal_length
		self.image_width = 640 # right values are assigned in update
		self.image_height = 480
		# transformation matrices
		self.anthromorphic_matrix = self.get_anthropomorphic_matrix()

		self.name = name
		self.window_size = (640,480)
		self._window = None
		self.input = None
		self.run_independently = run_independently

		camera_fov = math.degrees(2.0 * math.atan( self.image_height / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)

    def open_window(self):
        if not self._window:
            if self.fullscreen:
                monitor = glfwGetMonitors()[self.monitor_idx]
                mode = glfwGetVideoMode(monitor)
                height, width = mode[0], mode[1]
            else:
                monitor = None
                height, width = (
                    640,
                    int(640.0 / (self.real_world_size["x"] / self.real_world_size["y"])),
                )  # open with same aspect ratio as surface

            self._window = glfwCreateWindow(
                height, width, "Reference Surface: " + self.name, monitor=monitor, share=glfwGetCurrentContext()
            )
            if not self.fullscreen:
                glfwSetWindowPos(self._window, 200, 0)

            self.trackball = Trackball()
            self.input = {"down": False, "mouse": (0, 0)}

            # Register callbacks
            glfwSetFramebufferSizeCallback(self._window, self.on_resize)
            glfwSetKeyCallback(self._window, self.on_key)
            glfwSetWindowCloseCallback(self._window, self.on_close)
            glfwSetMouseButtonCallback(self._window, self.on_button)
            glfwSetCursorPosCallback(self._window, self.on_pos)
            glfwSetScrollCallback(self._window, self.on_scroll)

            self.on_resize(self._window, *glfwGetFramebufferSize(self._window))

            # gl_state settings
            active_window = glfwGetCurrentContext()
            glfwMakeContextCurrent(self._window)
            basic_gl_setup()
            make_coord_system_norm_based()

            # refresh speed settings
            glfwSwapInterval(0)

            glfwMakeContextCurrent(active_window)

class Calibration_Visualizer(Visualizer):
	def __init__(self, g_pool, world_camera_intrinsics , cal_ref_points_3d, cal_observed_points_3d, eye_camera_to_world_matrix0 , cal_gaze_points0_3d, eye_camera_to_world_matrix1 = np.eye(4) , cal_gaze_points1_3d = [],  run_independently = False , name = "Calibration Visualizer" ):
		super(Calibration_Visualizer, self).__init__( g_pool,name,  run_independently)

		self.image_width = 640 # right values are assigned in update
		self.focal_length = 620
		self.image_height = 480

		self.eye_camera_to_world_matrix0 = eye_camera_to_world_matrix0
		self.eye_camera_to_world_matrix1 = eye_camera_to_world_matrix1

		self.cal_ref_points_3d = cal_ref_points_3d
		self.cal_observed_points_3d = cal_observed_points_3d
		self.cal_gaze_points0_3d = cal_gaze_points0_3d
		self.cal_gaze_points1_3d = cal_gaze_points1_3d

		if world_camera_intrinsics:
			self.world_camera_width = world_camera_intrinsics['resolution'][0]
			self.world_camera_height = world_camera_intrinsics['resolution'][1]
			self.world_camera_focal = (world_camera_intrinsics['camera_matrix'][0][0] +  world_camera_intrinsics['camera_matrix'][1][1] ) / 2.0
		else:
			self.world_camera_width = 0
			self.world_camera_height = 0
			self.world_camera_focal = 0

		camera_fov = math.degrees(2.0 * math.atan( self.window_size[0] / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)
		self.trackball.distance = [0,0,-80.]
		self.trackball.pitch = 210
		self.trackball.roll = 0


	########### Open, update, close #####################

	def update_window(self, g_pool , gaze_points0 , sphere0 , gaze_points1 = [] , sphere1 = None, intersection_points = []  ):

		if not self.window:
			return

		self.begin_update_window() #sets context

		self.clear_gl_screen()
		self.trackball.push()

		glMatrixMode( GL_MODELVIEW )

		# draw things in world camera coordinate system
		glPushMatrix()
		glLoadIdentity()

		calibration_points_line_color = RGBA(0.5,0.5,0.5,0.1);
		error_line_color = RGBA(1.0,0.0,0.0,0.5)

		self.draw_coordinate_system(200)
		if self.world_camera_width != 0:
			self.draw_frustum( self.world_camera_width/ 10.0 , self.world_camera_height/ 10.0 , self.world_camera_focal / 10.0)

		for p in self.cal_observed_points_3d:
			glutils.draw_polyline( [ (0,0,0), p]  , 1 , calibration_points_line_color, line_type = GL_LINES)
			#draw error lines form eye gaze points to  ref points
		for(cal_point,ref_point) in zip(self.cal_ref_points_3d, self.cal_observed_points_3d):
				glutils.draw_polyline( [ cal_point, ref_point]  , 1 , error_line_color, line_type = GL_LINES)

		#calibration points
		glutils.draw_points( self.cal_ref_points_3d , 4 , RGBA( 0, 1, 1, 1 ) )


		glPopMatrix()

		if sphere0:
			# eye camera
			glPushMatrix()
			glLoadMatrixf( self.eye_camera_to_world_matrix0.T )

			self.draw_coordinate_system(60)
			self.draw_frustum( self.image_width / 10.0, self.image_height / 10.0, self.focal_length /10.)
			glPopMatrix()

			#everything else is in world coordinates

			#eye
			sphere_center0 = list(sphere0['center'])
			sphere_radius0 = sphere0['radius']
			self.draw_sphere(sphere_center0,sphere_radius0,  color = RGBA(1,1,0,1))

			#gazelines
			for p in self.cal_gaze_points0_3d:
				glutils.draw_polyline( [ sphere_center0, p]  , 1 , calibration_points_line_color, line_type = GL_LINES)

			#calibration points
			# glutils.draw_points( self.cal_gaze_points0_3d , 4 , RGBA( 1, 0, 1, 1 ) )

			#current gaze points
			glutils.draw_points( gaze_points0 , 2 , RGBA( 1, 0, 0, 1 ) )
			for p in gaze_points0:
				glutils.draw_polyline( [sphere_center0, p]  , 1 , RGBA(0,0,0,1), line_type = GL_LINES)

			#draw error lines form eye gaze points to  ref points
			for(cal_gaze_point,ref_point) in zip(self.cal_gaze_points0_3d, self.cal_ref_points_3d):
				glutils.draw_polyline( [ cal_gaze_point, ref_point]  , 1 , error_line_color, line_type = GL_LINES)
#.........这里部分代码省略.........

#.........这里部分代码省略.........

            # Draw the camera frustum and origin using the 3d tranformation obtained from solvepnp
            glPushMatrix()
            glMultMatrixf(self.camera_pose_3d.T.flatten())
            draw_frustum(self.img_size, K, 150)
            glLineWidth(1)
            draw_frustum(self.img_size, K, .1)
            draw_coordinate_system(l=5)
            glPopMatrix()


            self.trackball.pop()

            glfwSwapBuffers(self._window)
            glfwMakeContextCurrent(active_window)


    def open_window(self):
        if not self._window:
            if self.fullscreen:
                monitor = glfwGetMonitors()[self.monitor_idx]
                mode = glfwGetVideoMode(monitor)
                height,width= mode[0],mode[1]
            else:
                monitor = None
                height,width= 640,int(640./(self.real_world_size['x']/self.real_world_size['y'])) #open with same aspect ratio as surface

            self._window = glfwCreateWindow(height, width, "Reference Surface: " + self.name, monitor=monitor, share=glfwGetCurrentContext())
            if not self.fullscreen:
                glfwSetWindowPos(self._window,200,0)



            self.trackball = Trackball()
            self.input = {'down':False, 'mouse':(0,0)}


            #Register callbacks
            glfwSetFramebufferSizeCallback(self._window,self.on_resize)
            glfwSetKeyCallback(self._window,self.on_key)
            glfwSetWindowCloseCallback(self._window,self.on_close)
            glfwSetMouseButtonCallback(self._window,self.on_button)
            glfwSetCursorPosCallback(self._window,self.on_pos)
            glfwSetScrollCallback(self._window,self.on_scroll)

            self.on_resize(self._window,*glfwGetFramebufferSize(self._window))

            # gl_state settings
            active_window = glfwGetCurrentContext()
            glfwMakeContextCurrent(self._window)
            basic_gl_setup()
            make_coord_system_norm_based()

            # refresh speed settings
            glfwSwapInterval(0)

            glfwMakeContextCurrent(active_window)

    def close_window(self):
        if self._window:
            glfwDestroyWindow(self._window)
            self._window = None
            self.window_should_close = False

    def open_close_window(self):
        if self._window:

class Visualizer(object):
	def __init__(self,focal_length, name = "Debug Visualizer", run_independently = False):
       # super(Visualizer, self).__init__()
		self.focal_length = focal_length
		self.image_width = 640 # right values are assigned in update
		self.image_height = 480
		# transformation matrices
		self.anthromorphic_matrix = self.get_anthropomorphic_matrix()

		self.name = name
		self.window_size = (640,480)
		self._window = None
		self.input = None
		self.run_independently = run_independently

		camera_fov = math.degrees(2.0 * math.atan( self.image_height / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)

	############## MATRIX FUNCTIONS ##############################

	def get_anthropomorphic_matrix(self):
		temp =  np.identity(4)
		temp[2,2] *= -1
		return temp


	def get_adjusted_pixel_space_matrix(self,scale):
		# returns a homoegenous matrix
		temp = self.get_anthropomorphic_matrix()
		temp[3,3] *= scale
		return temp

	def get_image_space_matrix(self,scale=1.):
		temp = self.get_adjusted_pixel_space_matrix(scale)
		temp[1,1] *=-1 #image origin is top left
		temp[0,3] = -self.image_width/2.0
		temp[1,3] = self.image_height/2.0
		temp[2,3] = -self.focal_length
		return temp.T

	def get_pupil_transformation_matrix(self,circle_normal,circle_center, circle_scale = 1.0):
		"""
			OpenGL matrix convention for typical GL software
			with positive Y=up and positive Z=rearward direction
			RT = right
			UP = up
			BK = back
			POS = position/translation
			US = uniform scale

			float transform[16];

			[0] [4] [8 ] [12]
			[1] [5] [9 ] [13]
			[2] [6] [10] [14]
			[3] [7] [11] [15]

			[RT.x] [UP.x] [BK.x] [POS.x]
			[RT.y] [UP.y] [BK.y] [POS.y]
			[RT.z] [UP.z] [BK.z] [POS.Z]
			[    ] [    ] [    ] [US   ]
		"""
		temp = self.get_anthropomorphic_matrix()
		right = temp[:3,0]
		up = temp[:3,1]
		back = temp[:3,2]
		translation = temp[:3,3]
		back[:] = np.array(circle_normal)
		back[2] *=-1 #our z axis is inverted

		if np.linalg.norm(back) != 0:
			back[:] /= np.linalg.norm(back)
			right[:] = get_perpendicular_vector(back)/np.linalg.norm(get_perpendicular_vector(back))
			up[:] = np.cross(right,back)/np.linalg.norm(np.cross(right,back))
			right[:] *= circle_scale
			back[:] *=circle_scale
			up[:] *=circle_scale
			translation[:] = np.array(circle_center)
			translation[2] *= -1
		return   temp.T

	############## DRAWING FUNCTIONS ##############################

	def draw_frustum(self ):

		W = self.image_width/2.0
		H = self.image_height/2.0
		Z = self.focal_length
		glPushMatrix()
		# draw it
		glLineWidth(1)
		glColor4f( 1, 0.5, 0, 0.5 )
		glBegin( GL_LINE_LOOP )
		glVertex3f( 0, 0, 0 )
		glVertex3f( -W, H, Z )
		glVertex3f( W, H, Z )
		glVertex3f( 0, 0, 0 )
		glVertex3f( W, H, Z )
		glVertex3f( W, -H, Z )
		glVertex3f( 0, 0, 0 )
#.........这里部分代码省略.........

#.........这里部分代码省略.........

            # Draw the camera frustum and origin using the 3d tranformation obtained from solvepnp
            glPushMatrix()
            glMultMatrixf(self.camera_pose_3d.T.flatten())
            draw_frustum(img_size, K, 150)
            glLineWidth(1)
            draw_frustum(img_size, K, .1)
            draw_coordinate_system(l=5)
            glPopMatrix()


            self.trackball.pop()

            glfwSwapBuffers(self._window)
            glfwMakeContextCurrent(active_window)


    def open_window(self):
        if not self._window:
            if self.fullscreen:
                monitor = glfwGetMonitors()[self.monitor_idx]
                mode = glfwGetVideoMode(monitor)
                height,width= mode[0],mode[1]
            else:
                monitor = None
                height,width= 640,int(640./(self.real_world_size['x']/self.real_world_size['y'])) #open with same aspect ratio as surface

            self._window = glfwCreateWindow(height, width, "Reference Surface: " + self.name, monitor=monitor, share=glfwGetCurrentContext())
            if not self.fullscreen:
                glfwSetWindowPos(self._window,200,0)



            self.trackball = Trackball()
            self.input = {'down':False, 'mouse':(0,0)}


            #Register callbacks
            glfwSetFramebufferSizeCallback(self._window,self.on_resize)
            glfwSetKeyCallback(self._window,self.on_key)
            glfwSetWindowCloseCallback(self._window,self.on_close)
            glfwSetMouseButtonCallback(self._window,self.on_button)
            glfwSetCursorPosCallback(self._window,self.on_pos)
            glfwSetScrollCallback(self._window,self.on_scroll)

            self.on_resize(self._window,*glfwGetFramebufferSize(self._window))

            # gl_state settings
            active_window = glfwGetCurrentContext()
            glfwMakeContextCurrent(self._window)
            basic_gl_setup()
            make_coord_system_norm_based()

            # refresh speed settings
            glfwSwapInterval(0)

            glfwMakeContextCurrent(active_window)

    def close_window(self):
        if self._window:
            glfwDestroyWindow(self._window)
            self._window = None
            self.window_should_close = False

    def open_close_window(self):
        if self._window:

class Eye_Visualizer(Visualizer):
	def __init__(self,g_pool , focal_length ):
		super(Eye_Visualizer, self).__init__(g_pool , "Debug Visualizer", False)

		self.focal_length = focal_length
		self.image_width = 640 # right values are assigned in update
		self.image_height = 480

		camera_fov = math.degrees(2.0 * math.atan( self.image_height / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)

	############## MATRIX FUNCTIONS ##############################

	def get_anthropomorphic_matrix(self):
		temp =  np.identity(4)
		temp[2,2] *= -1
		return temp


	def get_adjusted_pixel_space_matrix(self,scale):
		# returns a homoegenous matrix
		temp = self.get_anthropomorphic_matrix()
		temp[3,3] *= scale
		return temp

	def get_image_space_matrix(self,scale=1.):
		temp = self.get_adjusted_pixel_space_matrix(scale)
		temp[1,1] *=-1 #image origin is top left
		temp[0,3] = -self.image_width/2.0
		temp[1,3] = self.image_height/2.0
		temp[2,3] = -self.focal_length
		return temp.T

	def get_pupil_transformation_matrix(self,circle_normal,circle_center, circle_scale = 1.0):
		"""
			OpenGL matrix convention for typical GL software
			with positive Y=up and positive Z=rearward direction
			RT = right
			UP = up
			BK = back
			POS = position/translation
			US = uniform scale

			float transform[16];

			[0] [4] [8 ] [12]
			[1] [5] [9 ] [13]
			[2] [6] [10] [14]
			[3] [7] [11] [15]

			[RT.x] [UP.x] [BK.x] [POS.x]
			[RT.y] [UP.y] [BK.y] [POS.y]
			[RT.z] [UP.z] [BK.z] [POS.Z]
			[    ] [    ] [    ] [US   ]
		"""
		temp = self.get_anthropomorphic_matrix()
		right = temp[:3,0]
		up = temp[:3,1]
		back = temp[:3,2]
		translation = temp[:3,3]
		back[:] = np.array(circle_normal)
		back[2] *=-1 #our z axis is inverted

		if np.linalg.norm(back) != 0:
			back[:] /= np.linalg.norm(back)
			right[:] = get_perpendicular_vector(back)/np.linalg.norm(get_perpendicular_vector(back))
			up[:] = np.cross(right,back)/np.linalg.norm(np.cross(right,back))
			right[:] *= circle_scale
			back[:] *=circle_scale
			up[:] *=circle_scale
			translation[:] = np.array(circle_center)
			translation[2] *= -1
		return   temp.T

	############## DRAWING FUNCTIONS ##############################

	def draw_debug_info(self, result  ):
		models = result['models']
		eye = models[0]['sphere'];
		direction = result['circle'][1];
		pupil_radius = result['circle'][2];

		status = ' Eyeball center : X: %.2fmm Y: %.2fmm Z: %.2fmm\n Pupil direction:  X: %.2f Y: %.2f Z: %.2f\n Pupil Diameter: %.2fmm\n  ' \
		%(eye[0][0], eye[0][1],eye[0][2],
		direction[0], direction[1],direction[2], pupil_radius*2)

		self.glfont.push_state()
		self.glfont.set_color_float( (0,0,0,1) )

		self.glfont.draw_multi_line_text(5,20,status)


		#draw model info for each model
		delta_y = 20
		for model in models:
			modelStatus =	('Model: %d \n' %  model['model_id'] ,
							'    age: %.1fs\n' %(self.g_pool.get_timestamp()-model['birth_timestamp']) ,
							'    maturity: %.3f\n' % model['maturity'] ,
							'    solver fit: %.6f\n' % model['solver_fit'] ,
							'    confidence: %.6f\n' % model['confidence'] ,
#.........这里部分代码省略.........

class Calibration_Visualizer(object):
	def __init__(self, g_pool, world_camera_intrinsics , cal_ref_points_3d, eye_to_world_matrix0 , cal_gaze_points0_3d, eye_to_world_matrix1 = np.eye(4) , cal_gaze_points1_3d = [],   name = "Debug Calibration Visualizer", run_independently = False):
       # super(Visualizer, self).__init__()

		self.g_pool = g_pool
		self.image_width = 640 # right values are assigned in update
		self.focal_length = 620
		self.image_height = 480

		self.eye_to_world_matrix0 = eye_to_world_matrix0
		self.eye_to_world_matrix1 = eye_to_world_matrix1

		self.cal_ref_points_3d = cal_ref_points_3d
		self.cal_gaze_points0_3d = cal_gaze_points0_3d
		self.cal_gaze_points1_3d = cal_gaze_points1_3d

		self.world_camera_width = world_camera_intrinsics['resolution'][0]
		self.world_camera_height = world_camera_intrinsics['resolution'][1]
		self.world_camera_focal = (world_camera_intrinsics['camera_matrix'][0][0] +  world_camera_intrinsics['camera_matrix'][1][1] ) / 2.0

		# transformation matrices
		self.anthromorphic_matrix = self.get_anthropomorphic_matrix()
		self.adjusted_pixel_space_matrix = self.get_adjusted_pixel_space_matrix(1)

		self.name = name
		self.window_size = (640,480)
		self.window = None
		self.input = None
		self.run_independently = run_independently

		camera_fov = math.degrees(2.0 * math.atan( self.window_size[0] / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)
		self.trackball.distance = [0,0,-0.1]
		self.trackball.pitch = 0
		self.trackball.roll = 180

	############## MATRIX FUNCTIONS ##############################

	def get_anthropomorphic_matrix(self):
		temp =  np.identity(4)
		return temp

	def get_adjusted_pixel_space_matrix(self,scale  = 1.0):
		# returns a homoegenous matrix
		temp = self.get_anthropomorphic_matrix()
		temp[3,3] *= scale
		return temp

	def get_image_space_matrix(self,scale=1.):
		temp = self.get_adjusted_pixel_space_matrix(scale)
		#temp[1,1] *=-1 #image origin is top left
		#temp[2,2] *=-1 #image origin is top left
		temp[0,3] = -self.world_camera_width/2.0
		temp[1,3] = -self.world_camera_height/2.0
		temp[2,3] = self.world_camera_focal
		return temp.T

	def get_pupil_transformation_matrix(self,circle_normal,circle_center, circle_scale = 1.0):
		"""
			OpenGL matrix convention for typical GL software
			with positive Y=up and positive Z=rearward direction
			RT = right
			UP = up
			BK = back
			POS = position/translation
			US = uniform scale

			float transform[16];

			[0] [4] [8 ] [12]
			[1] [5] [9 ] [13]
			[2] [6] [10] [14]
			[3] [7] [11] [15]

			[RT.x] [UP.x] [BK.x] [POS.x]
			[RT.y] [UP.y] [BK.y] [POS.y]
			[RT.z] [UP.z] [BK.z] [POS.Z]
			[    ] [    ] [    ] [US   ]
		"""
		temp = self.get_anthropomorphic_matrix()
		right = temp[:3,0]
		up = temp[:3,1]
		back = temp[:3,2]
		translation = temp[:3,3]
		back[:] = np.array(circle_normal)

		# if np.linalg.norm(back) != 0:
		back[:] /= np.linalg.norm(back)
		right[:] = get_perpendicular_vector(back)/np.linalg.norm(get_perpendicular_vector(back))
		up[:] = np.cross(right,back)/np.linalg.norm(np.cross(right,back))
		right[:] *= circle_scale
		back[:] *=circle_scale
		up[:] *=circle_scale
		translation[:] = np.array(circle_center)
		return   temp.T

	############## DRAWING FUNCTIONS ##############################

	def draw_frustum(self, width, height , length):

#.........这里部分代码省略.........