Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- from OpenGL.GL import *
- from OpenGL.GLU import *
- from OpenGL.GLUT import *
- import numpy as np
- import time
- import math
- # zmienne pomocnicze
- pointSize = 10
- windowSize = 100
- pixelMap = [[0.0 for y in range(windowSize)] for x in range(windowSize)]
- tick = 0.0
- # funkcja rysująca zawartość macierzy pixelMap
- def paint():
- glClear(GL_COLOR_BUFFER_BIT)
- glBegin(GL_POINTS)
- glColor3f(1.0, 0.0, 0.0)
- for i in range(windowSize):
- for j in range(windowSize):
- glColor3f(pixelMap[i][j], 1.0, 1.0)
- glVertex2f(0.5 + 1.0 * i, 0.5 + 1.0 * j)
- glEnd()
- glFlush()
- # inicjalizacja okna
- glutInit()
- glutInitWindowSize(windowSize*pointSize, windowSize*pointSize)
- glutInitWindowPosition(0, 0)
- glutCreateWindow(b"Newlist01")
- glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB)
- # inicjalizacja wyświetlania
- glMatrixMode(GL_PROJECTION)
- glLoadIdentity()
- gluOrtho2D(0.0, windowSize, 0.0, windowSize)
- glMatrixMode(GL_MODELVIEW)
- glLoadIdentity()
- glutDisplayFunc(paint)
- glutIdleFunc(paint)
- glClearColor(1.0, 1.0, 1.0, 1.0)
- glEnable(GL_PROGRAM_POINT_SIZE)
- glPointSize(pointSize)
- def cupdate(step=0.1):
- global tick
- ltime = time.clock()
- if ltime < tick + step:
- return False
- tick = ltime
- return True
- def odcinek(x1, y1, x2, y2):
- global pixelMap
- try:
- d = (y2 - y1) / (x2 - x1) # współczynnik kierunkowy
- except ZeroDivisionError:
- d = 0
- if -1 < d < 1:
- if x1 > x2: # rysujemy od lewej do prawej
- xtmp = x1; x1 = x2; x2 = xtmp
- ytmp = y1; y1 = y2; y2 = ytmp
- y = y1 # początkowa wartość y
- for x in range(round(x1), round(x2)+1): # przechodzimy przez piksele od x1 do x2
- y = y + d
- dcx = x
- dcy = round(y)
- if 0 <= dcx < windowSize:
- if 0 <= dcy < windowSize:
- pixelMap[dcx][dcy] = 1.0
- else:
- try:
- d = (x2 - x1) / (y2 - y1)
- except ZeroDivisionError:
- d = 0
- if y1 > y2: # rysujemy od lewej do prawej
- xtmp = x1; x1 = x2; x2 = xtmp
- ytmp = y1; y1 = y2; y2 = ytmp
- x = x1
- for y in range(round(y1), round(y2)):
- x += d
- dcx = round(x)
- dcy = round(y)
- if 0 <= dcx < windowSize and 0 <= dcy < windowSize:
- pixelMap[dcx][dcy] = 1.0
- def prostokat(x, dx, dy, a):
- # Translacja
- x[0] += dx; x[2] += dx; x[4] += dx; x[6] += dx
- x[1] += dy; x[3] += dy; x[5] += dy; x[7] += dy
- # Ze stopni na radiany
- a = math.radians(a)
- # Wyznaczenie środka prostokąta wokół którego będzie następować obrót
- middlePoint = [(x[0] + x[2] + x[4] + x[6])/4, (x[1] + x[3] + x[5] + x[7])/4]
- x[0] = (x[0] - middlePoint[0]) * math.cos(a) - (x[1] - middlePoint[1]) * math.sin(a) + middlePoint[0]
- x[2] = (x[2] - middlePoint[0]) * math.cos(a) - (x[3] - middlePoint[1]) * math.sin(a) + middlePoint[0]
- x[4] = (x[4] - middlePoint[0]) * math.cos(a) - (x[5] - middlePoint[1]) * math.sin(a) + middlePoint[0]
- x[6] = (x[6] - middlePoint[0]) * math.cos(a) - (x[7] - middlePoint[1]) * math.sin(a) + middlePoint[0]
- x[1] = (x[0] - middlePoint[0]) * math.sin(a) + (x[1] - middlePoint[1]) * math.cos(a) + middlePoint[1]
- x[3] = (x[2] - middlePoint[0]) * math.sin(a) + (x[3] - middlePoint[1]) * math.cos(a) + middlePoint[1]
- x[5] = (x[4] - middlePoint[0]) * math.sin(a) + (x[5] - middlePoint[1]) * math.cos(a) + middlePoint[1]
- x[7] = (x[6] - middlePoint[0]) * math.sin(a) + (x[7] - middlePoint[1]) * math.cos(a) + middlePoint[1]
- # Rysowanie odcinków łączących punkty
- odcinek(x[0], x[1], x[2], x[3])
- odcinek(x[2], x[3], x[4], x[5])
- odcinek(x[4], x[5], x[6], x[7])
- odcinek(x[6], x[7], x[0], x[1])
- def punkt(x, y, col):
- global pixelMap
- if 0 <= x <= windowSize:
- if 0 <= y <= windowSize:
- pixelMap[x][y] = col
- def ff(x, u): # równanie stanu
- # return np.array([math.cos(u[0]), math.sin(u[0])]).transpose()
- r = 2
- T = np.array([
- [math.cos(x[2]), 0],
- [math.sin(x[2]), 0],
- [0, 1]
- ])
- w = np.array([
- 1 / 2 * (u[0] + u[1]),
- 1 / r * (u[1] - u[0])
- ])
- return np.dot(T, w)
- def gg(x, u): # wyjście obiektu
- return np.array([x[0] * 10, x[1] * 10]).transpose()
- def uu(t): # wejście obiektu
- return np.array([t])
- def rk4(f, u, x, dt):
- # Runge-Kutta 4
- k1 = dt * f(x, u)
- k2 = dt * f(x + dt/2, u + k1/2)
- k3 = dt * f(x + dt/2, u + k2/2)
- k4 = dt * f(x + dt, u + k3)
- return x + dt * 1/6 * (k1 + 2*k2 + 3*k3 + k4)
- u = [2, 5, 0]
- x = np.array([0, 0, 0]).transpose()
- y = gg(x, uu(tick))
- while True:
- if cupdate() and tick < 100:
- punkt(int(windowSize / 2 + round(y[0])), int(windowSize / 2 + round(y[1])), 0.0)
- x = rk4(ff, u, x, 0.1)
- print(x)
- y = gg(x, uu(tick))
- punkt(int(windowSize / 2 + round(y[0])), int(windowSize / 2 + round(y[1])), 1.0)
- paint()
- glutMainLoopEvent()
- # zad 2
- from OpenGL.GL import *
- from OpenGL.GLU import *
- from OpenGL.GLUT import *
- import time
- import math
- import numpy as np
- from math import sin, cos
- ## zmienne pomocnicze
- pointSize = 10
- windowSize = 100
- pixelMap = [[0.0 for y in range(windowSize)] for x in range(windowSize)]
- tick = 0.0
- ## funkcja rysująca zawartość macierzy pixelMap
- def paint():
- glClear(GL_COLOR_BUFFER_BIT)
- glBegin(GL_POINTS)
- glColor3f(1.0, 0.0, 0.0)
- for i in range(windowSize):
- for j in range(windowSize):
- glColor3f(pixelMap[i][j], 1.0, 1.0)
- glVertex2f(0.5 + 1.0 * i, 0.5 + 1.0 * j)
- glEnd()
- glFlush()
- ## inicjalizacja okna
- glutInit()
- glutInitWindowSize(windowSize*pointSize, windowSize*pointSize)
- glutInitWindowPosition(0, 0)
- glutCreateWindow(b"Newlist01")
- glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB)
- ## inicjalizacja wyświetlania
- glMatrixMode(GL_PROJECTION)
- glLoadIdentity()
- gluOrtho2D(0.0, windowSize, 0.0, windowSize)
- glMatrixMode(GL_MODELVIEW)
- glLoadIdentity()
- glutDisplayFunc(paint)
- glutIdleFunc(paint)
- glClearColor(1.0, 1.0, 1.0, 1.0)
- glEnable(GL_PROGRAM_POINT_SIZE)
- glPointSize(pointSize)
- def clearMap():
- global pixelMap
- pixelMap = [[0.0 for y in range(windowSize)] for x in range(windowSize)]
- def cupdate(step=0.1):
- global tick
- ltime = time.clock()
- if ltime < tick + step:
- return False
- tick = ltime
- return True
- def odcinek(x1, y1, x2, y2):
- global pixelMap
- try:
- d = (y2 - y1) / (x2 - x1) # współczynnik kierunkowy
- except ZeroDivisionError:
- d = 0
- if -1 < d < 1:
- if x1 > x2: # rysujemy od lewej do prawej
- xtmp = x1; x1 = x2; x2 = xtmp
- ytmp = y1; y1 = y2; y2 = ytmp
- y = y1 # początkowa wartość y
- for x in range(round(x1), round(x2)+1): # przechodzimy przez piksele od x1 do x2
- y = y + d
- dcx = x
- dcy = round(y)
- if 0 <= dcx < windowSize:
- if 0 <= dcy < windowSize:
- pixelMap[dcx][dcy] = 1.0
- else:
- try:
- d = (x2 - x1) / (y2 - y1)
- except ZeroDivisionError:
- d = 0
- if y1 > y2: # rysujemy od lewej do prawej
- xtmp = x1; x1 = x2; x2 = xtmp
- ytmp = y1; y1 = y2; y2 = ytmp
- x = x1
- for y in range(round(y1), round(y2)):
- x += d
- dcx = round(x)
- dcy = round(y)
- if 0 <= dcx < windowSize and 0 <= dcy < windowSize:
- pixelMap[dcx][dcy] = 1.0
- def prostokat(x, dx, dy, a):
- # Translacja
- x[0] += dx; x[2] += dx; x[4] += dx; x[6] += dx
- x[1] += dy; x[3] += dy; x[5] += dy; x[7] += dy
- # Ze stopni na radiany
- a = math.radians(a)
- # Wyznaczenie środka prostokąta wokół którego będzie następować obrót
- middlePoint = [(x[0] + x[2] + x[4] + x[6])/4, (x[1] + x[3] + x[5] + x[7])/4]
- p1 = obrotPunktu((x[0], x[1]), middlePoint, a)
- p2 = obrotPunktu((x[2], x[3]), middlePoint, a)
- p3 = obrotPunktu((x[4], x[5]), middlePoint, a)
- p4 = obrotPunktu((x[6], x[7]), middlePoint, a)
- # Rysowanie odcinków łączących punkty
- odcinek(*p1, *p2)
- odcinek(*p2, *p3)
- odcinek(*p3, *p4)
- odcinek(*p4, *p1)
- def obrotPunktu(punkt, punktCentralny, a):
- nowyPunkt = [
- (punkt[0] - punktCentralny[0]) * math.cos(a) - (punkt[1] - punktCentralny[1]) * math.sin(a) + punktCentralny[0],
- (punkt[0] - punktCentralny[0]) * math.sin(a) + (punkt[1] - punktCentralny[1]) * math.cos(a) + punktCentralny[1]
- ]
- return nowyPunkt
- def punkt(x, y, col):
- global pixelMap
- if 0 <= x <= windowSize:
- if 0 <= y <= windowSize:
- pixelMap[x][y] = col
- def ff(x, u):
- # równanie stanu
- r = 5
- T = np.array([
- [math.cos(x[2]), 0],
- [math.sin(x[2]), 0],
- [0, 1]
- ])
- w = np.array([
- 1 / 2 * (u[0] + u[1]),
- 1 / r * (u[1] - u[0])
- ])
- return np.dot(T, w)
- def gg(x, u):
- # wyjście obiektu
- return np.array([x[0] * 10, x[1] * 10]).transpose()
- def uu(t):
- # wejście obiektu
- return np.array([t])
- def rk4(f, u, x, dt):
- # Runge-Kutta 4
- k1 = dt * f(x, u)
- k2 = dt * f(x + (k1/2)/2, u)
- k3 = dt * f(x + (k2/2)/2, u)
- k4 = dt * f(x + k3, u)
- return x + dt * 1/6 * (k1 + 2*k2 + 3*k3 + k4)
- def rysujRobota(x):
- robotSize = 10
- robotRadius = robotSize/2
- corners = []
- corners.append(int(x[0] + robotRadius))
- corners.append(int(x[1] + robotRadius))
- corners.append(int(x[0] + robotRadius))
- corners.append(int(x[1] - robotRadius))
- corners.append(int(x[0] - robotRadius))
- corners.append(int(x[1] - robotRadius))
- corners.append(int(x[0] - robotRadius))
- corners.append(int(x[1] + robotRadius))
- prostokat(corners, 10, 10, x[2])
- # u = uu(tick)
- u = [75, 75, 0]
- x = np.array([windowSize/2, windowSize/2, 279]).transpose() #3 parametr, kąt obrotu
- y = gg(x, u)
- while True:
- if cupdate() and tick < 100:
- clearMap()
- x = rk4(ff, u, x, 0.05)
- print(x)
- y = gg(x, u)
- rysujRobota(x)
- paint()
- glutMainLoopEvent()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement