mirror of
https://github.com/Astatin3/autoPlanner.git
synced 2026-06-08 15:48:00 -06:00
Make modular
This commit is contained in:
Michael Mikovsky
@@ -4,138 +4,66 @@ from pygame.locals import *
|
||||
from sys import exit
|
||||
import numpy as np
|
||||
|
||||
import src.pathRenderer as pathRenderer
|
||||
import src.pathEditor as pathEditor
|
||||
|
||||
doubleClickDuration = 200
|
||||
|
||||
nodeColor = (255, 255, 255)
|
||||
nodeRadius = 15
|
||||
|
||||
lineApproximationLineColor = (127, 127, 127, 0.5)
|
||||
lineApproximationLineWidth = 3
|
||||
|
||||
curvePointCount = 300
|
||||
curvePointColor = (255, 255, 0)
|
||||
curvePointRadius = 2
|
||||
|
||||
curveEditPointColor = (0, 255, 255)
|
||||
curveEditPointRadius = 5
|
||||
|
||||
pg.init()
|
||||
|
||||
# fieldImg = Background("frc2024.png", (0, 0))
|
||||
topBarHeight = 40
|
||||
bottomBarHeight = 40
|
||||
|
||||
fieldImg = pg.image.load("frc2024.png")
|
||||
|
||||
screen_width = fieldImg.get_width()
|
||||
screen_height = fieldImg.get_height()
|
||||
screen_width = 1200
|
||||
screen_height = (screen_width * (643/1286)) + topBarHeight + bottomBarHeight
|
||||
|
||||
screen = pg.display.set_mode((screen_width, screen_height))
|
||||
fieldImg = pg.image.load("frc2024.png").convert_alpha()
|
||||
pg.display.set_caption("Auto Planner")
|
||||
|
||||
pathR = pathRenderer.pathRenderer(pg, screen, topBarHeight)
|
||||
|
||||
curveEditPoints = []
|
||||
nodes = []
|
||||
tabIndex = 0
|
||||
|
||||
def addNode(pos):
|
||||
nodes.append(pos)
|
||||
if len(nodes) > 1:
|
||||
index = len(nodes)-1
|
||||
# Middle point between current point and previous point
|
||||
editPos = (nodes[index-1][0]+pos[0])/2,(nodes[index-1][1]+pos[1])/2
|
||||
curveEditPoints.append(editPos)
|
||||
tabs = [
|
||||
pathEditor.pathEditor(pg, pathR)
|
||||
]
|
||||
|
||||
def bezier(p0, p1, p2):
|
||||
#for p in [p0, p1, p2]:
|
||||
# pg.draw.circle(screen, (255, 255, 255), p, 5)
|
||||
for t in np.arange(0, 1, 1/curvePointCount):
|
||||
px = p0[0]*(1-t)**2 + 2*(1-t)*t*p1[0] + p2[0]*t**2
|
||||
py = p0[1]*(1-t)**2 + 2*(1-t)*t*p1[1] + p2[1]*t**2
|
||||
pg.draw.circle(screen, curvePointColor, (px, py), curvePointRadius)
|
||||
|
||||
def refresh():
|
||||
pg.draw.rect(screen, (0, 0, 0), (0, 0, screen_width, screen_height))
|
||||
screen.blit(fieldImg, screen.get_rect())
|
||||
for i in range(0,len(curveEditPoints)):
|
||||
pg.draw.line(screen, lineApproximationLineColor, nodes[i], curveEditPoints[i], lineApproximationLineWidth)
|
||||
pg.draw.line(screen, lineApproximationLineColor, curveEditPoints[i], nodes[i+1], lineApproximationLineWidth)
|
||||
bezier(nodes[i], curveEditPoints[i], nodes[i+1])
|
||||
pg.draw.circle(screen, curveEditPointColor, curveEditPoints[i], curveEditPointRadius)
|
||||
for pos in nodes:
|
||||
pg.draw.circle(screen, nodeColor, pos, nodeRadius)
|
||||
pg.display.update()
|
||||
|
||||
def getElemAt(pos):
|
||||
for i in range(0,len(curveEditPoints)):
|
||||
if getDist(pos, curveEditPoints[i], curveEditPointRadius):
|
||||
return 1, i
|
||||
for i in range(0,len(nodes)):
|
||||
if getDist(pos, nodes[i], nodeRadius):
|
||||
return 0, i
|
||||
return -1, -1
|
||||
|
||||
def getDist(pos1, pos2, dist):
|
||||
return math.sqrt(math.pow(pos1[0]-pos2[0], 2) + math.pow(pos1[1]-pos2[1], 2)) <= dist
|
||||
|
||||
def singleClick():
|
||||
pos = pg.mouse.get_pos()
|
||||
addNode(pos)
|
||||
refresh()
|
||||
|
||||
def doubleClick():
|
||||
curveEditPoints = []
|
||||
nodes = []
|
||||
refresh()
|
||||
|
||||
refresh()
|
||||
pass
|
||||
|
||||
running = True
|
||||
last_click = -1
|
||||
|
||||
clickType = -1
|
||||
clickIndex = -1
|
||||
# clickType = -1
|
||||
# clickIndex = -1
|
||||
|
||||
def offsetPos(pos):
|
||||
return (pos[0],pos[1])
|
||||
|
||||
while running:
|
||||
for event in pg.event.get():
|
||||
|
||||
if event.type == pg.MOUSEBUTTONDOWN:
|
||||
clickType, clickIndex = getElemAt(pg.mouse.get_pos())
|
||||
pos = pg.mouse.get_pos()
|
||||
if pos[1] > topBarHeight and pos[1] < (screen.get_width()-bottomBarHeight):
|
||||
now = pg.time.get_ticks()
|
||||
if now - last_click <= doubleClickDuration:
|
||||
tabs[tabIndex].doubleClick(offsetPos(pos))
|
||||
else:
|
||||
tabs[tabIndex].mouseDown(offsetPos(pos))
|
||||
last_click = pg.time.get_ticks()
|
||||
|
||||
now = pg.time.get_ticks()
|
||||
if now - last_click <= doubleClickDuration:
|
||||
if clickType == -1:
|
||||
pass
|
||||
if clickType == 0:
|
||||
print(nodes)
|
||||
if clickIndex > 0:
|
||||
if clickIndex < len(nodes)-1:
|
||||
newPos = (nodes[clickIndex-1][0]+nodes[clickIndex][0])/2,(nodes[clickIndex-1][1]+nodes[clickIndex][1])/2
|
||||
curveEditPoints[clickIndex] = newPos
|
||||
curveEditPoints.pop(clickIndex-1)
|
||||
elif clickIndex == 0 and len(nodes) > 1:
|
||||
curveEditPoints.pop(clickIndex)
|
||||
# if len(nodes) != 1:
|
||||
nodes.pop(clickIndex)
|
||||
# else: nodes = []
|
||||
refresh()
|
||||
|
||||
|
||||
else:
|
||||
if clickType == -1:
|
||||
singleClick()
|
||||
last_click = pg.time.get_ticks()
|
||||
|
||||
if event.type == pg.MOUSEMOTION and clickType != -1:
|
||||
if clickType == 0:
|
||||
nodes[clickIndex] = pg.mouse.get_pos()
|
||||
if clickType == 1:
|
||||
curveEditPoints[clickIndex] = pg.mouse.get_pos()
|
||||
refresh()
|
||||
if event.type == pg.MOUSEMOTION:
|
||||
pos = pg.mouse.get_pos()
|
||||
if pos[1] > topBarHeight and pos[1] < (screen.get_width()-bottomBarHeight):
|
||||
tabs[tabIndex].mouseMove(offsetPos(pos))
|
||||
|
||||
if event.type == pg.MOUSEBUTTONUP:
|
||||
if clickType != -1:
|
||||
clickType = -1
|
||||
clickIndex = -1
|
||||
pos = pg.mouse.get_pos()
|
||||
if pos[1] > topBarHeight and pos[1] < (screen.get_width()-bottomBarHeight):
|
||||
tabs[tabIndex].mouseUp(offsetPos(pos))
|
||||
|
||||
if event.type == pg.QUIT:
|
||||
running = False
|
||||
|
||||
pg.quit()
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
# class menu:
|
||||
@@ -0,0 +1,137 @@
|
||||
import math
|
||||
from pygame.locals import *
|
||||
|
||||
nodeColor = (255, 255, 255)
|
||||
nodeRadius = 15
|
||||
|
||||
rotNodeDist = 30
|
||||
rotNodeColor = (255, 0, 255)
|
||||
rotNodeRadius = 5
|
||||
|
||||
lineApproximationLineColor = (127, 127, 127, 0.5)
|
||||
lineApproximationLineWidth = 3
|
||||
|
||||
curveEditPointColor = (0, 255, 255)
|
||||
curveEditPointRadius = 5
|
||||
|
||||
name = "Path Editor"
|
||||
|
||||
nodes = []
|
||||
curveEditPoints = []
|
||||
nodeRotations = []
|
||||
|
||||
clickType = -1
|
||||
clickIndex = -1
|
||||
|
||||
pg = None
|
||||
pathRenderer = None
|
||||
|
||||
|
||||
def refresh():
|
||||
pathRenderer.render(nodes, curveEditPoints)
|
||||
|
||||
for i in range(0,len(curveEditPoints)):
|
||||
pathRenderer.line(lineApproximationLineColor, nodes[i], curveEditPoints[i], lineApproximationLineWidth)
|
||||
pathRenderer.line(lineApproximationLineColor, curveEditPoints[i], nodes[i+1], lineApproximationLineWidth)
|
||||
#bezier(nodes[i], curveEditPoints[i], nodes[i+1])
|
||||
pathRenderer.circle(curveEditPointColor, curveEditPoints[i], curveEditPointRadius)
|
||||
for i in range(0,len(nodeRotations)):
|
||||
posX = (math.sin(nodeRotations[i])*rotNodeDist) + nodes[i][0]
|
||||
posY = (math.cos(nodeRotations[i])*rotNodeDist) + nodes[i][1]
|
||||
pathRenderer.circle(rotNodeColor, (posX, posY), rotNodeRadius)
|
||||
for pos in nodes:
|
||||
pathRenderer.circle(nodeColor, pos, nodeRadius)
|
||||
pg.display.update()
|
||||
|
||||
def getElemAt(pos):
|
||||
for i in range(0,len(curveEditPoints)):
|
||||
if getDist(pos, curveEditPoints[i], curveEditPointRadius):
|
||||
return 1, i
|
||||
for i in range(0,len(nodeRotations)):
|
||||
posX = (math.sin(nodeRotations[i])*rotNodeDist) + nodes[i][0]
|
||||
posY = (math.cos(nodeRotations[i])*rotNodeDist) + nodes[i][1]
|
||||
if getDist(pos, (posX, posY), nodeRadius):
|
||||
return 2, i
|
||||
for i in range(0,len(nodes)):
|
||||
if getDist(pos, nodes[i], nodeRadius):
|
||||
return 0, i
|
||||
return -1, -1
|
||||
|
||||
def getDist(pos1, pos2, dist):
|
||||
return math.sqrt(math.pow(pos1[0]-pos2[0], 2) + math.pow(pos1[1]-pos2[1], 2)) <= dist
|
||||
|
||||
def addNode(pos):
|
||||
nodes.append(pos)
|
||||
nodeRotations.append(math.pi/2)
|
||||
if len(nodes) > 1:
|
||||
index = len(nodes)-1
|
||||
# Middle point between current point and previous point
|
||||
editPos = (nodes[index-1][0]+pos[0])/2,(nodes[index-1][1]+pos[1])/2
|
||||
curveEditPoints.append(editPos)
|
||||
refresh()
|
||||
|
||||
def nearestCirclePoint(center, pos, R):
|
||||
vX = pos[0] - center[0]
|
||||
vY = pos[1] - center[1]
|
||||
magV = math.sqrt(vX*vX + vY*vY)
|
||||
aX = center[0] + vX / magV * R
|
||||
aY = center[1] + vY / magV * R
|
||||
return (aX, aY)
|
||||
|
||||
def points2rad(center, pos):
|
||||
diffX = center[0] - pos[0]
|
||||
diffY = center[1] - pos[1]
|
||||
return -math.atan2(diffY, diffX) - (math.pi/2)
|
||||
|
||||
class pathEditor:
|
||||
|
||||
def __init__(self, tmppg, tmppathRenderer):
|
||||
global pg
|
||||
pg = tmppg
|
||||
# global screen
|
||||
# screen = tmpscreen
|
||||
global pathRenderer
|
||||
pathRenderer = tmppathRenderer
|
||||
|
||||
refresh()
|
||||
|
||||
def mouseDown(self, pos):
|
||||
global clickType
|
||||
global clickIndex
|
||||
clickType, clickIndex = getElemAt(pos)
|
||||
if clickType == -1:
|
||||
addNode(pos)
|
||||
|
||||
def mouseUp(self, pos):
|
||||
global clickType
|
||||
global clickIndex
|
||||
if clickType != -1:
|
||||
clickType = -1
|
||||
clickIndex = -1
|
||||
|
||||
def mouseMove(self, pos):
|
||||
if clickType != -1:
|
||||
if clickType == 0:
|
||||
nodes[clickIndex] = pos
|
||||
if clickType == 1:
|
||||
curveEditPoints[clickIndex] = pos
|
||||
if clickType == 2:
|
||||
nodeRotations[clickIndex] = points2rad(nodes[clickIndex], nearestCirclePoint(nodes[clickIndex], pos, rotNodeDist))
|
||||
refresh()
|
||||
|
||||
def doubleClick(self, pos):
|
||||
clickType, clickIndex = getElemAt(pg.mouse.get_pos())
|
||||
if clickType == -1:
|
||||
pass
|
||||
if clickType == 0:
|
||||
if clickIndex > 0:
|
||||
if clickIndex < len(nodes)-1:
|
||||
newPos = (nodes[clickIndex-1][0]+nodes[clickIndex][0])/2,(nodes[clickIndex-1][1]+nodes[clickIndex][1])/2
|
||||
curveEditPoints[clickIndex] = newPos
|
||||
curveEditPoints.pop(clickIndex-1)
|
||||
elif clickIndex == 0 and len(nodes) > 1:
|
||||
curveEditPoints.pop(clickIndex)
|
||||
nodes.pop(clickIndex)
|
||||
nodeRotations.pop(clickIndex)
|
||||
refresh()
|
||||
|
||||
@@ -0,0 +1,48 @@
|
||||
import math
|
||||
from pygame.locals import *
|
||||
import numpy as np
|
||||
|
||||
curvePointCount = 300
|
||||
curvePointColor = (255, 255, 0)
|
||||
curvePointRadius = 2
|
||||
|
||||
class pathRenderer():
|
||||
def __init__(self, pg, screen, offsetY):
|
||||
self.pg = pg
|
||||
self.screen = screen
|
||||
|
||||
self.offsetY = offsetY
|
||||
self.width = self.screen.get_width()
|
||||
self.height = self.screen.get_width() * (643/1286)
|
||||
self.rect = (0, self.offsetY, self.width, self.height)
|
||||
|
||||
self.fieldImg = pg.image.load("frc2024.png").convert_alpha()
|
||||
|
||||
self.offsetSize = self.fieldImg.get_width() / self.width
|
||||
|
||||
self.fieldImg = pg.transform.scale(self.fieldImg, (self.width, self.height))
|
||||
|
||||
def line(self, color, pos1, pos2, width):
|
||||
self.pg.draw.line(self.screen, color, pos1, pos2, round(width/self.offsetSize))
|
||||
|
||||
def circle(self, color, pos, radius):
|
||||
self.pg.draw.circle(self.screen, color, pos, radius/self.offsetSize)
|
||||
|
||||
def bezier(self, p0, p1, p2):
|
||||
#for p in [p0, p1, p2]:
|
||||
# pg.draw.circle(self.screen, (255, 255, 255), p, 5)
|
||||
for t in np.arange(0, 1, 1/curvePointCount):
|
||||
px = p0[0]*(1-t)**2 + 2*(1-t)*t*p1[0] + p2[0]*t**2
|
||||
py = p0[1]*(1-t)**2 + 2*(1-t)*t*p1[1] + p2[1]*t**2
|
||||
self.circle(curvePointColor, (px, py), curvePointRadius)
|
||||
|
||||
def render(self, nodes, curveEditPoints):
|
||||
self.pg.draw.rect(self.screen, (0, 0, 0), self.rect)
|
||||
self.screen.blit(self.fieldImg, self.rect)
|
||||
for i in range(0,len(curveEditPoints)):
|
||||
# self.pg.draw.line(self.screen, lineApproximationLineColor, nodes[i], curveEditPoints[i], lineApproximationLineWidth)
|
||||
# self.pg.draw.line(self.screen, lineApproximationLineColor, curveEditPoints[i], nodes[i+1], lineApproximationLineWidth)
|
||||
self.bezier(nodes[i], curveEditPoints[i], nodes[i+1])
|
||||
# self.pg.draw.circle(self.screen, curveEditPointColor, curveEditPoints[i], curveEditPointRadius)
|
||||
self.pg.display.update()
|
||||
|
||||
Reference in New Issue
Block a user