From a252c564261eca4ae6b6f426bb9d4cc6a093dd25 Mon Sep 17 00:00:00 2001 From: Astatin3 <77305074+Astatin3@users.noreply.github.com> Date: Thu, 14 Mar 2024 20:06:12 -0600 Subject: [PATCH] Robot angle --- src/buttonEditor.py | 208 +++++++++++++++++++++++++++++++++----------- src/pathEditor.py | 26 ++---- src/render.py | 35 ++++++-- 3 files changed, 194 insertions(+), 75 deletions(-) diff --git a/src/buttonEditor.py b/src/buttonEditor.py index 22265d2..8fd15c6 100644 --- a/src/buttonEditor.py +++ b/src/buttonEditor.py @@ -1,54 +1,124 @@ +import math + render = None pathEditor = None bottomBarRect = None ogNodes = [] ogCtrlNodes = [] +ogRotNodes = [] -events = [] +keyFrames = [] matchLength = 15 matchTicks = 15 * 50 -displayTickResolution = 5 +displayTickResolution = 7 displayTicks = round(matchTicks / displayTickResolution) -indicatorBarRect = None +indicatorBarHeight = None selFrame = -1 -# def addTab(i): -# x1 = i * (render.width/(displayTicks)) -# x2 = (render.width/(displayTicks)) -# rect = (x1, bottomBarRect[1], x2, bottomBarRect[3]) - -# def getIsSelected(): -# return False - -# def onClick(pos): -# pass - -# render.addButton(rect, "", getIsSelected, onClick) +def getKeyframeAtPos(index): + for frame in keyFrames: + if frame["timeIndex"] == index: + return frame + return None +def getPosKeyframeAtPos(index): + for frame in keyFrames: + if frame["timeIndex"] == index and frame['type'] == 'position': + return frame + return None + +def getBezierPointCounts(): + counts = [] + dist = 0 + for i in range(1,displayTicks): + frame = getPosKeyframeAtPos(i) + if frame == None: + dist += 1 + else: + counts.append(dist) + dist = 0 + return counts + +def getPosKeyframeByIndex(index): + for frame in keyFrames: + if frame["index"] == index and frame['type'] == 'position': + return frame + return None + +def getSurroundingPosFrames(index): + prevFrame = None + for i in range(index,-1,-1): + frame = getKeyframeAtPos(i) + if frame != None: + prevFrame = frame + break + nextFrame = None + for i in range(index,displayTicks,1): + frame = getKeyframeAtPos(i) + if frame != None: + nextFrame = frame + break + + if nextFrame == None and prevFrame == None: + return prevFrame, nextFrame + elif nextFrame == None: + return prevFrame, prevFrame + elif prevFrame == None: + return nextFrame, nextFrame + + return prevFrame, nextFrame + +def getRobotAtIndex(index): + prevFrame, nextFrame = getSurroundingPosFrames(index) + + if prevFrame['timeIndex'] - nextFrame['timeIndex'] == 0: + return prevFrame['position'], prevFrame['rotation'] + + relPos = -((prevFrame['timeIndex'] - index)/(nextFrame['timeIndex'] - prevFrame['timeIndex']-1)) + + pos = calcBezierPoint(prevFrame['position'], ogCtrlNodes[prevFrame['index']], nextFrame['position'], relPos) + + if prevFrame['rotation'] - nextFrame['rotation'] < -math.pi: + rot = ((nextFrame['rotation']-prevFrame['rotation']-math.pi*2)*relPos) + prevFrame['rotation'] + elif prevFrame['rotation'] - nextFrame['rotation'] > math.pi: + rot = ((nextFrame['rotation']-prevFrame['rotation']+math.pi*2)*relPos) + prevFrame['rotation'] + else: + rot = ((nextFrame['rotation']-prevFrame['rotation'])*relPos) + prevFrame['rotation'] + + + return pos, rot + def getTimeBarColor(index): - for event in events: - if event["timeIndex"] == index: - if event['type'] == 'position': - return (127,127,0) - elif event['type'] == 'controller': - return (0,127,0) + frame = getKeyframeAtPos(index) + if frame == None: + return (0,0,0) + if frame['type'] == 'position': + return (127,127,0) + elif frame['type'] == 'controller': + return (0,127,0) return (16,16,32) -def renderSelectIndicator(i): - x1 = i * (render.width/(displayTicks)) - x2 = (render.width/(displayTicks)) - rect = (x1, indicatorBarRect[1], x2, indicatorBarRect[3]) - - if i == selFrame: - render.drawrect((255,0,0), rect) - - +# def renderSelectIndicator(i): +# if i == selFrame: +# x1 = i * (render.width/(displayTicks)) +# x2 = (render.width/(displayTicks)) + +# # render.drawrect((255,0,0), (x1, bottomBarRect[1], x2, indicatorBarHeight)) +# # render.drawrect((255,0,0), (x1, render.screen.get_height()-indicatorBarHeight, x2, indicatorBarHeight)) + +# render.drawrect((255,0,0), (x1, bottomBarRect[1]+((bottomBarRect[3]-indicatorBarHeight)/2), x2, indicatorBarHeight)) + +# # render.drawrect((255,0,0), rect) +def calcBezierPoint(p0, p1, p2, t): + 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 + return (px, py) def reloadBar(pos): @@ -71,7 +141,7 @@ def reloadBar(pos): toggle = not toggle render.drawrect(color, rect) - renderSelectIndicator(i) + # renderSelectIndicator(i) render.update() def clickBar(pos): @@ -83,7 +153,6 @@ def clickBar(pos): if render.isInRect(pos, rect): global selFrame selFrame = i - reloadBar(pos) return class buttonEditor: @@ -91,27 +160,39 @@ class buttonEditor: def __init__(self, tmprender, tmppathEditor): global render - render = tmprender global pathEditor + render = tmprender pathEditor = tmppathEditor - global indicatorBarRect + global indicatorBarHeight indicatorBarHeight = round(render.screen.get_width()/displayTicks) - indicatorBarRect = (0, render.screen.get_height()-indicatorBarHeight, render.screen.get_width(), indicatorBarHeight) global bottomBarRect - bottomBarRect = (0, (render.screen.get_height()-render.bottomBarHeight), render.screen.get_width(), render.screen.get_height()) + bottomBarRect = (0, (render.screen.get_height()-render.bottomBarHeight), render.screen.get_width(), render.bottomBarHeight) def refresh(self): + global ogNodes + global ogCtrlNodes + global ogRotNodes + render.clear() render.drawField() - render.renderBezier(pathEditor.nodes, pathEditor.curveEditPoints) + + pointCounts = getBezierPointCounts() + for i in range(0,len(ogCtrlNodes)): + render.bezier(ogNodes[i], ogCtrlNodes[i], ogNodes[i+1], pointCounts[i]) + + if selFrame != -1 and len(ogNodes) > 0: + pos, rot = getRobotAtIndex(selFrame) + render.robotSquare(pos, rot) + reloadBar((0,0)) render.update() def mouseDown(self, pos): if pos[1] > bottomBarRect[1]: clickBar(pos) + self.refresh() pass def mouseUp(self, pos): @@ -124,25 +205,54 @@ class buttonEditor: pass def keyDown(self, key): - pass - - def load(self): - + global selFrame + if key == render.pg.K_LEFT and selFrame > 0: + selFrame -= 1 + self.refresh() + elif key == render.pg.K_RIGHT and selFrame < displayTicks-1: + selFrame += 1 + self.refresh() + + def updateNodes(self, loadKeyframes): global ogNodes global ogCtrlNodes + global ogRotNodes + ogNodes = pathEditor.nodes.copy() + ogCtrlNodes = pathEditor.curveEditPoints.copy() + ogRotNodes = pathEditor.nodeRotations.copy() + + if not loadKeyframes: + return + + for i in range(len(ogNodes)): + frame = getPosKeyframeByIndex(i) + frame['position'] = ogNodes[i] + frame['rotation'] = ogRotNodes[i] + + def load(self): global selFrame selFrame = -1 - # for i in range(displayTicks): - # addTab(i) + global ogNodes + global ogCtrlNodes + global ogRotNodes - if ogNodes != pathEditor.nodes or \ - ogCtrlNodes != pathEditor.curveEditPoints: + if len(ogNodes) != len(pathEditor.nodes): - ogNodes = pathEditor.nodes - ogCtrlNodes = pathEditor.curveEditPoints + global keyFrames + keyFrames = [] - # for i in range(len(ogNodes)): - # events + self.updateNodes(False) + + for i in range(len(ogNodes)): + keyFrames.append({ + "type": "position", + "timeIndex": round(((i)/len(ogNodes)) * (displayTicks-1)), + "index": i, + "position": ogNodes[i], + "rotation": ogRotNodes[i] + }) + else: + self.updateNodes(True) self.refresh() \ No newline at end of file diff --git a/src/pathEditor.py b/src/pathEditor.py index a892974..ffd0373 100644 --- a/src/pathEditor.py +++ b/src/pathEditor.py @@ -8,16 +8,14 @@ rotNodeDist = 35 rotNodeColor = (255, 0, 255) rotNodeRadius = 8 -nodeSquareRadius = 35 -nodeSquareColor = (127, 127, 127, 0.5) -nodeSquareWidth = 3 - lineApproximationLineColor = (127, 127, 127, 0.5) lineApproximationLineWidth = 3 curveEditPointColor = (0, 255, 255) curveEditPointRadius = 8 +curvePointCount = 80 + nodes = [] curveEditPoints = [] nodeRotations = [] @@ -31,31 +29,19 @@ render = None def refresh(): render.clear() render.drawField() - render.renderBezier(nodes, curveEditPoints) for i in range(0,len(curveEditPoints)): render.line(lineApproximationLineColor, nodes[i], curveEditPoints[i], lineApproximationLineWidth) render.line(lineApproximationLineColor, curveEditPoints[i], nodes[i+1], lineApproximationLineWidth) - #bezier(nodes[i], curveEditPoints[i], nodes[i+1]) + + render.bezier(nodes[i], curveEditPoints[i], nodes[i+1], curvePointCount) + render.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] render.circle(rotNodeColor, (posX, posY), rotNodeRadius) - - rect1 = ((math.sin(nodeRotations[i] + math.pi*-0.25)*nodeSquareRadius) + nodes[i][0], - (math.cos(nodeRotations[i] + math.pi*-0.25)*nodeSquareRadius) + nodes[i][1]) - rect2 = ((math.sin(nodeRotations[i] + math.pi*0.25)*nodeSquareRadius) + nodes[i][0], - (math.cos(nodeRotations[i] + math.pi*0.25)*nodeSquareRadius) + nodes[i][1]) - rect3 = ((math.sin(nodeRotations[i] + math.pi*0.75)*nodeSquareRadius) + nodes[i][0], - (math.cos(nodeRotations[i] + math.pi*0.75)*nodeSquareRadius) + nodes[i][1]) - rect4 = ((math.sin(nodeRotations[i] + math.pi*1.25)*nodeSquareRadius) + nodes[i][0], - (math.cos(nodeRotations[i] + math.pi*1.25)*nodeSquareRadius) + nodes[i][1]) - - render.line(nodeSquareColor, rect1, rect2, nodeSquareWidth) - render.line(nodeSquareColor, rect2, rect3, nodeSquareWidth) - render.line(nodeSquareColor, rect3, rect4, nodeSquareWidth) - render.line(nodeSquareColor, rect4, rect1, nodeSquareWidth) + render.robotSquare(nodes[i], nodeRotations[i]) for pos in nodes: render.circle(nodeColor, pos, nodeRadius) diff --git a/src/render.py b/src/render.py index 979c7d8..3cef231 100644 --- a/src/render.py +++ b/src/render.py @@ -2,13 +2,18 @@ import math from pygame.locals import * import numpy as np -curvePointCount = 300 curvePointColor = (255, 255, 0) curvePointRadius = 2 selTabBorderSize = 2 selTabBorderIndent = 3 +nodeSquareRadius = 35 +nodeSquareColor = (127, 127, 127, 0.5) +nodeSquareWidth = 3 + +nodeTickLength = 5 + class render(): def __init__(self, pg, screen, topBarHeight, bottomBarHeight): self.pg = pg @@ -44,8 +49,30 @@ class render(): pos[1] >= rect[1] and \ pos[1] <= rect[1]+rect[3] + def robotSquare(self, pos, rot): + pos1 = ((math.sin(rot + math.pi*-0.25)*nodeSquareRadius) + pos[0], + (math.cos(rot + math.pi*-0.25)*nodeSquareRadius) + pos[1]) + pos2 = ((math.sin(rot + math.pi*0.25)*nodeSquareRadius) + pos[0], + (math.cos(rot + math.pi*0.25)*nodeSquareRadius) + pos[1]) + pos3 = ((math.sin(rot + math.pi*0.75)*nodeSquareRadius) + pos[0], + (math.cos(rot + math.pi*0.75)*nodeSquareRadius) + pos[1]) + pos4 = ((math.sin(rot + math.pi*1.25)*nodeSquareRadius) + pos[0], + (math.cos(rot + math.pi*1.25)*nodeSquareRadius) + pos[1]) - def bezier(self, p0, p1, p2): + pos5 = ((math.sin(rot)*(nodeSquareRadius+nodeTickLength)) + pos[0], + (math.cos(rot)*(nodeSquareRadius+nodeTickLength)) + pos[1]) + pos6 = ((math.sin(rot)*(nodeSquareRadius-nodeTickLength)) + pos[0], + (math.cos(rot)*(nodeSquareRadius-nodeTickLength)) + pos[1]) + + self.line(nodeSquareColor, pos1, pos2, nodeSquareWidth) + self.line(nodeSquareColor, pos2, pos3, nodeSquareWidth) + self.line(nodeSquareColor, pos3, pos4, nodeSquareWidth) + self.line(nodeSquareColor, pos4, pos1, nodeSquareWidth) + + self.line(nodeSquareColor, pos5, pos6, nodeSquareWidth) + + + def bezier(self, p0, p1, p2, curvePointCount): #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): @@ -70,10 +97,6 @@ class render(): if elem['type'] == 'button' and self.isInRect(pos, elem['rect']): elem['onClick'](pos) - def renderBezier(self, nodes, curveEditPoints): - for i in range(0,len(curveEditPoints)): - self.bezier(nodes[i], curveEditPoints[i], nodes[i+1]) - def update(self): self.pg.display.update()