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 minRot(ax1, ax2): "find the rotation matrix that takes ax1 to ax2" if almostEq(ax1, ax2): L.debug("minRot: same vector") return eye(3) elif almostEq(ax1, -ax2): L.debug("minRot: opp vector") return -diag([-1, -1, 0]) else: ax_rot = cross(ax2, ax1) return angle_axis2mat(angBetween(ax1, ax2), ax_rot)
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))