def makeArch(thread, center, mainax, up, elev):
p_start = center - mainax
p_end = center + mainax
v_start = normr(mainax + normr(up) * norm(mainax) * tan(elev))
v_end = normr(mainax - normr(up) * norm(mainax) * tan(elev))
print "p_start,p_end", p_start, p_end
while True:
xyz = thread.getXYZ()
s0 = xyz.T[0]
e0 = xyz.T[-1]
while True:
xyz = thread.getXYZ()
s0, s1, e1, e0 = xyz.T[[0, 1, -2, -1]]
print "s0,e0", s0, e0
print "ang_start", angBetween(v_start, s1 - s0)
print "ang_end", angBetween(v_end, e0 - e1)
if almostEq(s0,p_start) and almostEq(e0,p_end) and\
almostEq(v_start,s1-s0) and almostEq(v_end,e0-e1):
break
else:
t_start = trunc(p_end - s0, .5)
t_end = trunc(p_start - e0, .5)
#print t_start,t_end
ang1 = trunc(angBetween(v_start, s1 - s0), .05)
ang2 = trunc(angBetween(v_end, e0 - e1), .05)
cons = moveEndCons(thread, t_start, t_end, "towards",
(v_start, ang1), (v_end, ang2))
#cons = thread.getConstraints()
#cons = r_[cons[0:3]+t_start,cons[3:6],cons[6:9]+t_end,cons[9:12]]
yield cons
def makeArch(thread, center, mainax, up, elev):
p_start = center - mainax
p_end = center + mainax
v_start = normr(mainax + normr(up)*norm(mainax)*tan(elev))
v_end = normr(mainax - normr(up)*norm(mainax)*tan(elev))
print "p_start,p_end",p_start,p_end
while True:
xyz = thread.getXYZ()
s0 = xyz.T[0]
e0 = xyz.T[-1]
while True:
xyz = thread.getXYZ()
s0,s1,e1,e0 = xyz.T[[0,1,-2,-1]]
print "s0,e0",s0,e0
print "ang_start",angBetween(v_start,s1-s0)
print "ang_end",angBetween(v_end,e0-e1)
if almostEq(s0,p_start) and almostEq(e0,p_end) and\
almostEq(v_start,s1-s0) and almostEq(v_end,e0-e1): break
else:
t_start = trunc(p_end - s0,.5)
t_end = trunc(p_start - e0,.5)
#print t_start,t_end
ang1 = trunc(angBetween(v_start,s1-s0),.05)
ang2 = trunc(angBetween(v_end,e0-e1),.05)
cons = moveEndCons(thread,t_start,t_end,"towards",(v_start,ang1),(v_end,ang2))
#cons = thread.getConstraints()
#cons = r_[cons[0:3]+t_start,cons[3:6],cons[6:9]+t_end,cons[9:12]]
yield cons
def plotOrs(*ptss):
mlab.figure(34); mlab.clf()
r = 1
phi, theta = mgrid[0:pi:101j, 0:2*pi:101j]
x = r*sin(phi)*cos(theta)
y = r*sin(phi)*sin(theta)
z = r*cos(phi)
mlab.mesh(x,y,z, colormap='gray',opacity=.2)
colors = [(1,0,0),(0,1,0),(0,0,1)]
print len(colors)
print len(ptss)
for (pts,col) in izip(ptss,colors):
print col
ors = normr(pts)
# Create a sphere
x,y,z = ors.T
mlab.points3d(x,y,z,color=col)
mlab.plot3d(x,y,z,color=col,tube_radius=.025)
def plotOrs(*ptss):
mlab.figure(34)
mlab.clf()
r = 1
phi, theta = mgrid[0:pi:101j, 0:2 * pi:101j]
x = r * sin(phi) * cos(theta)
y = r * sin(phi) * sin(theta)
z = r * cos(phi)
mlab.mesh(x, y, z, colormap='gray', opacity=.2)
colors = [(1, 0, 0), (0, 1, 0), (0, 0, 1)]
print len(colors)
print len(ptss)
for (pts, col) in izip(ptss, colors):
print col
ors = normr(pts)
# Create a sphere
x, y, z = ors.T
mlab.points3d(x, y, z, color=col)
mlab.plot3d(x, y, z, color=col, tube_radius=.025)
def infTwist(xyz):
pts = xyz.T
ors = normr(pts[1:] - pts[:-1])
start2end_rope = eye(3)
for (or1, or2) in zip(ors[:-1], ors[1:]):
start2end_rope = dot(minRot(or2, or1), start2end_rope)
assert almostEq(dot(start2end_rope, ors[0]), ors[-1])
end2start_min = minRot(ors[0], ors[-1])
twist_mat = dot(end2start_min, start2end_rope)
ang, _ = quat2angle_axis(mat2quat(twist_mat))
return ang
def infTwist(xyz):
pts = xyz.T
ors = normr(pts[1:] - pts[:-1])
start2end_rope = eye(3)
for (or1,or2) in zip(ors[:-1],ors[1:]):
start2end_rope = dot(minRot(or2,or1),start2end_rope)
assert almostEq(dot(start2end_rope,ors[0]),ors[-1])
end2start_min = minRot(ors[0],ors[-1])
twist_mat = dot(end2start_min,start2end_rope)
ang,_ = quat2angle_axis(mat2quat(twist_mat))
return ang
def test_minRot():
for _ in xrange(100):
x1 = normr(randn(3))
x2 = normr(randn(3))
assert almostEq(x1, dot(minRot(x1, x2), x2))
def getState(thread):
"get position and orientation of ends"
xyz = thread.getXYZ()
s0, s1, e1, e0 = xyz.T[[0, 1, -2, -1]]
return s0, normr(s1 - s0), e0, normr(e0 - e1)
def getState(thread):
"get position and orientation of ends"
xyz = thread.getXYZ()
s0,s1,e1,e0 = xyz.T[[0,1,-2,-1]]
return s0,normr(s1-s0),e0,normr(e0-e1)
def test_minRot():
for _ in xrange(100):
x1 = normr(randn(3))
x2 = normr(randn(3))
assert almostEq(x1,dot(minRot(x1,x2),x2))
colors = colorSeq(100)
T = 30
center = r_[0,0,0]
mainax = r_[10,0,0]
up = r_[0,0,1]
elev = pi/3
thread = Thread()
x,y,z = xyz = thread.getXYZ()
p1_arch = center - mainax
p2_arch = center + mainax
v1_arch = normr(mainax + normr(up)*norm(mainax)*tan(elev))
v2_arch = normr(mainax - normr(up)*norm(mainax)*tan(elev))
p1_now,v1_now,p2_now,v2_now = getState(thread)
p1s = upsample2D(array([p1_now,p1_arch]),T,1)
p2s = upsample2D(array([p2_now,p2_arch]),T,1)
v1s = upsample2D(array([v1_now,v1_arch]),T,1)
v2s = upsample2D(array([v2_now,v2_arch]),T,1)
for (p1,p2,v1,v2) in zip(p1s,p2s,v1s,v2s):
col = colors.next()
setOrs(thread,p1,v1,p2,v2)
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.15,color=col)
