| John Park | 91e6973 | 2019-03-03 13:12:43 -0800 | [diff] [blame] | 1 | from constants import * |
| 2 | import numpy as np |
| 3 | from libspline import Spline, DistanceSpline, Trajectory |
| 4 | |
| 5 | |
| 6 | class Points(): |
| 7 | def __init__(self): |
| 8 | self.points = [] # Holds all points not yet in spline |
| 9 | self.libsplines = [] # Formatted for libspline library usage |
| 10 | self.splines = [] # Formatted for drawing |
| 11 | |
| 12 | def getPoints(self): |
| 13 | return self.points |
| 14 | |
| 15 | def resetPoints(self): |
| 16 | self.points = [] |
| 17 | |
| 18 | def getLibsplines(self): |
| 19 | return self.libsplines |
| 20 | |
| 21 | def splineExtrapolate(self, o_spline_edit): |
| 22 | spline_edit = o_spline_edit |
| 23 | if not spline_edit == len(self.splines) - 1: |
| 24 | spline_edit = spline_edit + 1 |
| 25 | f = self.splines[spline_edit][5] |
| 26 | e = self.splines[spline_edit][4] |
| 27 | d = self.splines[spline_edit][3] |
| 28 | self.splines[spline_edit][0] = f |
| 29 | self.splines[spline_edit][1] = f * 2 + e * -1 |
| 30 | self.splines[spline_edit][2] = d + f * 4 + e * -4 |
| 31 | |
| 32 | if not spline_edit == 0: |
| 33 | spline_edit = spline_edit - 1 |
| 34 | a = self.splines[spline_edit][0] |
| 35 | b = self.splines[spline_edit][1] |
| 36 | c = self.splines[spline_edit][2] |
| 37 | self.splines[spline_edit][5] = a |
| 38 | self.splines[spline_edit][4] = a * 2 + b * -1 |
| 39 | self.splines[spline_edit][3] = c + a * 4 + b * -4 |
| 40 | |
| 41 | return spline_edit |
| 42 | |
| 43 | def updates_for_mouse_move(self, index_of_edit, spline_edit, x, y, difs): |
| 44 | if index_of_edit > -1: |
| 45 | self.splines[spline_edit][index_of_edit] = [pxToM(x), pxToM(y)] |
| 46 | |
| 47 | if index_of_edit == 5: |
| 48 | self.splines[spline_edit][ |
| 49 | index_of_edit - |
| 50 | 2] = self.splines[spline_edit][index_of_edit - 2] + difs |
| 51 | self.splines[spline_edit][ |
| 52 | index_of_edit - |
| 53 | 1] = self.splines[spline_edit][index_of_edit - 1] + difs |
| 54 | |
| 55 | if index_of_edit == 0: |
| 56 | self.splines[spline_edit][ |
| 57 | index_of_edit + |
| 58 | 2] = self.splines[spline_edit][index_of_edit + 2] + difs |
| 59 | self.splines[spline_edit][ |
| 60 | index_of_edit + |
| 61 | 1] = self.splines[spline_edit][index_of_edit + 1] + difs |
| 62 | |
| 63 | if index_of_edit == 4: |
| 64 | self.splines[spline_edit][ |
| 65 | index_of_edit - |
| 66 | 1] = self.splines[spline_edit][index_of_edit - 1] + difs |
| 67 | |
| 68 | if index_of_edit == 1: |
| 69 | self.splines[spline_edit][ |
| 70 | index_of_edit + |
| 71 | 1] = self.splines[spline_edit][index_of_edit + 1] + difs |
| 72 | |
| 73 | return self.splineExtrapolate(spline_edit) |
| 74 | |
| 75 | def update_lib_spline(self): |
| 76 | self.libsplines = [] |
| 77 | array = np.zeros(shape=(6, 2), dtype=float) |
| 78 | for points in self.splines: |
| 79 | for j, point in enumerate(points): |
| 80 | array[j, 0] = point[0] |
| 81 | array[j, 1] = point[1] |
| 82 | spline = Spline(np.ascontiguousarray(np.transpose(array))) |
| 83 | self.libsplines.append(spline) |
| 84 | |
| 85 | def getSplines(self): |
| 86 | return self.splines |
| 87 | |
| 88 | def resetSplines(self): |
| 89 | self.splines = [] |
| 90 | |
| 91 | def setUpSplines(self, newSplines): |
| 92 | self.splines = newSplines |
| 93 | |
| 94 | def setSplines(self, spline_edit, index_of_edit, x, y): |
| 95 | self.splines[spline_edit][index_of_edit] = [x, y] |
| 96 | |
| 97 | def add_point(self, x, y): |
| 98 | if (len(self.points) < 6): |
| 99 | self.points.append([pxToM(x), pxToM(y)]) |
| 100 | if (len(self.points) == 6): |
| 101 | self.splines.append(np.array(self.points)) |
| 102 | self.points = [] |
| 103 | self.update_lib_spline() |
| 104 | return True |
| 105 | |
| 106 | def extrapolate(self, point1, point2, |
| 107 | point3): # where point3 is 3rd to last point |
| 108 | self.points.append(point1) |
| 109 | self.points.append(point1 * 2 - point2) |
| 110 | self.points.append(point3 + point1 * 4 - point2 * 4) |