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 )