Last active
February 23, 2022 14:52
-
-
Save soulslicer/1224bfc6a81f25835054cadf18325251 to your computer and use it in GitHub Desktop.
B Spline Pure Numpy
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import numpy as np | |
import matplotlib.patches as patches | |
class BSpline(): | |
def __init__(self): | |
pass | |
self.warp = 4. | |
self.count = 26*2 | |
def process(self, Px, Py, Pw, ax, MODE=1): | |
self.mode = MODE # Can be 0 or 1 for terminal tangents | |
self.ax = ax | |
self.Px = Px | |
self.Py = Py | |
self.Pw = Pw | |
count = self.count | |
if self.mode == 0: | |
self.n = len(Px) - 2 | |
else: | |
self.n = len(Px) - 2 | |
self.n1 = self.n+1; | |
self.B0 = [None]*count | |
self.B1 = [None]*count | |
self.B2 = [None]*count | |
self.B3 = [None]*count | |
self.dx = [None]*self.n | |
self.dy = [None]*self.n | |
t = 0.; | |
for i in range(0, count): | |
t1 = 1-t | |
t12 = t1*t1 | |
t2 = t*t | |
self.B0[i] = t1*t12 | |
self.B1[i] = 3*t*t12 | |
self.B2[i] = 3*t2*t1 | |
self.B3[i] = t*t2 | |
t += 1.00/count | |
return self.drawSpline() | |
def findCPoints(self): | |
if self.mode == 0: | |
self.dx[0] = self.Px[self.n] - self.Px[0]; | |
self.dy[0] = self.Py[self.n] - self.Py[0]; | |
self.dx[self.n-1] = -(self.Px[self.n1] - self.Px[self.n-1]) | |
self.dy[self.n-1] = -(self.Py[self.n1] - self.Py[self.n - 1]) | |
else: | |
DIV = 3. | |
self.dx[0] = (self.Px[1] - self.Px[0])/DIV; | |
self.dy[0] = (self.Py[1] - self.Py[0])/DIV; | |
self.dx[self.n-1] = (self.Px[self.n-1] - self.Px[self.n-2])/DIV; | |
self.dy[self.n-1] = (self.Py[self.n-1] - self.Py[self.n-2])/DIV; | |
#self.warp += 0.05 | |
warps = self.Pw | |
Ax = [None]*self.n | |
Ay = [None]*self.n | |
Bi = [None]*self.n | |
Bi[1] = -1/warps[1] | |
Ax[1] = -(self.Px[2] - self.Px[0] - self.dx[0])*Bi[1] | |
Ay[1] = -(self.Py[2] - self.Py[0] - self.dy[0])*Bi[1]; | |
for i in range(2, self.n-1): | |
warpval = warps[i] | |
Bi[i] = -1/(warpval+ Bi[i-1]); | |
Ax[i] = -(self.Px[i+1] - self.Px[i-1] - Ax[i-1])*Bi[i]; | |
Ay[i] = -(self.Py[i+1] - self.Py[i-1] - Ay[i-1])*Bi[i]; | |
for i in range(self.n-2, 0, -1): | |
self.dx[i] = Ax[i] + self.dx[i+1]*Bi[i]; | |
self.dy[i] = Ay[i] + self.dy[i+1]*Bi[i]; | |
#self.dx[1] = 0 | |
#self.dy[0] = 0 | |
#self.dy[1] = 0 | |
# for i in range(0, len(self.dx)): | |
# self.dx[i] = 0 | |
# self.dy[i] = 0 | |
# print self.dx | |
# print self.dy | |
def drawSpline(self): | |
self.findCPoints() | |
w = 1. | |
h = 1. | |
d = 0. | |
h1 = h | |
d2 = d | |
step = 1./w | |
t = step; | |
scPx = [None]*self.n | |
scPy = [None]*self.n | |
scDx = [None]*self.n | |
scDy = [None]*self.n | |
X = None | |
Y = None | |
squaresize = 0.3 | |
for i in range(0, self.n): | |
X = scPx[i] = self.Px[i]*w | |
Y = scPy[i] = self.Py[i]*h; | |
scDx[i] = self.dx[i]*w; | |
scDy[i] = self.dy[i]*h; | |
rect = patches.Rectangle((X - squaresize/2.,Y - squaresize/2.),squaresize,squaresize,linewidth=1,edgecolor='r',facecolor='none') | |
self.ax.add_patch(rect) | |
#if self.mode == 0: | |
self.ax.add_patch(patches.Rectangle((scPx[0]+scDx[0] - squaresize/2., (scPy[0]+scDy[0])- squaresize/2.),squaresize,squaresize, | |
linewidth=1,edgecolor='b',facecolor='none')) | |
self.ax.add_patch(patches.Rectangle((scPx[self.n-1]-scDx[self.n-1] - squaresize/2., (scPy[self.n-1]-scDy[self.n-1])- squaresize/2.),squaresize,squaresize, | |
linewidth=1,edgecolor='b',facecolor='none')) | |
# if self.n > 1: | |
# paths = [[scPx[0], scPy[0]]] | |
# for i in range(1, self.n): | |
# paths.append([scPx[i-1]+scDx[i-1], scPy[i-1]-scDy[i-1]]) | |
# paths.append([scPx[i]-scDx[i], scPy[i]+scDy[i]]) | |
# paths.append([scPx[i], scPy[i]]) | |
# paths_arr = np.array(paths) | |
# plt.plot(paths_arr[:, 0], paths_arr[:, 1]) | |
# #for path in paths: | |
paths = [[scPx[0], scPy[0]]] | |
for i in range(0, self.n-1): | |
for k in range(0, self.count): | |
X = (scPx[i]*self.B0[k] + (scPx[i] + scDx[i])*self.B1[k] + | |
(scPx[i+1] - scDx[i+1])*self.B2[k] + scPx[i+1]*self.B3[k]) | |
Y = (scPy[i]*self.B0[k] + (scPy[i] + scDy[i])*self.B1[k] + | |
(scPy[i+1] - scDy[i+1])*self.B2[k] + scPy[i+1]*self.B3[k]); | |
paths.append([X,Y]) | |
paths_arr = np.array(paths) | |
plt.scatter(paths_arr[:, 0], paths_arr[:, 1], s=0.2) | |
return paths_arr | |
# cv = np.array( | |
# [[ 2.70967742, 1.30411255, 4. ], # D | |
# [ 2.45564516, 6.28246753, 4. ], # C | |
# [-0.90322581, 6.39069264, 4. ], | |
# [-1.04435484, 1.27705628, 4. ], | |
# [-5.75806452, 1.41233766, 4. ], # B | |
# [-5.95564516, 6.14718615, 4. ], # A | |
# [ 2.51209677, 3.79329004, 4. ], # T: Should be center of CD | |
# [-6.06854839, 4.11796537, 4. ]] # T: Should be center of AB | |
# ) | |
cv = np.array( | |
[[ 6.35080645, 5.95779221, 4. ], | |
[ 6.32258065, 1.46645022, 4. ], | |
[ 4.20564516, 1.46645022, 4. ], | |
[ 4.20564516, 6.01190476, 4. ], | |
[ 1.80645161, 5.95779221, 4. ], | |
[ 1.66532258, 1.46645022, 4. ], | |
[-0.56451613, 1.38528139, 4. ], | |
[-0.64919355, 6.06601732, 4. ], | |
[-2.73790323, 6.03896104, 4. ], | |
[-2.96370968, 1.33116883, 4. ], | |
[-5.75806452, 1.41233766, 4. ], | |
[-5.95564516, 6.14718615, 4. ], | |
[ 6.29435484, 3.76623377, 4. ], | |
[-6.06854839, 4.11796537, 4. ]] | |
) | |
class Click(): | |
def __init__(self, ax, button=1): | |
self.ax=ax | |
self.button=button | |
self.press=False | |
self.move = False | |
self.c1=self.ax.figure.canvas.mpl_connect('button_press_event', self.onpress) | |
self.c2=self.ax.figure.canvas.mpl_connect('button_release_event', self.onrelease) | |
self.c3=self.ax.figure.canvas.mpl_connect('motion_notify_event', self.onmove) | |
self.c4=self.ax.figure.canvas.mpl_connect('scroll_event',self.onzoom) | |
def onzoom(self, event): | |
print("zoom") | |
global cv | |
lowest_dist = 1000 | |
lowest_index = -1 | |
for i in range(0, cv.shape[0]): | |
dist = np.sqrt((cv[i,0]-event.xdata)**2 + (cv[i,1]-event.ydata)**2) | |
if dist < lowest_dist: | |
lowest_dist = dist | |
lowest_index = i | |
if lowest_dist < 2: | |
if event.button == "up": | |
cv[lowest_index, 2] += 0.5 | |
elif event.button == "down": | |
if cv[lowest_index, 2] > 1.5: | |
cv[lowest_index, 2] -= 0.5 | |
def onclick(self,event): | |
if event.inaxes == self.ax: | |
global cv | |
lowest_dist = 1000 | |
lowest_index = -1 | |
for i in range(0, cv.shape[0]): | |
dist = np.sqrt((cv[i,0]-event.xdata)**2 + (cv[i,1]-event.ydata)**2) | |
if dist < lowest_dist: | |
lowest_dist = dist | |
lowest_index = i | |
if event.button == self.button: | |
if lowest_dist > 1: | |
cv = np.vstack((np.array([event.xdata, event.ydata, 4.]),cv)) | |
# Get Centre | |
print(cv[0,:]) | |
print(cv[1,:]) | |
center_x = (cv[0,0] + cv[1,0])/2 | |
center_y = (cv[0,1] + cv[1,1])/2 | |
print(center_x, center_y) | |
cv[-2,0] = center_x | |
cv[-2,1] = center_y | |
if event.button == 3: | |
if lowest_dist < 1: | |
cv = np.delete(cv, (lowest_index), axis=0) | |
def onpress(self,event): | |
self.press=True | |
def onmove(self,event): | |
if self.press: | |
self.move=True | |
# Find the closest point to the click | |
global cv | |
lowest_dist = 1000 | |
lowest_index = -1 | |
for i in range(0, cv.shape[0]): | |
dist = np.sqrt((cv[i,0]-event.xdata)**2 + (cv[i,1]-event.ydata)**2) | |
if dist < lowest_dist: | |
lowest_dist = dist | |
lowest_index = i | |
if lowest_dist < 2: | |
cv[lowest_index, 0] = event.xdata | |
cv[lowest_index, 1] = event.ydata | |
print(lowest_index) | |
print(cv) | |
def onrelease(self,event): | |
if self.press and not self.move: | |
self.onclick(event) | |
self.press=False; self.move=False | |
#print("Release") | |
if __name__ == "__main__": | |
import matplotlib.pyplot as plt | |
first_run = True | |
click = None | |
axes = plt.gca() | |
bspline = BSpline() | |
def test(closed): | |
global click | |
global first_run | |
global cv | |
cvT = cv.T | |
ax2 = plt.gca() | |
spline = bspline.process(cvT[0],cvT[1],cvT[2],ax2) | |
# plt.plot(x,y,'k-',label='Curve') | |
plt.minorticks_on() | |
plt.legend() | |
plt.xlabel('x') | |
plt.ylabel('y') | |
plt.xlim(-7, 7) | |
plt.ylim(0, 10) | |
#plt.gca().set_aspect('equal', adjustable='box-forced') | |
plt.pause(0.005) | |
plt.cla() | |
if(first_run): | |
first_run = False | |
click = Click(ax2, button=1) | |
while 1: | |
test(False) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment