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)