Created
May 10, 2016 03:35
-
-
Save pinetum/d392e6935a906882c6ff2ff82bfdaa94 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| # 5550 | |
| # 1978 | |
| # 5551 | |
| # 1979 | |
| # 5552 | |
| # 1980 | |
| # 5553 | |
| # 1981 | |
| # 5554 | |
| # 1982 | |
| # 5557 | |
| # 1983 | |
| # 5558 | |
| # 1984 | |
| # 555a | |
| # 1985 | |
| import os | |
| import sys | |
| import math | |
| import serial | |
| import pygame | |
| from operator import itemgetter | |
| class Point3D: | |
| def __init__(self, x = 0, y = 0, z = 0): | |
| self.x, self.y, self.z = float(x), float(y), float(z) | |
| def rotateX(self, angle): | |
| """ Rotates the point around the X axis by the given angle in degrees. """ | |
| rad = angle * math.pi / 180 | |
| cosa = math.cos(rad) | |
| sina = math.sin(rad) | |
| y = self.y * cosa - self.z * sina | |
| z = self.y * sina + self.z * cosa | |
| return Point3D(self.x, y, z) | |
| def rotateY(self, angle): | |
| """ Rotates the point around the Y axis by the given angle in degrees. """ | |
| rad = angle * math.pi / 180 | |
| cosa = math.cos(rad) | |
| sina = math.sin(rad) | |
| z = self.z * cosa - self.x * sina | |
| x = self.z * sina + self.x * cosa | |
| return Point3D(x, self.y, z) | |
| def rotateZ(self, angle): | |
| """ Rotates the point around the Z axis by the given angle in degrees. """ | |
| rad = angle * math.pi / 180 | |
| cosa = math.cos(rad) | |
| sina = math.sin(rad) | |
| x = self.x * cosa - self.y * sina | |
| y = self.x * sina + self.y * cosa | |
| return Point3D(x, y, self.z) | |
| def project(self, win_width, win_height, fov, viewer_distance): | |
| """ Transforms this 3D point to 2D using a perspective projection. """ | |
| factor = fov / (viewer_distance + self.z) | |
| x = self.x * factor + win_width / 2 | |
| y = -self.y * factor + win_height / 2 | |
| return Point3D(x, y, self.z) | |
| class RealTimeCube: | |
| def __init__(self, win_width = 640, win_height = 480): | |
| pygame.init() | |
| self.screen = pygame.display.set_mode((win_width, win_height)) | |
| pygame.display.set_caption("JY901-realtime Cube by qtlintw") | |
| self.clock = pygame.time.Clock() | |
| self.vertices = [ | |
| Point3D(-1,1,-1), | |
| Point3D(1,1,-1), | |
| Point3D(1,-1,-1), | |
| Point3D(-1,-1,-1), | |
| Point3D(-1,1,1), | |
| Point3D(1,1,1), | |
| Point3D(1,-1,1), | |
| Point3D(-1,-1,1) | |
| ] | |
| # Define the vertices that compose each of the 6 faces. These numbers are | |
| # indices to the vertices list defined above. | |
| self.faces = [(0,1,2,3),(1,5,6,2),(5,4,7,6),(4,0,3,7),(0,4,5,1),(3,2,6,7)] | |
| # Define colors for each face | |
| self.colors = [(255,0,255),(255,0,0),(0,255,0),(0,0,255),(0,255,255),(255,255,0)] | |
| self.angle_x = 0 | |
| self.angle_y = 0 | |
| self.angle_z = 0 | |
| def run(self): | |
| #inital serial port | |
| jy_sensor = serial.Serial(port=sys.argv[1], baudrate="9600", timeout=2) | |
| print(jy_sensor.name) | |
| try: | |
| while True: | |
| data = jy_sensor.read(size=1) | |
| if data == b'\x55': | |
| jy_sensor.read(size=10) | |
| print("success!") | |
| break; | |
| print("trying", data) | |
| #main loop | |
| while 1: | |
| for event in pygame.event.get(): | |
| if event.type == pygame.QUIT: | |
| pygame.quit() | |
| sys.exit() | |
| data = jy_sensor.read(size=11) | |
| if not len(data) == 11: | |
| print("error byte lenth:", len(data), "data=", data) | |
| continue | |
| if data[0] != b'\x55': | |
| print("error occurr, nor 55 start:", data[0]) | |
| break | |
| if data[1] == 83: | |
| self.angle_x = int.from_bytes(data[2:4], byteorder='little')/32768*180 | |
| self.angle_y = int.from_bytes(data[4:6], byteorder='little')/32768*180 | |
| self.angle_z = int.from_bytes(data[6:8], byteorder='little')/32768*180 | |
| print("Angle output:{}, {}, {}".format(self.angle_x, self.angle_y, self.angle_z)) | |
| else: | |
| print(data) | |
| continue | |
| #print("-----", data[0], data[1]) | |
| # It will hold transformed vertices. | |
| #self.clock.tick(50) | |
| self.screen.fill((0,32,0)) | |
| t = [] | |
| for v in self.vertices: | |
| # Rotate the point around X axis, then around Y axis, and finally around Z axis. | |
| r = v.rotateX(self.angle_x).rotateY(self.angle_y).rotateZ(self.angle_z) | |
| # Transform the point from 3D to 2D | |
| p = r.project(self.screen.get_width(), self.screen.get_height(), 256, 4) | |
| # Put the point in the list of transformed vertices | |
| t.append(p) | |
| # Calculate the average Z values of each face. | |
| avg_z = [] | |
| i = 0 | |
| for f in self.faces: | |
| z = (t[f[0]].z + t[f[1]].z + t[f[2]].z + t[f[3]].z) / 4.0 | |
| avg_z.append([i,z]) | |
| i = i + 1 | |
| # Draw the faces using the Painter's algorithm: | |
| # Distant faces are drawn before the closer ones. | |
| for tmp in sorted(avg_z,key=itemgetter(1),reverse=True): | |
| face_index = tmp[0] | |
| f = self.faces[face_index] | |
| pointlist = [(t[f[0]].x, t[f[0]].y), (t[f[1]].x, t[f[1]].y), | |
| (t[f[1]].x, t[f[1]].y), (t[f[2]].x, t[f[2]].y), | |
| (t[f[2]].x, t[f[2]].y), (t[f[3]].x, t[f[3]].y), | |
| (t[f[3]].x, t[f[3]].y), (t[f[0]].x, t[f[0]].y)] | |
| pygame.draw.polygon(self.screen,self.colors[face_index],pointlist) | |
| pygame.display.flip() | |
| except KeyboardInterrupt: | |
| jy_sensor.close() | |
| print("close port") | |
| if __name__ == "__main__": | |
| RealTimeCube().run() | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment