eic-opticks/analytic/catmull_rom_spline.py

0001 #!/usr/bin/env python
0002 """
0003 catmull_rom_spline.py  : SEE catmull_rom_spline_2.py FOR A BETTER IMPLEMENTATION
0004 ======================================================================================
0005
0006 ::
0007
0008    epsilon:opticks blyth$ opticks-rl sympy
0009
0010
0011 * https://www.cs.cmu.edu/~fp/courses/graphics/asst5/catmullRom.pdf
0012
0013 * https://qroph.github.io/2018/07/30/smooth-paths-using-catmull-rom-splines.html
0014
0015 * https://www.cs.cmu.edu/~fp/courses/graphics/asst5/catmullRom.pdf
0016
0017   ~/opticks_refs/derivation_catmullRom_spline.pdf
0018
0019
0020 * https://dev.to/ndesmic/splines-from-scratch-catmull-rom-3m66
0021
0022
0023 """
0024
0025 import numpy as np, sympy as sp, matplotlib as mp
0026 from sympy.utilities.lambdify import lambdify
0027 SIZE = np.array([1280, 720])
0028
0029 def pp(q, q_label="?", note=""):
0030     if type(q) is np.ndarray:
0031         q_type = "np"
0032     elif q.__class__.__name__  == 'MutableDenseMatrix':
0033         q_type = "sp.MutableDenseMatrix"
0034     else:
0035         q_type = "?"
0036     pass
0037     print("\n%s : %s : %s : %s \n" % (q_label, q_type, str(q.shape), note) )
0038
0039     if q_type.startswith("sp"):
0040         sp.pprint(q)
0041     elif q_type.startswith("np"):
0042         print(q)
0043     else:
0044         print(q)
0045     pass
0046
0047
0048
0049 if __name__ == '__main__':
0050
0051     """
0052
0053       0,1        1,1
0054         +--------+
0055         |        |
0056         |        |
0057         |        |
0058         +--------+
0059       0,0       1,0
0060
0061     """
0062
0063
0064     points = np.zeros( (16, 3) )
0065     for j in range(len(points)):
0066         angle = 2*np.pi*float(j)/(len(points))
0067         # when looped avoiding the repeated point from 0,2pi degeneracy
0068         # avoids the joint being visible
0069         points[j] = [np.cos(angle), np.sin(angle), 0]
0070     pass
0071
0072     #points = np.array([ [0,0,0], [1,0,0], [0,1,0], [1,1,0] ])
0073
0074     A = 0.5   # eg 0:straight lines, 0.5 default,smoothest? 1:wiggly  "tension"
0075
0076     assert len(points) >= 4
0077     numseg = len(points)-3    # sliding window of 4 points for each segment
0078
0079     looped = True
0080     if looped: numseg += 3
0081
0082     segnum = 100              # number of points to interpolate each segment into
0083     tt = np.linspace(0,1, segnum)
0084
0085     xpp = np.zeros( (numseg*segnum,3) )
0086
0087     u, a  = sp.symbols("u a")
0088     M = sp.Matrix([
0089            [  0   , 1    , 0      ,  0 ],
0090            [  -a  , 0    , a      ,  0 ],
0091            [  2*a , a - 3, 3 - 2*a, -a ],
0092            [  -a  , 2 - a, a - 2  ,  a ]
0093            ])
0094
0095     pp(M,"M")
0096
0097     U = sp.Matrix( [1, u, u*u, u*u*u ] )     # column vector
0098     V = sp.Matrix( [[1, u, u*u, u*u*u ]] )   # row vector
0099     pp(U,"U")
0100     pp(V,"V")
0101     pp(U.T,"U.T")
0102     pp(V.T,"V.T")
0103     VM = V*M
0104     pp(VM,"VM")
0105     pp(VM.T,"VM.T")
0106     VMa = VM.subs([(a,A)])
0107
0108     numpoi = len(points)
0109
0110     for iseg in range(numseg):
0111
0112         i0 = (iseg+0) % numpoi
0113         i1 = (iseg+3) % numpoi
0114         #p = points[i0:i1+1]   # python one beyond index
0115         p = np.take( points, np.arange(iseg,iseg+4), mode='wrap', axis=0 )
0116
0117         ## hmm now to loop the array indices in numpy ?
0118         ## https://stackoverflow.com/questions/28398220/circular-numpy-array-indices
0119
0120         print(" iseg %2d i0 %3d i1 %3d numpoi %3d p %s " % (iseg, i0,i1,numpoi, str(p.shape) ) )
0121
0122         # ordinarily, when not looped the modulus will not kick in
0123         #
0124         #     numseg : numpoi-3     (sliding window of 4 points for each segment)
0125         #   max iseg : numseg-1  = numpoi-4
0126         #
0127
0128         VMa_p = VMa*p
0129         #pp(VMa_p, "VMa_p")
0130         fn = lambdify(u, VMa_p,'numpy')
0131         for i in range(len(tt)):
0132            xpp[iseg*segnum+i] = fn(tt[i])
0133            # how to directly pull an array of arrays here ? avoiding the python loop
0134         pass
0135
0136
0137     fig, ax = mp.pyplot.subplots(figsize=SIZE/100.)
0138     fig.suptitle("analytic/catmull_rom_spline.py")
0139     ax.set_aspect('equal')
0140     ax.plot( xpp[:,0], xpp[:,1], label="xpp"  )
0141     ax.scatter( points[:,0], points[:,1], label="points" )
0142     ax.legend()
0143     fig.show()
0144