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))
