def rotate(v, angle=None, rm=None, c1=None, c2=None, loc_r=None, siz2=None, default_val=float('NaN')):
if (angle is not None):
assert (rm is None)
angle = N.array(angle, dtype=N.float).flatten()
rm = AA.rotation_matrix_zyz(angle)
if (rm is None):
rm = N.eye(v.ndim)
siz1 = N.array(v.shape, dtype=N.float)
if (c1 is None):
c1 = ((siz1 - 1) / 2.0)
else:
c1 = c1.flatten()
assert (c1.shape == (3,))
if (siz2 is None):
siz2 = siz1
siz2 = N.array(siz2, dtype=N.float)
if (c2 is None):
c2 = ((siz2 - 1) / 2.0)
else:
c2 = c2.flatten()
assert (c2.shape == (3,))
if (loc_r is not None):
loc_r = N.array(loc_r, dtype=N.float).flatten()
assert (loc_r.shape == (3,))
c2 += loc_r
c = ((- rm.dot(c2)) + c1)
vr = SNI.affine_transform(input=v, matrix=rm, offset=c, output_shape=siz2.astype(N.int), cval=default_val)
return vr
def distance_6d_sq__frobenius(p1, p2, weight=1.0):
loc1 = np.array(p1[:3])
assert len(loc1) == 3
ang1 = p1[3:]
assert len(ang1) == 3
rm1 = TGA.rotation_matrix_zyz(ang1)
loc2 = np.array(p2[:3])
assert len(loc2) == 3
ang2 = p2[3:]
assert len(ang2) == 3
rm2 = TGA.rotation_matrix_zyz(ang2)
d_rm = np.square(np.eye(3) - rm1.transpose().dot(rm2)).sum()
d_loc = np.square(loc1 - loc2).sum()
return d_rm + weight * d_loc