def test_rotation_signed_area(random_quat):
"""Ensure that rotating does not change the signed area."""
assume(not np.all(random_quat == 0))
random_quat = rowan.normalize(random_quat)
rotated_points = rowan.rotate(random_quat, get_square_points())
poly = Polygon(rotated_points)
assert np.isclose(poly.signed_area, 1)
def test_angle(self):
"""Test computation of angles."""
self.assertTrue(geometry.angle(one) == 0)
p = normalize(self.saved_data["p"])
my_ans = geometry.angle(p)
self.assertTrue(np.allclose(self.saved_data["angle"], my_ans))
def render(self, rotation=(1, 0, 0, 0), translation=(0, 0, 0), **kwargs):
(positions, orientations, colors) = mesh.unfoldProperties(
[self.positions, self.orientations, self.colors])
positions = rowan.rotate(rotation, positions)
positions += translation
orientations = rowan.multiply(rotation, rowan.normalize(orientations))
rotations = np.degrees(rowan.to_euler(orientations))
lines = []
for (pos, rot, col, alpha) in zip(positions, rotations, colors[:, :3],
1 - colors[:, 3]):
lines.append(
'sphere {{ '
'0, 1 scale<{a}, {b}, {c}> '
'rotate <{rot[2]}, {rot[1]}, {rot[0]}> '
'translate <{pos[0]}, {pos[1]}, {pos[2]}> '
'pigment {{ color <{col[0]}, {col[1]}, {col[2]}> transmit {alpha} }} '
'}}'.format(a=self.a,
b=self.b,
c=self.c,
pos=pos,
rot=rot,
col=col,
alpha=alpha))
return lines
def RungeKuttaOrient(OmegaInput, QuatInput):
#Make sure Omega is in body frame before putting it in here
#Input matrix to quaternion
QuatInput = rot.from_matrix(QuatInput)
QuatInput = QuatInput.as_quat()
#Does everything in body frame
k1 = np.array(TimeStep * ObtainQuatDot(OmegaInput, QuatInput))
k2 = np.array(TimeStep * ObtainQuatDot(OmegaInput, QuatInput + 0.5 * k1))
k3 = np.array(TimeStep * ObtainQuatDot(OmegaInput, QuatInput + 0.5 * k2))
k4 = np.array(TimeStep * ObtainQuatDot(OmegaInput, QuatInput + k3))
# Update next value of Omega
QuatOutput = QuatInput + (1.0 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4)
#Syntactical acrobatics to return the correct rotation matrix
#It would otheriwse have rotations around z and x axis confused
QuatOutput = rot.from_quat(QuatOutput)
#print("Quat Output (as quaternion);\n", QuatOutput.as_quat())
#These conversions cause errors, but we need to flip the matrices around so things rotate around the axes properly --> need new funciton
#QuatOutput = QuatOutput.as_euler("zyx")
#QuatOutput = rot.from_euler("xyz", QuatOutput)
QuatOutput = QuatOutput.as_matrix()
#print("Quat Output (as matrix):\n", QuatOutput)
#print("Norm: ", quat.norm(QuatOutput))
QuatOutput = quat.normalize(QuatOutput)
return QuatOutput #Gets rid of extra parentheses; in matrix form
def test_from_to_euler(self):
"""2-way conversion starting from Euler angles."""
np.random.seed(0)
quats = rowan.normalize(np.random.rand(25, 4))
conventions = [
"xzx",
"xyx",
"yxy",
"yzy",
"zyz",
"zxz",
"xzy",
"xyz",
"yxz",
"yzx",
"zyx",
"zxy",
]
axis_types = ["extrinsic", "intrinsic"]
for convention in conventions:
for axis_type in axis_types:
euler = rowan.to_euler(quats, convention, axis_type)
out = rowan.from_euler(euler[..., 0], euler[..., 1],
euler[..., 2], convention, axis_type)
self.assertTrue(
np.all(
np.logical_or(np.isclose(out - quats, 0),
np.isclose(out + quats, 0))),
msg="Failed for convention {}, axis type {}".format(
convention, axis_type),
)
def test_normalize(self):
"""Test quaternion normalize"""
np.random.seed(0)
shapes = [(4, ), (5, 4), (5, 5, 4), (5, 5, 5, 4)]
for shape in shapes:
quats = np.random.random_sample(shape)
norms = np.linalg.norm(quats, axis=-1)
self.assertTrue(np.all(rowan.normalize(quats)
== quats / norms[..., np.newaxis]))
def render(self, scene):
geometry = fresnel.geometry.Polygon(
scene=scene,
vertices=self.vertices,
position=self.positions,
angle=rowan.geometry.angle(rowan.normalize(self.orientations)),
color=fresnel.color.linear(self.colors),
material=self._material,
outline_width=self.outline)
return geometry
def test_bounding_circle_radius_random_hull_rotation(points, rotation):
"""Test that rotating vertices does not change the bounding radius."""
assume(not np.all(rotation == 0))
hull = ConvexHull(points)
poly = Polygon(points[hull.vertices])
rotation = rowan.normalize(rotation)
rotated_vertices = rowan.rotate(rotation, poly.vertices)
poly_rotated = Polygon(rotated_vertices)
circle = poly.bounding_circle
rotated_circle = poly_rotated.bounding_circle
assert np.isclose(circle.radius, rotated_circle.radius)
def testfun(random_quat):
assume(not np.all(random_quat == 0))
random_quat = rowan.normalize(random_quat)
rotated_points = rowan.rotate(random_quat, square_points)
r = 1
poly = ConvexSpheropolygon(rotated_points, radius=r)
hull = ConvexHull(square_points[:, :2])
cap_area = np.pi * r * r
edge_area = np.sum(
np.linalg.norm(square_points - np.roll(square_points, 1, 0), axis=1), axis=0
)
sphero_area = cap_area + edge_area
assert np.isclose(poly.signed_area, hull.volume + sphero_area)
def f(self, s, a):
# input:
# s, nd array, (n,)
# a, nd array, (m,1)
# output
# dsdt, nd array, (n,1)
dsdt = np.zeros(self.n)
q = s[6:10]
omega = s[10:]
# get input
a = np.reshape(a, (self.m, ))
eta = np.dot(self.B0, a)
f_u = np.array([0, 0, eta[0]])
tau_u = np.array([eta[1], eta[2], eta[3]])
# dynamics
# dot{p} = v
dsdt[0:3] = s[3:6]
# mv = mg + R f_u
dsdt[3:6] = self.g + rowan.rotate(q, f_u) / self.mass
# dot{R} = R S(w)
# to integrate the dynamics, see
# https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/, and
# https://arxiv.org/pdf/1604.08139.pdf
qnew = rowan.calculus.integrate(q, omega, self.ave_dt)
qnew = rowan.normalize(qnew)
if qnew[0] < 0:
qnew = -qnew
# transform qnew to a "delta q" that works with the usual euler integration
dsdt[6:10] = (qnew - q) / self.ave_dt
# mJ = Jw x w + tau_u
dsdt[10:] = self.inv_J * (np.cross(self.J * omega, omega) + tau_u)
return dsdt.reshape((len(dsdt), 1))
def f(self, X, Z, A, t, logentry=None):
# x = X[0]
# y = X[1]
# z = X[2]
# xdot = X[3]
# ydot = X[4]
# zdot = X[5]
# qw = X[6]
# qx = X[7]
# qy = X[8]
# qz = X[9]
# wx = X[10]
# wy = X[11]
# wz = X[12]
a = Z**2
# a = A
J = self.J
dxdt = np.zeros(13)
q = X[6:10]
q = rowan.normalize(q)
omega = X[10:]
B0 = self.B0 * self.params['C_T']
# B0 = self.B0
eta = np.dot(B0, a)
f_u = np.array([0, 0, eta[0]])
tau_u = np.array([eta[1], eta[2], eta[3]])
dxdt[0:3] = X[3:6]
# dxdt[3:6] = -9.81 + rowan.rotate(q, f_u) / self.params['m']
dxdt[3:6] = np.array([0., 0., -9.81
]) + rowan.rotate(q, f_u) / self.params['m']
Vinf = self.params['Vwind_mean'] - np.linalg.norm(X[3:6])
Vinf_B = rowan.rotate(rowan.inverse(q), Vinf)
Vz_B = np.array([0.0, 0.0, Vinf_B[2]])
Vs_B = Vinf_B - Vz_B
alpha = np.arcsin(np.linalg.norm(Vz_B) / np.linalg.norm(Vinf_B))
n = np.sqrt(
np.multiply(a, self.B0[0, :]) /
(self.params['C_T'] * self.params['rho'] * self.params['D']**4))
Fs_B = (Vs_B / np.linalg.norm(Vs_B)
) * self.params['C_s'] * self.params['rho'] * sum(
n**self.params['k1']) * (np.linalg.norm(Vinf)**(
2 - self.params['k1'])) * (self.params['D']**(
2 + self.params['k1'])) * (
(np.pi / 2)**2 -
alpha**2) * (alpha + self.params['k2'])
# Fs_B = [0,0,0]
dxdt[3:6] += rowan.rotate(q, Fs_B) / self.mass
qnew = rowan.calculus.integrate(q, omega, self.ave_dt)
if qnew[0] < 0:
qnew = -qnew
dxdt[6:10] = (qnew - q) / self.ave_dt
dxdt[10:] = 1 / J * (np.cross(J * omega, omega) + tau_u)
dxdt[10:] += 1 / J * np.cross(
np.array([0.0, 0.0, self.params['D'] / 4]), Fs_B)
euler_o = rowan.to_euler(q, 'xyz')
if logentry is not None:
logentry['f_u'] = f_u
logentry['tau'] = tau_u
logentry['euler_o'] = euler_o
return dxdt.reshape((len(dxdt), 1))
def policy(self,
X,
pd=np.zeros(3),
vd=np.zeros(3),
ad=np.zeros(3),
jd=np.zeros(3),
sd=np.zeros(3),
logentry=None,
time=None,
K_i=0,
K_p=0,
K_d=0):
# print('baseline: X = ', X)
p_e = pd - X[:3]
v_e = vd - X[3:6]
print(X)
int_p_e = self.integrate_error(p_e, time)
F_d = (self.K_i * int_p_e + self.K_p * p_e +
self.K_d * v_e) * self.mass # TODO: add integral term
F_d[2] += self.g * self.mass
T_d = np.linalg.norm(F_d)
yaw_d = 0
roll_d = np.arcsin(
(F_d[0] * np.sin(yaw_d) - F_d[1] * np.cos(yaw_d)) / T_d)
pitch_d = np.arctan(
(F_d[0] * np.cos(yaw_d) + F_d[1] * np.sin(yaw_d)) / F_d[2])
euler_d = np.array([roll_d, pitch_d, yaw_d])
q = rowan.normalize(X[6:10])
euler = rowan.to_euler(q, 'xyz')
att_e = -(euler - euler_d)
th_r = rowan.normalize(
rowan.from_euler(att_e[0], att_e[1], att_e[2], 'xyz'))
th_d = rowan.normalize(
rowan.from_euler(euler_d[0], euler_d[1], euler_d[2], 'xyz'))
att_v_e = -X[10:]
torque = self.Att_p * att_e + self.Att_d * att_v_e
torque[0] *= self.J[0]
torque[1] *= self.J[1]
torque[2] *= self.J[2]
Jomega = np.array(
[self.J[0] * X[10], self.J[1] * X[11], self.J[2] * X[12]])
torque -= np.cross(Jomega, X[10:])
yawpart = -0.25 * torque[2] / self.t2t
rollpart = 0.25 / self.arm * torque[0] #/ self.t2t
pitchpart = 0.25 / self.arm * torque[1] #/ self.t2t
thrustpart = 0.25 * T_d
motorForce = np.array([
thrustpart - rollpart - pitchpart + yawpart,
thrustpart - rollpart + pitchpart - yawpart,
thrustpart + rollpart + pitchpart + yawpart,
thrustpart + rollpart - pitchpart - yawpart
])
Fc = motorForce / self.params['C_T']
omega = np.sqrt(np.maximum(Fc, self.params['motor_min_speed'])
) # Ensure each prop has a positive thrust
omega = np.minimum(
omega, self.params['motor_max_speed']) # Maximum rotation speed
# print('baseline: omega = ', omega)
# print('T', T_r)
# print('tau',tau_r)
# x_error = pd[0] - X[0]
# y_error = pd[1] - X[1]
# z_error = pd[2] - X[2]
#
# vx_error = vd[0] - X[3]
# vy_error = vd[1] - X[4]
# vz_error = vd[2] - X[5]
#
# # print('baseline: x_error = ', x_error)
# # print('baseline: y_error = ', y_error)
#
# ax_d = ad[0]
# ay_d = ad[1]
# az_d = ad[2]
#
# self.int_x_error += self.params.dt_posctrl * x_error
# self.int_y_error += self.params.dt_posctrl * y_error
# self.int_z_error += self.params.dt_posctrl * z_error
#
# ax_r = self.params['K_p_x'] * x_error + self.params['K_d_x'] * vx_error + self.params[
# 'K_i_x'] * self.int_x_error + ax_d
# ay_r = self.params['K_p_y'] * y_error + self.params['K_d_y'] * vy_error + self.params[
# 'K_i_y'] * self.int_y_error + ay_d
# az_r = self.params['K_p_z'] * z_error + self.params['K_d_z'] * vz_error + self.params[
# 'K_i_z'] * self.int_z_error + az_d
# # print('baseline: x PDI terms:',K_p_x * x_error, K_d_x * vx_error, K_i_x * intx_error, ax_d)
# # print('baseline: y PDI terms:',K_p_y * y_error, K_d_y * vy_error, K_i_y * inty_error, ay_d)
#
# jx_d = jd[0]
# jy_d = jd[1]
# jz_d = jd[2]
# # jx_r = K_p_x * vx_error + K_d_x * (ax_d - ax_r) + K_i_x * x_error + jx_d
# # jy_r = K_p_y * vy_error + K_d_y * (ay_d - ay_r) + K_i_y * y_error + jy_d
#
# sx_d = sd[0]
# sy_d = sd[1]
# sz_d = sd[2]
# # sx_r = K_p_x * (ax_d - ax_r) + K_d_x * (jx_d - jx_r) + K_i_x * vx_error + sx_d
# # sy_r = K_p_y * (ay_d - ay_r) + K_d_y * (jy_d - jy_r) + K_i_y * vy_error + sy_d
#
# # print('baseline: ay_r = ', ay_r)
#
# Fx_r = ax_r * self.params['m']
# Fy_r = (ay_r + self.params['g']) * self.params['m']
# Fz_r = az_r * self.params['m']
#
# Fy_r = np.maximum(Fy_r, 0.10 * self.params['g'] * self.params['m'])
#
#
# # print('baseline: Fx_r, Fy_r = ', Fx_r, Fy_r)
#
# T_r = np.minimum(np.sqrt(Fy_r ** 2 + Fx_r ** 2 + Fz_r ** 2), 125.) # / np.cos(X[2])
# if T_r > 124.:
# warn('thrust gets too high')
# # print('T_r = ', T_r)
# T_d = self.params['m'] * np.sqrt(ax_d ** 2 + (ay_d + self.params['g']) ** 2 + az_d ** 2)
#
# # th = X[2]
# th_r =
# th_d =
#
# mat = np.array([[-np.sin(th_d), -T_d * np.cos(th_d)], #TODO: use th_d, T_d
# [np.cos(th_d), -T_d * np.sin(th_d)]])
# b = self.params['m'] * np.array([jx_d, jy_d, jz_d])#
# v = np.linalg.solve(mat, b)
# T_d_dot = v[0]
# w_d = v[1]
# # w = X[5]
#
# mat = np.array([[-np.sin(th_d), -T_d * np.cos(th_d)],
# [np.cos(th_d), -T_d * np.sin(th_d)]])
# b = self.params['m'] * np.array([sx_d, sy_d]) - np.array(
# [-2. * T_d_dot * np.cos(th_d) * w_d + T_d * np.sin(th_d) * (w_d ** 2),
# -2. * T_d_dot * np.sin(th_d) * w_d - T_d * np.cos(th_d) * (w_d ** 2)])
#
# v = np.linalg.solve(mat, b)
# # T_d_ddot = v[0]
# alpha_d = v[1]
# # print('baseline_pos: Tdot, w_d, alpha_d', Tdot, w_d, alpha_d)
w_d = 0
alpha_d = 0
if logentry is not None:
logentry['th_d'] = th_d
logentry['th_r'] = th_r
logentry['T_d'] = T_d
logentry['euler'] = euler
logentry['att_e'] = att_e
# logentry['Fx_r'] = Fx_r
# logentry['Fy_r'] = Fy_r
# logentry['Fz_r'] = Fz_r
# logentry['int_x_error'] = self.int_x_error
# logentry['int_y_error'] = self.int_y_error
# logentry['int_y_error'] = self.int_z_error
return T_d, th_d, w_d, alpha_d, omega, motorForce
def quat():
np.random.seed(0)
return rowan.normalize(rowan.random.rand(1000))
fibre_q[fi] = {}
nn_gridPts[fi] = {}
nn_gridDist[fi] = {}
for yi in trange(len(xyz_pf)):
y = xyz_pf[yi]
# for fi,fam in enumerate(pf.symHKL):
axis = np.cross(fam, y)
angle = np.arccos(np.dot(fam, y))
q0 = quat.from_axis_angle(axis, angle)
q0_n = quat.normalize(q0)
q1_n = [quat.normalize(quat.from_axis_angle(h, omega)) for h in fam]
# eu = np.zeros((len(omega),3,fam.shape[0]))
eu2 = []
qfib = np.zeros((len(q1_n[0]), len(q0_n), 4))
for sym_eq, (qA, qB) in enumerate(zip(q0_n, q1_n)):
temp = quat.multiply(qA, qB)
qfib[:, sym_eq, :] = temp
phi1, Phi, phi2 = quat2eu(qfib)
def check_orientation(central_orientation, constituent_orientation,
local_orientation):
expected_orientation = rowan.normalize(
rowan.multiply(central_orientation, local_orientation))
assert np.allclose(expected_orientation, local_orientation)
def calcFibre(symHKL, yset, qgrid, phi, rad, tree, euc_rad):
cphi = np.cos(phi / 2)
sphi = np.sin(phi / 2)
q0 = {}
q = {}
qf = {}
axis = {}
omega = {}
fibre_e = {}
fibre_q = {}
nn_gridPts = {}
nn_gridDist = {}
egrid_trun = {}
for fi, fam in enumerate(tqdm(symHKL)):
fibre_e[fi] = {}
fibre_q[fi] = {}
nn_gridPts[fi] = {}
nn_gridDist[fi] = {}
egrid_trun[fi] = {}
q0[fi] = {}
q[fi] = {}
axis[fi] = {}
omega[fi] = {}
""" set proper iterator """
if isinstance(yset, dict): it = yset[fi]
else: it = yset
for yi, y in enumerate(it):
axis[fi][yi] = np.cross(fam, y)
axis[fi][yi] = axis[fi][yi] / np.linalg.norm(axis[fi][yi],
axis=1)[:, None]
omega[fi][yi] = np.arccos(np.dot(fam, y))
q0[fi][yi] = {}
q[fi][yi] = {}
qf[yi] = {}
qfib = np.zeros((len(phi), len(fam), 4))
for hi, HxY in enumerate(axis[fi][yi]):
q0[fi][yi][hi] = quat.normalize(
np.hstack([
np.cos(omega[fi][yi][hi] / 2),
np.sin(omega[fi][yi][hi] / 2) * HxY
]))
q[fi][yi][hi] = quat.normalize(
np.hstack([
cphi[:, np.newaxis],
np.tile(y, (len(cphi), 1)) * sphi[:, np.newaxis]
]))
qf[yi][hi] = quat.multiply(q[fi][yi][hi], q0[fi][yi][hi])
for qi in range(qf[yi][hi].shape[0]):
qfib[qi, hi, :] = qf[yi][hi][qi, :]
phi1, Phi, phi2 = quat2eu(qfib)
phi1 = np.where(phi1 < 0, phi1 + 2 * np.pi,
phi1) #brnng back to 0 - 2pi
Phi = np.where(Phi < 0, Phi + np.pi, Phi) #brnng back to 0 - pi
phi2 = np.where(phi2 < 0, phi2 + 2 * np.pi,
phi2) #brnng back to 0 - 2pi
eu_fib = np.stack((phi1, Phi, phi2), axis=2)
eu_fib = np.reshape(eu_fib, (eu_fib.shape[0] * eu_fib.shape[1],
eu_fib.shape[2])) #new method
fz = (eu_fib[:, 0] <= od._phi1max) & (
eu_fib[:, 1] <= od._Phimax) & (eu_fib[:, 2] <= od._phi2max)
fz_idx = np.nonzero(fz)
fibre_e[fi][yi] = eu_fib
fib_idx = np.unravel_index(fz_idx[0],
(qfib.shape[0], qfib.shape[1]))
fibre_q[fi][yi] = qfib
""" reduce geodesic query size """
qfib_pos = np.copy(qfib[fib_idx])
qfib_pos[qfib_pos[:, 0] < 0] *= -1
query = np.concatenate(tree.query_radius(qfib_pos, euc_rad))
query_uni = np.unique(query)
qgrid_trun = qgrid[query_uni]
qgrid_trun_idx = np.arange(len(qgrid))[
query_uni] #store indexes to retrieve original grid pts later
""" distance calc """
temp = quatMetricNumba(qgrid_trun, qfib[fib_idx])
""" find tube """
tube = (temp <= rad)
temp = np.column_stack((np.argwhere(tube)[:, 0], temp[tube]))
""" round very small values """
temp = np.round(temp, decimals=7)
""" move values at zero to very small (1E-5) """
temp[:, 1] = np.where(temp[:, 1] == 0, 1E-5, temp[:, 1])
""" sort by min distance """
temp = temp[np.argsort(temp[:, 1], axis=0)]
""" return unique pts (first in list) """
uni_pts = np.unique(temp[:, 0], return_index=True)
nn_gridPts[fi][yi] = qgrid_trun_idx[uni_pts[0].astype(int)]
nn_gridDist[fi][yi] = temp[uni_pts[1], 1]
# egrid_trun[fi][yi] = bungeAngs[query_uni]
return nn_gridPts, nn_gridDist, fibre_e, axis, omega
def calcFibre(h, pf_y, omega):
fibre_e = {}
fibre_q = {}
nn_gridPts = {}
nn_gridDist = {}
h_y = np.cross(h, pf_y)
h_yAng = np.arccos(np.dot(pf_y, h))
qh_y = np.column_stack((np.cos(h_yAng/2),np.sin(h_yAng/2)[:,None]*h_y))
qh_y = quat.normalize(qh_y)
qy_om = np.column_stack((np.cos(omega/2), np.sin(omega/2)))
# qh_y = quat.vector_vector_rotation(h,pf_y)
# qh_om = quat.from_axis_angle(h,omega)
for yi,y in enumerate(pf_y):
qy_om = np.column_stack((np.cos(omega/2), np.sin(omega/2)[:,None]*np.tile(y,(len(omega),1))))
qy_om = quat.normalize(qy_om)
qfib = quat.multiply(qy_om,qh_y[yi,:])
""" reshape for tiling, simplify?? """
qfib = qfib.T.reshape((1,4,len(omega)),order='F')
qfib = np.tile(qfib,(quatSymOps.shape[1],1,1))
qfib = qfib.transpose((2,0,1))
""" apply symmetry """
qfib = quat.multiply(quatSymOps,qfib)
""" symmetry ops, then filter by given maximum bunge angles """
phi1, Phi, phi2 = quat2eu(qfib)
phi1 = np.where(phi1 < 0, phi1 + 2*np.pi, phi1) #brnng back to 0 - 2pi
Phi = np.where(Phi < 0, Phi + np.pi, Phi) #brnng back to 0 - pi
phi2 = np.where(phi2 < 0, phi2 + 2*np.pi, phi2) #brnng back to 0 - 2pi
eu_fib = np.stack( (phi1, Phi, phi2), axis=2 )
eu_fib = np.reshape( eu_fib, (eu_fib.shape[0]*eu_fib.shape[1], eu_fib.shape[2]) )
fz = (eu_fib[:,0] < od._phi1max) & (eu_fib[:,1] < od._Phimax) & (eu_fib[:,2] < od._phi2max)
fz_idx = np.nonzero(fz)
fibre_e[yi] = eu_fib[fz]
fib_idx = np.unravel_index(fz_idx[0], (qfib.shape[0],qfib.shape[1]))
fibre_q[yi] = qfib[fib_idx].reshape((len(fz_idx[0]),4))
""" distance calc """
temp = quatMetricNumba(qgrid,qfib[fib_idx])
""" find tube """
tube = (temp <= rad)
temp = np.column_stack((np.argwhere(tube)[:,0],temp[tube]))
""" sort by min distance """
temp = temp[np.argsort(temp[:,1],axis=0)]
""" return unique pts (first in list) """
uni_pts = np.unique(temp[:,0],return_index=True)
nn_gridPts[yi] = uni_pts[0]
nn_gridDist[yi] = temp[uni_pts[1],1]
return nn_gridPts, nn_gridDist, fibre_e
def deduce_state(self, s):
rpy = np.degrees(rowan.to_euler(rowan.normalize(s[6:10]), 'xyz'))
return rpy
omega = np.radians(np.arange(0,365,5))
ro_y = {}
yi = 0
for v in tqdm(pf_xyz):
ro_h = {}
for fi,fam in enumerate(pf.symHKL):
axis = np.cross(fam,v)
angle = np.arccos(np.dot(fam,v))
q0 = rowan.from_axis_angle(axis, angle)
q0_n = rowan.normalize(q0)
q1_n = [rowan.normalize(rowan.from_axis_angle(h, omega)) for h in fam]
# eu = np.zeros((len(omega),3,fam.shape[0]))
eu = []
for i,(qA,qB) in enumerate(zip(q0_n,q1_n)):
qF = rowan.multiply(qA, qB)
with np.errstate(divide='ignore'):
temp = np.array((qF[:,1]/qF[:,0],qF[:,2]/qF[:,0],qF[:,3]/qF[:,0])).T
ax = ro2ax(temp)
om = ax2om(ax)
for yi,by in enumerate(xyz_pf):
# brass_y2[:,:,yi] = by
HxY[yi] = np.cross(symHKL[0],by)
HxY[yi] = HxY[yi] / np.linalg.norm(HxY[yi],axis=1)[:,None]
ome[yi] = np.arccos(np.dot(symHKL[0],by))
q0[yi] = {}
q[yi] = {}
qf[yi] = {}
fibre_marc[yi] = np.zeros_like(brassFibre[yi])
for hi,h in enumerate(HxY[yi]):
q0[yi][hi] = quat.normalize(np.hstack( [ np.cos(ome[yi][hi]/2), np.sin(ome[yi][hi]/2) * h ] ))
q[yi][hi] = quat.normalize(np.hstack( [ cphi[:, np.newaxis], np.tile( by, (len(cphi),1) ) * sphi[:, np.newaxis] ] ))
qf[yi][hi] = quat.multiply(q[yi][hi], q0[yi][hi])
for qi in range(qf[yi][hi].shape[0]):
fibre_marc[yi][qi,hi,:] = qf[yi][hi][qi,:]
phi1, Phi, phi2 = quat2eu(fibre_marc[yi])
phi1 = np.where(phi1 < 0, phi1 + 2*np.pi, phi1) #brnng back to 0 - 2pi
Phi = np.where(Phi < 0, Phi + np.pi, Phi) #brnng back to 0 - pi
phi2 = np.where(phi2 < 0, phi2 + 2*np.pi, phi2) #brnng back to 0 - 2pi
eu_fib = np.stack( (phi1, Phi, phi2), axis=2 )
