Robot angle

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):
+def getKeyframeAtPos(index):
-#     x1 = i * (render.width/(displayTicks))
+  for frame in keyFrames:
-#     x2 = (render.width/(displayTicks))
+      if frame["timeIndex"] == index:
-#     rect = (x1, bottomBarRect[1], x2, bottomBarRect[3])
+        return frame
-    
+  return None
-#     def getIsSelected():
-#       return False
-    
-#     def onClick(pos):
-#       pass
-    
-#     render.addButton(rect, "", getIsSelected, onClick)
 
+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:
+  frame = getKeyframeAtPos(index)
-    if event["timeIndex"] == index:
+  if frame == None:
-      if event['type'] == 'position':
+    return (0,0,0)
-        return (127,127,0)
+  if frame['type'] == 'position':
-      elif event['type'] == 'controller':
+    return (127,127,0)
-        return (0,127,0)
+  elif frame['type'] == 'controller':
+    return (0,127,0)
       
   return (16,16,32)
 
-def renderSelectIndicator(i):
+# def renderSelectIndicator(i):
-  x1 = i * (render.width/(displayTicks))
+#   if i == selFrame:
-  x2 = (render.width/(displayTicks))
+#     x1 = i * (render.width/(displayTicks))
-  rect = (x1, indicatorBarRect[1], x2, indicatorBarRect[3])
+#     x2 = (render.width/(displayTicks))
-  
+    
-  if i == selFrame:
+#     # render.drawrect((255,0,0), (x1, bottomBarRect[1], x2, indicatorBarHeight))
-    render.drawrect((255,0,0), rect)
+#     # 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
+    global selFrame
-
+    if key == render.pg.K_LEFT and selFrame > 0:
-  def load(self):
+      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):
+    global ogNodes
-    #   addTab(i)
+    global ogCtrlNodes
+    global ogRotNodes
     
-    if ogNodes != pathEditor.nodes or \
+    if len(ogNodes) != len(pathEditor.nodes):
-    ogCtrlNodes != pathEditor.curveEditPoints:
       
-      ogNodes = pathEditor.nodes
+      global keyFrames
-      ogCtrlNodes = pathEditor.curveEditPoints
+      keyFrames = []
       
-      # for i in range(len(ogNodes)):
+      self.updateNodes(False)
-      #   events
+      
+      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()

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)
-
+    render.robotSquare(nodes[i], nodeRotations[i])
-    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)
   for pos in nodes:
 
     render.circle(nodeColor, pos, nodeRadius)

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()