def readpoly(fnpoly='polygon.txt'):
f = open(fnpoly, 'r')
txt = f.read()
f.close()
lines = txt.split("\n")
n = int(lines[0])
pts = []
k = 1
for i in range(n):
pt = lines[k]
pt = pt.split(' ')
pt = map(int, pt)
pts.append(pt)
k += 1
PTS = np.array(pts)
xmax, ymax = np.max(PTS, axis=0)
pts = []
for i in range(n):
x, y = PTS[i]
w = h = 200
x = mapto.MapTo(0, 0, xmax, w, x)
y = mapto.MapTo(0, h, ymax, 0, y)
pt = [x, y]
pts.append(pt)
m = int(lines[k])
k += 1
idxs = []
for i in range(m):
idx = int(lines[k])
idxs.append(idx)
k += 1
shape = [pts, idxs]
return shape
def Cone0(x1,y1,x2,y2,t,q,s,m=10,n=10):
curve = [[0,0]]
for i in range(m):
ti = 1.0*i/(m-1)
xi = mapto.MapTo(0,x1,1,x2, ti)
yi = mapto.MapTo(0,y1,1,y2, ti)
pt = [xi,yi]
curve.append(pt)
curve.append([0,yi])
H = rc.RevolveCurve(curve,t,q,s, n=n, bcap=True,ecap=True)
return H
def D(gr, f):
for j in range(h):
for i in range(w):
x = mapto.MapTo(0, 0, w - 1, 1, i)
y = mapto.MapTo(0, 0, h - 1, 1, j)
l = f([x, y])
if l == '0':
color = [255, 0, 0]
else:
color = [0, 0, 255]
gr.canvas[j, i, :] = color
return
def F2im(F, w, h):
# Implicit function F(x,y) to image
im = np.zeros((h, w))
xlo = 0
xhi = w
ylo = 0
yhi = h
for j in range(h):
y = mapto.MapTo(ylo, 0, yhi, h - 1, j)
for i in range(w):
x = mapto.MapTo(xlo, 0, xhi, w - 1, i)
im[j, i] = F(x, y)
return im
def ElevationMap0(im, w, h, d, m=10):
n = m
H1 = gra.Gnm(m, n)
pts = []
F = []
N = []
sh = im.shape
hh = sh[1]
ww = sh[0]
sz = max(hh, ww)
sh = (sz, sz)
im = cv2.resize(im, sh)
hh, ww = im.shape
I = list(range(0, ww, int(ww / m)))
J = list(range(0, hh, int(hh / n)))
for j in J:
y = mapto.MapTo(0, -h / 2., n - 1, h / 2., j)
for i in I:
x = mapto.MapTo(0, -w / 2., m - 1, w / 2., i)
z = im[j, i]
pt = [x, y, z * d]
pts.append(pt)
H1['pts'] = pts
idx = lambda u, v: v + n * u
for tup in H1['object']:
u, v = tup
u1 = u
u2 = clamp(u1 + 1, 0, m - 1)
v1 = v
v2 = clamp(v1 + 1, 0, n - 1)
a1 = idx(u1, v1)
a2 = idx(u2, v1)
a3 = idx(u2, v2)
a4 = idx(u1, v2)
f = [a1, a2, a3, a4] # rectangle
f1 = [a1, a2, a3] # triangle
f2 = [a1, a3, a4] # triangle
#print f
F.append(f1)
F.append(f2)
A, B, C = [H1['pts'][v] for v in f1]
N1 = FaceNormalABC(A, B, C)
N.append(N1)
A, B, C = [H1['pts'][v] for v in f2]
N2 = FaceNormalABC(A, B, C)
N.append(N2)
H1['N'] = N
H1['F'] = F
return H1
def CreateGridGeometry(doc, w, h):
"""
Create a grid geometry of pts size n x n
It assumes a doc["object"] attribute containing
(u,v) the u,v coordinate of a vertex.
"""
doc2 = deepcopy(doc)
doc2["pts"] = []
umax, vmax = max(doc["object"])
for obj in doc2["object"]:
u, v = obj
x = w / 2.0 + mapto.MapTo(umax / 2.0, w / 2.0, umax, w, u)
y = h / 2.0 + mapto.MapTo(vmax / 2.0, h / 2.0, vmax, h, v)
z = 0
pt = [x, y, z]
pt = map(int, map(round, pt))
doc2["pts"].append(pt)
return doc2
def interpolation(x, X, Y):
if x <= X[0]:
return Y[0]
elif x >= X[-1]:
return Y[-1]
else:
i = 0
while X[i] < x:
i += 1
return mapto.MapTo(X[i - 1], Y[i - 1], X[i], Y[i], x)
def D(gr, f):
for j in range(h):
for i in range(w):
x = mapto.MapTo(0, 0, w - 1, 1, i)
y = mapto.MapTo(0, 0, h - 1, 1, j)
u2 = fmod(2 * x, 1)
v2 = fmod(2 * y, 1)
u1 = fmod(4 * x, 1)
v1 = fmod(4 * y, 1)
l = f([u2, v2, u1, v1])
if l == '0':
color = [255, 0, 0]
elif l == '1':
color = [0, 255, 0]
elif l == '2':
color = [0, 0, 255]
else:
color = [0, 0, 0]
gr.canvas[j, i, :] = color
return
def CreateCircleGeometry(doc, cx, cy, cz, r):
doc2 = deepcopy(doc)
doc2["pts"] = []
n = len(doc2["V"])
for i in range(n):
theta = mapto.MapTo(0, 0, n, 2 * pi, i)
x = cx + r * cos(theta)
y = cy + r * sin(theta)
z = 0
pt = [x, y, z]
doc2["pts"].append(pt)
return doc2
def SaveKern(fn, im):
print "Saving file...", fn
h, w = im.shape
im *= w * h
min0 = np.min(im)
max0 = np.max(im)
im2 = np.ones((h, w), dtype='uint8') * 255
for j in range(h):
for i in range(w):
val = im[j, i]
im2[j, i] = int(mapto.MapTo(min0, 0, max0, 255, val))
cv2.imwrite(fn, im2)
return im2
def Ellipsoid(rx,ry,t,q,s,m=10,n=10):
curve = [[0,0]]
a,b = [0,0]
for i in range(m):
val = 1.0*i/(m-1)
ti = mapto.MapTo(0,-pi/2., 1,pi/2., val) # pi = 180 degrees only half circle
xi = a + rx*cos(ti)
yi = b + ry*sin(ti)
pt = [xi,yi]
curve.append(pt)
H = rc.RevolveCurve(curve,t,q,s, n=n, bcap=True,ecap=True)
return H
def TableLeg(height,radius1,radius2,n=10,m=10):
h = height
r1 = radius1
r2 = radius2
curve = []
x0 = 0
for i in range(1,m-1):
r = mapto.MapTo(0,r1,m-1,r2,i)
xi = r
yi = mapto.MapTo(0,0,m-1,h,i)
pt = [xi,yi]
curve.append(pt)
yi = mapto.MapTo(0,0,m-1,h,.1)
curve = [[0,yi]]+curve+[[0,height]]
# Assume curve has symmetry y-axis
x0 = curve[0][0]
x_min = min(map(lambda pt: pt[0]-x0,curve))
x_max = max(map(lambda pt: pt[0]-x0,curve))
y_min = min(map(lambda pt: pt[1],curve))
y_max = max(map(lambda pt: pt[1],curve))
poly_wb = deepcopy(curve)
pts = map(lambda pt: [0,pt[1],0], poly_wb)
y_min = min(map(lambda pt: pt[1],poly_wb))
y_max = max(map(lambda pt: pt[1],poly_wb))
doc = g.Cn(n)
cx,cy,cz = [0,0,0]
r0 = 1.
doc1 = rc.CreateCircleGeometry(doc,cx,cy,cz,r0)
spath = []
path = []
pti = poly_wb[0]
xi,yi = pti
yi_last = yi
dx = x_max-x_min
dy = y_max-y_min
epsilon = 1e-4
if abs(dx) > epsilon:
aspect = 1.*dy/dx
else:
aspect = 1.*dx/dy
for i in range(len(poly_wb)):
pti = poly_wb[i]
xi,yi = pti
r = abs(xi - x0)
dy = yi-yi_last
epsilon = .1
if abs(dy) > epsilon:
spath.append(r)
path.append([yi,0,0])
yi_last = yi
degrees = 90+180
axis = [0,1,0]
q0 = aff.HH.rotation_quaternion(degrees,axis[0],axis[1],axis[2])
q = q0
t = [0,0,-height*.4]
s = [1,1,1]
C = aff.Center(path)
pts = aff.Translate(path,-C[0],-C[1],-C[2],align=False)
pts = aff.Rotate(pts,q,align=False)
pts = aff.Scale(pts, s[0],s[1],s[2],align=False)
pts = aff.Translate(pts,t[0],t[1],t[2],align=False)
path = pts
#path = [[0,0,0],[10,1,0],[20,4,0],[30,9,0]]
H = ext.Extrusion0(doc1,path,spath)
return H
def MassSpectrometer(I, show=True):
# W,H used in calculation of measurement
W = 600
H = 600
if show:
gr = racg.Graphics(w=W, h=H)
E0 = 0
B0 = 2 # light amplitude
lam = 5 # light wavelength
black = [0, 0, 0]
blue = [0, 0, 255]
pt_last = [0, 300]
direction = 1 # direction y-axis or -(y-axis)
#print "Press 'e' to exit"
tmax = 55 # time units maximum
Z = 0
while I.t <= tmax:
# make direction change periodically
direction = 1
#gr.Clear()
r = I.x
# Screen transformation of r -> [x,y,z]
xmin = -10
xmax = 10
ymin = -10
ymax = 10
x = mapto.MapTo(xmin, 0, xmax, W, r[0])
y = mapto.MapTo(ymin, 0, ymax, H, r[1])
z = 0
# Projective Transformation of [x,y,z] -> [x2,y2]
pt = [x - W / 2, y]
#gr.Point(pt,color=black)
#print pt,r
if show:
gr.Line(pt, pt_last, color=blue)
sW = 0.5
P = [W * sW, 0]
Q = [W * sW, H]
if show:
gr.Line(P, Q, color=black)
flag, X, Y = li.SegmentIntersect(
pt[0],
pt[1],
pt_last[0],
pt_last[1], # segment 1
P[0],
P[1],
Q[0],
Q[1])
s = "Y = %.2f" % (Y)
if show:
gr.Text(s, X + 10, Y, black)
pt_last = pt
if show:
if fmod(I.t, 0.1) < 0.001:
ch = gr.Show("result", 5)
if ch == ord('e'):
break
E, B = EMfield(I.t, lam, E0, B0)
F = LorenzForce(I.q, I.v, E, B)
I.Update(F)
if flag:
Z = Y
if show:
m0 = finv(Z) * I.q
s = "mass m = %.3f" % (m0)
gr.Text(s, X + 10, Y - 25, black)
if show:
gr.Show("result", -1)
break
if show:
gr.Close()
return Z
"\"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n" +\
"<svg width=\"12cm\" height=\"12cm\" viewBox=\"0 0 1200 1200\"\n" +\
"xmlns=\"http://www.w3.org/2000/svg\" version=\"1.1\">\n" +\
"<desc>Polyline</desc>"
PolylineFill = lambda s,pts,color="black" : "<polyline fill=\""+s+\
"\" stroke=\""+color+"\" stroke-width=\"1\"\npoints=\""+\
reduce(lambda t,pt: t+WritePoint(pt[0],pt[1]),pts,"") + "\"/>"
Polyline = lambda pts, color="black": PolylineFill("none", pts, color)
Ender = "</svg>"
xa = 0
xb = 1200
ya = 0
yb = 1200
Screen_x = lambda x: int(mapto.MapTo(xa, 0, xb, scr_x_max, x))
Screen_y = lambda x: int(mapto.MapTo(ya, 0, yb, scr_y_max, x))
Coord_x = lambda x: int(mapto.MapTo(0, xa, scr_x_max, xb, x))
Coord_y = lambda x: int(mapto.MapTo(0, ya, scr_y_max, yb, x))
Drawline = lambda A, B, color="black": Polyline(
[map(Screen_x, A), map(Screen_y, B)], color)
def EllipseFill(s, pt, ra, rb, color="black"):
x, y = pt
t = range(101)
xx = map(lambda a: ra * cos(2 * pi * a / 100) + x, t)
yy = map(lambda a: rb * sin(2 * pi * a / 100) + y, t)
pts = map(list, zip(xx, yy))
return PolylineFill(s, pts, color)
q = JA.Solve_q(Xdd)
X = np.array(pt_mouse)
X_last = np.array(P.Start())
us = 1 # microsecond
# [1] Wikipedia "Pulse Width Modulation"
# [2] Wikipedia "Servo Control"
# Angle of Servo Motor Joint depends on how long
# a pulse is sent every 50Hz or 20ms. 0 degrees 1500 us
# -90 degrees is 1000 us and 90 degrees is 2000 us.
# YouTube videos have control set by DutyCycle say
# for example between -90 as 2% and +90 as 12% duty cycles
# on Raspberry Pi using an example servo, and via degrees
# with Arduino
DC_lo = 2 # 2 percent duty cycle
DC_hi = 12 # 12 percent duty cycle
T = lambda x: mapto.MapTo(-90,DC_lo,90,DC_hi,clamp(-90,90,x))
while True:
gr.Clear()
G = P.Graph()
PlotGraph(gr,G,black)
gr.Line(X_last,X,red)
s = "ServoPWM=[%.2fDC,%.2fDC,%.2fDC]" % tuple(map(T,list(JA.q)))
gr.Text(s,50,50,blue,scale=.5)
ch = gr.Show("result",15)
if ch == ord('e'):
break
q = JA.Solve_q(Xdd)
# Set Robot Arm to Joint Parameters q
P.Theta = list(q)
X = np.array(pt_mouse)
a = idxs[i]
b = idxs[(i + 1) % len(idxs)]
if b != 0:
pts0 = pts[a:b]
else:
pts0 = pts[a:]
n0 = len(pts0)
v = 3
for j in range(int(n0 / v) + 1):
if j == n0 / v:
z = z_up
else:
z = z_down
x, y = pts0[(v * j) % n0]
a = 1
x = w / 4 + mapto.MapTo(0, 0, 200, 300, x)
y = h / 3 + mapto.MapTo(0, 300, 200, 0, y)
# map canvas [0,200]x[0,200] to gr canvas
# [0,200]x[300,500]
C = [x, y, z]
G = P.Graph()
A = P.Start()
if A[2] <= z_thresh and A_last[2] <= z_thresh:
gr.Line(A_last[:2], A[:2], blue)
im_pre = deepcopy(gr.canvas)
PlotGraph(gr, G, black)
A_last = deepcopy(A)
theta1 = fmod(P.Theta[0], 360)
theta2 = fmod(P.Theta[1], 360)
theta3 = fmod(P.Theta[2], 360)
d = P.Theta[3]
