cons_end = cons_start + scale*np.r_[randn(3)*10,randn(3)*.5,randn(3)*10,randn(3)*.5]
tip1_start,tip2_start = cons_start[0:3],cons_start[6:9]
tip1_end,tip2_end = cons_end[0:3],cons_end[6:9]
tips = np.c_[tip1_start,tip2_start,tip1_end,tip2_end]
colors = [(1,0,0),(0,1,0),(0,0,1)]
mlab.clf()
x,y,z = thread_start.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.4,color=(1,1,1))
#x,y,z = tips
#mlab.points3d(x,y,z,color=(1,1,1))
for (i,thread,color) in zip(xrange(n_clones),threads,colors):
print "thread",i
cons_mid = cons_start + scale*np.r_[randn(3)*10,randn(3)*.5,randn(3)*10,randn(3)*.5]
cons_t = upsample2D(np.array([cons_start,cons_mid,cons_end]),T)
thread = threads[i]
for (t,cons) in enumerate(cons_t):
print t
thread.setConstraints(cons)
x,y,z = threads[i].getXYZ()
if t==T-1: tube_radius = .4
else: tube_radius = .1
mlab.plot3d(x,y,z,color=color,tube_radius=tube_radius)
randn(3) * 10, randn(3) * .5]
tip1_start, tip2_start = cons_start[0:3], cons_start[6:9]
tip1_end, tip2_end = cons_end[0:3], cons_end[6:9]
tips = np.c_[tip1_start, tip2_start, tip1_end, tip2_end]
colors = [(1, 0, 0), (0, 1, 0), (0, 0, 1)]
mlab.clf()
x, y, z = thread_start.getXYZ()
mlab.plot3d(x, y, z, tube_radius=.4, color=(1, 1, 1))
#x,y,z = tips
#mlab.points3d(x,y,z,color=(1,1,1))
for (i, thread, color) in zip(xrange(n_clones), threads, colors):
print "thread", i
cons_mid = cons_start + scale * np.r_[randn(3) * 10,
randn(3) * .5,
randn(3) * 10,
randn(3) * .5]
cons_t = upsample2D(np.array([cons_start, cons_mid, cons_end]), T)
thread = threads[i]
for (t, cons) in enumerate(cons_t):
print t
thread.setConstraints(cons)
x, y, z = threads[i].getXYZ()
if t == T - 1: tube_radius = .4
else: tube_radius = .1
mlab.plot3d(x, y, z, color=color, tube_radius=tube_radius)
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)
x,y,z = c_[p1,p1+3*v1]
mlab.plot3d(x,y,z,tube_radius=.3,color=col)
x,y,z = c_[p2,p2+3*v2]
mlab.plot3d(x,y,z,tube_radius=.3,color=col)
scalar =5
T = 30
n_waypoints = 3
#np.random.seed(23)
np.random.seed(24)
#np.random.seed(28)
thread = Thread()
thread_targ = thread.clone()
cons = thread.getConstraints()
waypoints = cons[None,:] + np.cumsum(np.r_[np.zeros((1,12)),scalar*USCALE*randn(n_waypoints,12)],0)
for cons in upsample2D(waypoints,n_waypoints*T):
thread_targ.setConstraints(cons)
x,y,z = thread_targ.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.05,color=(1,1,1))
mlab.clf()
x,y,z = thread_targ.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.2,color=(1,1,1))
colors = colorSeq(100)
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.2,color=colors.next())
scalar = 5
T = 30
n_waypoints = 3
#np.random.seed(23)
np.random.seed(24)
#np.random.seed(28)
thread = Thread()
thread_targ = thread.clone()
cons = thread.getConstraints()
waypoints = cons[None, :] + np.cumsum(
np.r_[np.zeros((1, 12)), scalar * USCALE * randn(n_waypoints, 12)], 0)
for cons in upsample2D(waypoints, n_waypoints * T):
thread_targ.setConstraints(cons)
x, y, z = thread_targ.getXYZ()
mlab.plot3d(x, y, z, tube_radius=.05, color=(1, 1, 1))
mlab.clf()
x, y, z = thread_targ.getXYZ()
mlab.plot3d(x, y, z, tube_radius=.2, color=(1, 1, 1))
colors = colorSeq(100)
x, y, z = thread.getXYZ()
mlab.plot3d(x, y, z, tube_radius=.2, color=colors.next())
for cons in C.match_cons(thread, thread_targ.getConstraints()):
thread.setConstraints(cons)
