from OpenGL.GL import * from OpenGL.GLUT import * from OpenGL.GLU import * import ctypes import sys import math from threading import Lock import numpy as np import array import pyzed.sl as sl M_PI = 3.1415926 VERTEX_SHADER = """ # version 330 core layout(location = 0) in vec3 in_Vertex; layout(location = 1) in vec4 in_Color; uniform mat4 u_mvpMatrix; out vec4 b_color; void main() { b_color = in_Color; gl_Position = u_mvpMatrix * vec4(in_Vertex, 1); } """ FRAGMENT_SHADER = """ # version 330 core in vec4 b_color; layout(location = 0) out vec4 out_Color; void main() { out_Color = b_color; } """ POINTCLOUD_VERTEX_SHADER =""" #version 330 core layout(location = 0) in vec4 in_VertexRGBA; uniform mat4 u_mvpMatrix; out vec4 b_color; void main() { uint vertexColor = floatBitsToUint(in_VertexRGBA.w); vec3 clr_int = vec3((vertexColor & uint(0x000000FF)), (vertexColor & uint(0x0000FF00)) >> 8, (vertexColor & uint(0x00FF0000)) >> 16); b_color = vec4(clr_int.r / 255.0f, clr_int.g / 255.0f, clr_int.b / 255.0f, 1.f); gl_Position = u_mvpMatrix * vec4(in_VertexRGBA.xyz, 1); } """ POINTCLOUD_FRAGMENT_SHADER = """ #version 330 core in vec4 b_color; layout(location = 0) out vec4 out_Color; void main() { out_Color = b_color; } """ try: from cuda.bindings import runtime as cudart import cupy as cp GPU_ACCELERATION_AVAILABLE = True def format_cudart_err(err): return ( f"{cudart.cudaGetErrorName(err)[1].decode('utf-8')}({int(err)}): " f"{cudart.cudaGetErrorString(err)[1].decode('utf-8')}" ) def check_cudart_err(args): if isinstance(args, tuple): assert len(args) >= 1 err = args[0] if len(args) == 1: ret = None elif len(args) == 2: ret = args[1] else: ret = args[1:] else: err = args ret = None assert isinstance(err, cudart.cudaError_t), type(err) if err != cudart.cudaError_t.cudaSuccess: raise RuntimeError(format_cudart_err(err)) return ret class CudaOpenGLMappedBuffer: def __init__(self, gl_buffer, flags=0): self._gl_buffer = int(gl_buffer) self._flags = int(flags) self._graphics_ressource = None self._cuda_buffer = None self.register() @property def gl_buffer(self): return self._gl_buffer @property def cuda_buffer(self): assert self.mapped return self._cuda_buffer @property def graphics_ressource(self): assert self.registered return self._graphics_ressource @property def registered(self): return self._graphics_ressource is not None @property def mapped(self): return self._cuda_buffer is not None def __enter__(self): return self.map() def __exit__(self, exc_type, exc_value, trace): self.unmap() return False def __del__(self): try: self.unregister() except: # Ignore errors during cleanup (e.g., during Python shutdown) pass def register(self): if self.registered: return self._graphics_ressource self._graphics_ressource = check_cudart_err( cudart.cudaGraphicsGLRegisterBuffer(self._gl_buffer, self._flags) ) return self._graphics_ressource def unregister(self): if not self.registered: return self try: self.unmap() if cudart is not None: # Check if cudart is still available check_cudart_err( cudart.cudaGraphicsUnregisterResource(self._graphics_ressource) ) self._graphics_ressource = None except Exception: # Ignore errors during cleanup (e.g., during Python shutdown) self._graphics_ressource = None return self def map(self, stream=None): if not self.registered: raise RuntimeError("Cannot map an unregistered buffer.") if self.mapped: return self._cuda_buffer check_cudart_err( cudart.cudaGraphicsMapResources(1, self._graphics_ressource, stream) ) ptr, size = check_cudart_err( cudart.cudaGraphicsResourceGetMappedPointer(self._graphics_ressource) ) self._cuda_buffer = cp.cuda.MemoryPointer( cp.cuda.UnownedMemory(ptr, size, self), 0 ) return self._cuda_buffer def unmap(self, stream=None): if not self.registered: raise RuntimeError("Cannot unmap an unregistered buffer.") if not self.mapped: return self try: if cudart is not None: # Check if cudart is still available check_cudart_err( cudart.cudaGraphicsUnmapResources(1, self._graphics_ressource, stream) ) self._cuda_buffer = None except Exception: # Force cleanup even if unmap fails self._cuda_buffer = None return self class CudaOpenGLMappedArray(CudaOpenGLMappedBuffer): def __init__(self, dtype, shape, gl_buffer, flags=0, strides=None, order='C'): super().__init__(gl_buffer, flags) self._dtype = dtype self._shape = shape self._strides = strides self._order = order @property def cuda_array(self): assert self.mapped return cp.ndarray( shape=self._shape, dtype=self._dtype, strides=self._strides, order=self._order, memptr=self._cuda_buffer, ) def map(self, *args, **kwargs): super().map(*args, **kwargs) return self.cuda_array except ImportError: GPU_ACCELERATION_AVAILABLE = False class Shader: def __init__(self, _vs, _fs): self.program_id = glCreateProgram() vertex_id = self.compile(GL_VERTEX_SHADER, _vs) fragment_id = self.compile(GL_FRAGMENT_SHADER, _fs) glAttachShader(self.program_id, vertex_id) glAttachShader(self.program_id, fragment_id) glBindAttribLocation( self.program_id, 0, "in_vertex") glBindAttribLocation( self.program_id, 1, "in_texCoord") glLinkProgram(self.program_id) if glGetProgramiv(self.program_id, GL_LINK_STATUS) != GL_TRUE: info = glGetProgramInfoLog(self.program_id) if (self.program_id is not None) and (self.program_id > 0) and glIsProgram(self.program_id): glDeleteProgram(self.program_id) if (vertex_id is not None) and (vertex_id > 0) and glIsShader(vertex_id): glDeleteShader(vertex_id) if (fragment_id is not None) and (fragment_id > 0) and glIsShader(fragment_id): glDeleteShader(fragment_id) raise RuntimeError('Error linking program: %s' % (info)) if (vertex_id is not None) and (vertex_id > 0) and glIsShader(vertex_id): glDeleteShader(vertex_id) if (fragment_id is not None) and (fragment_id > 0) and glIsShader(fragment_id): glDeleteShader(fragment_id) def compile(self, _type, _src): shader_id = None try: shader_id = glCreateShader(_type) if shader_id == 0: print("ERROR: shader type {0} does not exist".format(_type)) exit() glShaderSource(shader_id, _src) glCompileShader(shader_id) if glGetShaderiv(shader_id, GL_COMPILE_STATUS) != GL_TRUE: info = glGetShaderInfoLog(shader_id) if (shader_id is not None) and (shader_id > 0) and glIsShader(shader_id): glDeleteShader(shader_id) raise RuntimeError('Shader compilation failed: %s' % (info)) return shader_id except: if (shader_id is not None) and (shader_id > 0) and glIsShader(shader_id): glDeleteShader(shader_id) raise def get_program_id(self): return self.program_id class Simple3DObject: def __init__(self, _is_static, pts_size = 3, clr_size = 3): self.is_init = False self.drawing_type = GL_TRIANGLES self.is_static = _is_static self.clear() self.pt_type = pts_size self.clr_type = clr_size self.data = sl.Mat() self.cuda_mapped_buffer = None self.use_gpu = GPU_ACCELERATION_AVAILABLE and not _is_static def add_pt(self, _pts): # _pts [x,y,z] for pt in _pts: self.vertices.append(pt) def add_clr(self, _clrs): # _clr [r,g,b] for clr in _clrs: self.colors.append(clr) def add_point_clr(self, _pt, _clr): self.add_pt(_pt) self.add_clr(_clr) self.indices.append(len(self.indices)) def add_line(self, _p1, _p2, _clr): self.add_point_clr(_p1, _clr) self.add_point_clr(_p2, _clr) def addFace(self, p1, p2, p3, clr): self.add_point_clr(p1, clr) self.add_point_clr(p2, clr) self.add_point_clr(p3, clr) def push_to_GPU(self): if not self.is_init: self.vboID = glGenBuffers(3) self.is_init = True if self.is_static: type_draw = GL_STATIC_DRAW else: type_draw = GL_DYNAMIC_DRAW if len(self.vertices): glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) glBufferData(GL_ARRAY_BUFFER, len(self.vertices) * self.vertices.itemsize, (GLfloat * len(self.vertices))(*self.vertices), type_draw) if len(self.colors): glBindBuffer(GL_ARRAY_BUFFER, self.vboID[1]) glBufferData(GL_ARRAY_BUFFER, len(self.colors) * self.colors.itemsize, (GLfloat * len(self.colors))(*self.colors), type_draw) if len(self.indices): glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[2]) glBufferData(GL_ELEMENT_ARRAY_BUFFER,len(self.indices) * self.indices.itemsize,(GLuint * len(self.indices))(*self.indices), type_draw) self.elementbufferSize = len(self.indices) def init(self, res): if not self.is_init: self.vboID = glGenBuffers(3) self.is_init = True if self.is_static: type_draw = GL_STATIC_DRAW else: type_draw = GL_DYNAMIC_DRAW self.elementbufferSize = res.width * res.height # Initialize vertex buffer (for XYZRGBA data) glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) glBufferData(GL_ARRAY_BUFFER, self.elementbufferSize * self.pt_type * self.vertices.itemsize, None, type_draw) # Try to set up GPU acceleration if available if self.use_gpu: try: flags = cudart.cudaGraphicsRegisterFlags.cudaGraphicsRegisterFlagsWriteDiscard self.cuda_mapped_buffer = CudaOpenGLMappedArray( dtype=np.float32, shape=(self.elementbufferSize, self.pt_type), gl_buffer=self.vboID[0], flags=flags ) except Exception as e: print(f"Failed to initialize GPU acceleration, falling back to CPU: {e}") self.use_gpu = False self.cuda_mapped_buffer = None # Initialize color buffer (not used for point clouds with XYZRGBA) if self.clr_type: glBindBuffer(GL_ARRAY_BUFFER, self.vboID[1]) glBufferData(GL_ARRAY_BUFFER, self.elementbufferSize * self.clr_type * self.colors.itemsize, None, type_draw) for i in range (0, self.elementbufferSize): self.indices.append(i) glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[2]) glBufferData(GL_ELEMENT_ARRAY_BUFFER,len(self.indices) * self.indices.itemsize,(GLuint * len(self.indices))(*self.indices), type_draw) def setPoints(self, pc): """Update point cloud data from sl.Mat""" if not pc.is_init(): return try: if self.use_gpu and self.cuda_mapped_buffer and pc.get_memory_type() in (sl.MEM.GPU, sl.MEM.BOTH): self.setPointsGPU(pc) else: self.setPointsCPU(pc) except Exception as e: print(f"Error setting points: {e}") # Fallback to CPU if GPU fails if self.use_gpu: print("Falling back to CPU processing") self.use_gpu = False self.setPointsCPU(pc) def setPointsGPU(self, pc): """Set points using GPU acceleration with CUDA-OpenGL interop""" try: # Get point cloud data from GPU memory cupy_arr = pc.get_data(sl.MEM.GPU) # Map OpenGL buffer to CUDA memory with self.cuda_mapped_buffer as cuda_array: # Reshape point cloud data to match buffer format if cupy_arr.ndim == 3: # (height, width, channels) pc_flat = cupy_arr.reshape(-1, cupy_arr.shape[-1]) else: pc_flat = cupy_arr # Copy data to GPU buffer (optimized GPU-to-GPU copy with continuous memory) points_to_copy = min(pc_flat.shape[0], cuda_array.shape[0]) cuda_array[:points_to_copy] = pc_flat[:points_to_copy] # Zero out remaining buffer if needed if points_to_copy < cuda_array.shape[0]: cuda_array[points_to_copy:] = 0 except Exception as e: print(f"GPU point cloud update failed: {e}") raise def setPointsCPU(self, pc): """Fallback CPU method for setting points""" try: # Ensure data is available on CPU if pc.get_memory_type() == sl.MEM.GPU: pc.update_cpu_from_gpu() # Get CPU pointer and upload to GPU buffer glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) data_ptr = pc.get_pointer(sl.MEM.CPU) buffer_size = self.elementbufferSize * self.pt_type * 4 # 4 bytes per float32 glBufferSubData(GL_ARRAY_BUFFER, 0, buffer_size, ctypes.c_void_p(data_ptr)) glBindBuffer(GL_ARRAY_BUFFER, 0) except Exception as e: print(f"CPU point cloud update failed: {e}") raise def clear(self): self.vertices = array.array('f') self.colors = array.array('f') self.indices = array.array('I') self.elementbufferSize = 0 def set_drawing_type(self, _type): self.drawing_type = _type def draw(self): if self.elementbufferSize: glEnableVertexAttribArray(0) glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) glVertexAttribPointer(0,self.pt_type,GL_FLOAT,GL_FALSE,0,None) if(self.clr_type): glEnableVertexAttribArray(1) glBindBuffer(GL_ARRAY_BUFFER, self.vboID[1]) glVertexAttribPointer(1,self.clr_type,GL_FLOAT,GL_FALSE,0,None) glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[2]) glDrawElements(self.drawing_type, self.elementbufferSize, GL_UNSIGNED_INT, None) glDisableVertexAttribArray(0) if self.clr_type: glDisableVertexAttribArray(1) def __del__(self): """Cleanup GPU resources""" if hasattr(self, 'cuda_mapped_buffer') and self.cuda_mapped_buffer: try: self.cuda_mapped_buffer.unregister() except: pass class GLViewer: def __init__(self): self.available = False self.mutex = Lock() self.camera = CameraGL() self.wheelPosition = 0. self.mouse_button = [False, False] self.mouseCurrentPosition = [0., 0.] self.previousMouseMotion = [0., 0.] self.mouseMotion = [0., 0.] self.zedModel = Simple3DObject(True) self.point_cloud = Simple3DObject(False, 4) self.save_data = False def init(self, _argc, _argv, res): # _params = sl.CameraParameters glutInit(_argc, _argv) wnd_w = int(glutGet(GLUT_SCREEN_WIDTH)*0.9) wnd_h = int(glutGet(GLUT_SCREEN_HEIGHT) *0.9) glutInitWindowSize(wnd_w, wnd_h) glutInitWindowPosition(int(wnd_w*0.05), int(wnd_h*0.05)) glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH) glutCreateWindow(b"ZED Depth Sensing") glViewport(0, 0, wnd_w, wnd_h) glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_CONTINUE_EXECUTION) glEnable(GL_DEPTH_TEST) glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glEnable(GL_LINE_SMOOTH) glHint(GL_LINE_SMOOTH_HINT, GL_NICEST) # Compile and create the shader for 3D objects self.shader_image = Shader(VERTEX_SHADER, FRAGMENT_SHADER) self.shader_image_MVP = glGetUniformLocation(self.shader_image.get_program_id(), "u_mvpMatrix") self.shader_pc = Shader(POINTCLOUD_VERTEX_SHADER, POINTCLOUD_FRAGMENT_SHADER) self.shader_pc_MVP = glGetUniformLocation(self.shader_pc.get_program_id(), "u_mvpMatrix") self.bckgrnd_clr = np.array([223/255., 230/255., 233/255.]) # Create the camera model Z_ = -0.15 Y_ = Z_ * math.tan(95. * M_PI / 180. / 2.) X_ = Y_ * 16./9. A = np.array([0, 0, 0]) B = np.array([X_, Y_, Z_]) C = np.array([-X_, Y_, Z_]) D = np.array([-X_, -Y_, Z_]) E = np.array([X_, -Y_, Z_]) lime_clr = np.array([217 / 255, 255/255, 66/255]) self.zedModel.add_line(A, B, lime_clr) self.zedModel.add_line(A, C, lime_clr) self.zedModel.add_line(A, D, lime_clr) self.zedModel.add_line(A, E, lime_clr) self.zedModel.add_line(B, C, lime_clr) self.zedModel.add_line(C, D, lime_clr) self.zedModel.add_line(D, E, lime_clr) self.zedModel.add_line(E, B, lime_clr) self.zedModel.set_drawing_type(GL_LINES) self.zedModel.push_to_GPU() self.point_cloud.init(res) self.point_cloud.set_drawing_type(GL_POINTS) # Register GLUT callback functions glutDisplayFunc(self.draw_callback) glutIdleFunc(self.idle) glutKeyboardFunc(self.keyPressedCallback) glutCloseFunc(self.close_func) glutMouseFunc(self.on_mouse) glutMotionFunc(self.on_mousemove) glutReshapeFunc(self.on_resize) self.available = True def is_available(self): if self.available: glutMainLoopEvent() return self.available def updateData(self, pc): self.mutex.acquire() try: self.point_cloud.setPoints(pc) finally: self.mutex.release() def idle(self): if self.available: glutPostRedisplay() def exit(self): if self.available: self.available = False def close_func(self): if self.available: self.available = False def keyPressedCallback(self, key, x, y): if ord(key) == 27: self.close_func() if (ord(key) == 83 or ord(key) == 115): self.save_data = True def on_mouse(self,*args,**kwargs): (key,Up,x,y) = args if key==0: self.mouse_button[0] = (Up == 0) elif key==2 : self.mouse_button[1] = (Up == 0) elif(key == 3): self.wheelPosition = self.wheelPosition + 1 elif(key == 4): self.wheelPosition = self.wheelPosition - 1 self.mouseCurrentPosition = [x, y] self.previousMouseMotion = [x, y] def on_mousemove(self,*args,**kwargs): (x,y) = args self.mouseMotion[0] = x - self.previousMouseMotion[0] self.mouseMotion[1] = y - self.previousMouseMotion[1] self.previousMouseMotion = [x, y] glutPostRedisplay() def on_resize(self,Width,Height): glViewport(0, 0, Width, Height) self.camera.setProjection(Height / Width) def draw_callback(self): if self.available: glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) glClearColor(self.bckgrnd_clr[0], self.bckgrnd_clr[1], self.bckgrnd_clr[2], 1.) self.mutex.acquire() self.update() self.draw() self.mutex.release() glutSwapBuffers() glutPostRedisplay() def update(self): if(self.mouse_button[0]): r = sl.Rotation() vert=self.camera.vertical_ tmp = vert.get() vert.init_vector(tmp[0] * 1.,tmp[1] * 1., tmp[2] * 1.) r.init_angle_translation(self.mouseMotion[0] * 0.02, vert) self.camera.rotate(r) r.init_angle_translation(self.mouseMotion[1] * 0.02, self.camera.right_) self.camera.rotate(r) if(self.mouse_button[1]): t = sl.Translation() tmp = self.camera.right_.get() scale = self.mouseMotion[0] *-0.05 t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale) self.camera.translate(t) tmp = self.camera.up_.get() scale = self.mouseMotion[1] * 0.05 t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale) self.camera.translate(t) if (self.wheelPosition != 0): t = sl.Translation() tmp = self.camera.forward_.get() scale = self.wheelPosition * -0.065 t.init_vector(tmp[0] * scale, tmp[1] * scale, tmp[2] * scale) self.camera.translate(t) self.camera.update() self.mouseMotion = [0., 0.] self.wheelPosition = 0 def draw(self): vpMatrix = self.camera.getViewProjectionMatrix() glUseProgram(self.shader_image.get_program_id()) glUniformMatrix4fv(self.shader_image_MVP, 1, GL_TRUE, (GLfloat * len(vpMatrix))(*vpMatrix)) glPolygonMode(GL_FRONT_AND_BACK, GL_FILL) self.zedModel.draw() glUseProgram(0) glUseProgram(self.shader_pc.get_program_id()) glUniformMatrix4fv(self.shader_pc_MVP, 1, GL_TRUE, (GLfloat * len(vpMatrix))(*vpMatrix)) glPointSize(1.) self.point_cloud.draw() glUseProgram(0) class CameraGL: def __init__(self): self.ORIGINAL_FORWARD = sl.Translation() self.ORIGINAL_FORWARD.init_vector(0,0,1) self.ORIGINAL_UP = sl.Translation() self.ORIGINAL_UP.init_vector(0,1,0) self.ORIGINAL_RIGHT = sl.Translation() self.ORIGINAL_RIGHT.init_vector(1,0,0) self.znear = 0.5 self.zfar = 100. self.horizontalFOV = 70. self.orientation_ = sl.Orientation() self.position_ = sl.Translation() self.forward_ = sl.Translation() self.up_ = sl.Translation() self.right_ = sl.Translation() self.vertical_ = sl.Translation() self.vpMatrix_ = sl.Matrix4f() self.offset_ = sl.Translation() self.offset_.init_vector(0,0,5) self.projection_ = sl.Matrix4f() self.projection_.set_identity() self.setProjection(1.78) self.position_.init_vector(0., 0., 0.) tmp = sl.Translation() tmp.init_vector(0, 0, -.1) tmp2 = sl.Translation() tmp2.init_vector(0, 1, 0) self.setDirection(tmp, tmp2) def update(self): dot_ = sl.Translation.dot_translation(self.vertical_, self.up_) if(dot_ < 0.): tmp = self.vertical_.get() self.vertical_.init_vector(tmp[0] * -1.,tmp[1] * -1., tmp[2] * -1.) transformation = sl.Transform() tmp_position = self.position_.get() tmp = (self.offset_ * self.orientation_).get() new_position = sl.Translation() new_position.init_vector(tmp_position[0] + tmp[0], tmp_position[1] + tmp[1], tmp_position[2] + tmp[2]) transformation.init_orientation_translation(self.orientation_, new_position) transformation.inverse() self.vpMatrix_ = self.projection_ * transformation def setProjection(self, im_ratio): fov_x = self.horizontalFOV * 3.1416 / 180. fov_y = self.horizontalFOV * im_ratio * 3.1416 / 180. self.projection_[(0,0)] = 1. / math.tan(fov_x * .5) self.projection_[(1,1)] = 1. / math.tan(fov_y * .5) self.projection_[(2,2)] = -(self.zfar + self.znear) / (self.zfar - self.znear) self.projection_[(3,2)] = -1. self.projection_[(2,3)] = -(2. * self.zfar * self.znear) / (self.zfar - self.znear) self.projection_[(3,3)] = 0. def getViewProjectionMatrix(self): tmp = self.vpMatrix_.m vpMat = array.array('f') for row in tmp: for v in row: vpMat.append(v) return vpMat def getViewProjectionMatrixRT(self, tr): tmp = self.vpMatrix_ tmp.transpose() tr.transpose() tmp = (tr * tmp).m vpMat = array.array('f') for row in tmp: for v in row: vpMat.append(v) return vpMat def setDirection(self, dir, vert): dir.normalize() tmp = dir.get() dir.init_vector(tmp[0] * -1.,tmp[1] * -1., tmp[2] * -1.) self.orientation_.init_translation(self.ORIGINAL_FORWARD, dir) self.updateVectors() self.vertical_ = vert if(sl.Translation.dot_translation(self.vertical_, self.up_) < 0.): tmp = sl.Rotation() tmp.init_angle_translation(3.14, self.ORIGINAL_FORWARD) self.rotate(tmp) def translate(self, t): ref = self.position_.get() tmp = t.get() self.position_.init_vector(ref[0] + tmp[0], ref[1] + tmp[1], ref[2] + tmp[2]) def setPosition(self, p): self.position_ = p def rotate(self, r): tmp = sl.Orientation() tmp.init_rotation(r) self.orientation_ = tmp * self.orientation_ self.updateVectors() def setRotation(self, r): self.orientation_.init_rotation(r) self.updateVectors() def updateVectors(self): self.forward_ = self.ORIGINAL_FORWARD * self.orientation_ self.up_ = self.ORIGINAL_UP * self.orientation_ right = self.ORIGINAL_RIGHT tmp = right.get() right.init_vector(tmp[0] * -1.,tmp[1] * -1., tmp[2] * -1.) self.right_ = right * self.orientation_