def twist_around_axis(thread, angle1, angle2, axis):
xyz = thread.getXYZ()
cons = thread.getConstraints()
mat1 = euler2mat(*cons[3:6])
mat2 = euler2mat(*cons[9:12])
tmat1 = euler2mat(*angle_axis2euler(angle1, axis))
tmat2 = euler2mat(*angle_axis2euler(angle2, axis))
newmat1 = dot(tmat1, mat1)
newmat2 = dot(tmat2, mat2)
neweul1 = mat2euler(newmat1)
neweul2 = mat2euler(newmat2)
cons[3:6] = neweul1
cons[9:12] = neweul2
thread.setConstraints(cons)
def twist_around_axis(thread,angle1,angle2,axis):
xyz = thread.getXYZ()
cons = thread.getConstraints()
mat1 = euler2mat(*cons[3:6])
mat2 = euler2mat(*cons[9:12])
tmat1 = euler2mat(*angle_axis2euler(angle1,axis))
tmat2 = euler2mat(*angle_axis2euler(angle2,axis))
newmat1 = dot(tmat1,mat1)
newmat2 = dot(tmat2,mat2)
neweul1 = mat2euler(newmat1)
neweul2 = mat2euler(newmat2)
cons[3:6] = neweul1
cons[9:12] = neweul2
thread.setConstraints(cons)
def twist_ctrl(thread, angle1, angle2):
xyz = thread.getXYZ()
axis1 = xyz[:, 1] - xyz[:, 0]
axis2 = xyz[:, -1] - xyz[:, -2]
cons = thread.getConstraints()
mat1 = euler2mat(*cons[3:6])
mat2 = euler2mat(*cons[9:12])
tmat1 = euler2mat(*angle_axis2euler(angle1, axis1))
tmat2 = euler2mat(*angle_axis2euler(angle2, axis2))
newmat1 = dot(tmat1, mat1)
newmat2 = dot(tmat2, mat2)
neweul1 = mat2euler(newmat1)
neweul2 = mat2euler(newmat2)
#print "orig",cons[blah]
cons[3:6] = neweul1
cons[9:12] = neweul2
thread.setConstraints(cons)
def twist_ctrl(thread,angle1,angle2):
xyz = thread.getXYZ()
axis1 = xyz[:,1] - xyz[:,0]
axis2 = xyz[:,-1] - xyz[:,-2]
cons = thread.getConstraints()
mat1 = euler2mat(*cons[3:6])
mat2 = euler2mat(*cons[9:12])
tmat1 = euler2mat(*angle_axis2euler(angle1,axis1))
tmat2 = euler2mat(*angle_axis2euler(angle2,axis2))
newmat1 = dot(tmat1,mat1)
newmat2 = dot(tmat2,mat2)
neweul1 = mat2euler(newmat1)
neweul2 = mat2euler(newmat2)
#print "orig",cons[blah]
cons[3:6] = neweul1
cons[9:12] = neweul2
thread.setConstraints(cons)
def euler(self):
"""
Calculate Euler angles for all affine transformations
Notes
-----
This only makes sense, if the affine transformations are rigid.
"""
assert self.are_rigid, 'affines must be rigid'
euler = np.zeros((self.n, 3))
for t in range(self.n):
euler[t] = mat2euler(self.affines[t, :3, :3])
return euler
def aff2euler(affine):
"""
Compute Euler angles from 4 x 4 `affine`
Parameters
----------
affine : 4 by 4 array
An affine transformation matrix
Returns
-------
The Euler angles associated with the affine
"""
return euler.mat2euler(aff2rot_zooms(affine)[0])
def aff2euler(affine):
"""
Compute Euler angles from 4 x 4 `affine`
Parameters
----------
affine : 4 by 4 array
An affine transformation matrix
Returns
-------
The Euler angles associated with the affine
"""
return euler.mat2euler(aff2rot_zooms(affine)[0])
def mat2euler(M):
"""Return Euler angles according to z, y, x
intrinsic rotation according z, y, x
M = Rz(gamma).Ry(beta).Rx(alpha)
Parameters
----------
M : 2D array
Returns
-------
eul : list of Euler angles [gamma, beta, alpha] or [yaw, pitch, roll]
"""
res = ea.mat2euler(M.T)
return [-ang for ang in res]
def transformEuler(eul1,rot_type,rot):
mat1 = euler2mat(*eul1)
if rot_type == "angle_axis":
ang1,ax1 = rot
tmat1 = angle_axis2mat(ax1,ang1)
elif rot_type == "mat":
tmat1 = rot
elif rot_type == "euler":
eul1 = rot
tmat1 = euler2mat(*eul1)
elif rot_type == "towards":
targ_vec1,ang1 = rot
cur_vec1 = euler2mat(*eul1)[:,0]
ax_rot1 = cross(cur_vec1,targ_vec1)
tmat1 = angle_axis2mat(ang1,ax_rot1)
else: raise Exception("rotation type %s not understood"%rot_type)
mat1new = dot(tmat1,mat1)
return mat2euler(mat1new)
def transformEuler(eul1, rot_type, rot):
mat1 = euler2mat(*eul1)
if rot_type == "angle_axis":
ang1, ax1 = rot
tmat1 = angle_axis2mat(ax1, ang1)
elif rot_type == "mat":
tmat1 = rot
elif rot_type == "euler":
eul1 = rot
tmat1 = euler2mat(*eul1)
elif rot_type == "towards":
targ_vec1, ang1 = rot
cur_vec1 = euler2mat(*eul1)[:, 0]
ax_rot1 = cross(cur_vec1, targ_vec1)
tmat1 = angle_axis2mat(ang1, ax_rot1)
else:
raise Exception("rotation type %s not understood" % rot_type)
mat1new = dot(tmat1, mat1)
return mat2euler(mat1new)
def applyRotEul(eul, ang, ax):
return mat2euler(dot(angle_axis2mat(-ang, ax), euler2mat(*eul)))
def applyRotEul(eul,ang,ax):
"compose a rotation of ang around ax with rotation given by euler angles eul"
return mat2euler(dot(angle_axis2mat(ang,ax),euler2mat(*eul)))
def euler(self):
"""
Calculate Euler angles
"""
assert self.is_rigid, 'does only work if affine is rigid'
return mat2euler(self.affine[:3, :3])
def applyRotEul(eul,ang,ax):
return mat2euler(dot(angle_axis2mat(-ang,ax),euler2mat(*eul)))
