Commit 8e8dfa43 authored by Viktor Reshniak's avatar Viktor Reshniak
Browse files

update rotation matrix

parent 3b3f3f84

peak_integration.py

+50 −4
Original line number Diff line number Diff line
@@ -325,13 +325,59 @@ def rotation_matrix(angles, return_gradient=False, return_hessian=False): 
	# Rotation matrix of 0-1-2 rotation

	if ndims==1:

		R = np.array([[1]])

		if return_gradient:

			dR = np.array([[0]])

		if return_hessian:

			d2R = np.array([[0]])

	elif ndims==2:

		R = np.array([c,-s],[s,c])

		if return_gradient:

			dR = np.array([-s,-c],[c,-s])

		if return_hessian:

			d2R = -R

	elif ndims==3:

		R = np.array([ [c[1]*c[0],-c[1]*s[0],s[1]], [s[2]*s[1]*c[0]+c[2]*s[0],-s[2]*s[1]*s[0]+c[2]*c[0],-s[2]*c[1]], [-c[2]*s[1]*c[0]+s[2]*s[0],c[2]*s[1]*s[0]+s[2]*c[0],c[2]*c[1]] ])

	# # Rotation matrix of 2-1-0 rotation

	# R = np.array([ [c[2]*c[1],-s[2]*c[0]+c[2]*s[1]*s[0],s[2]*s[0]+c[2]*s[1]*c[0]], [s[2]*c[1], c[2]*c[0]+s[2]*s[1]*s[0],-c[2]*s[0]+s[2]*s[1]*c[0]], [-s[1],c[1]*s[0],c[1]*c[0]] ])

	return R

		Rx = np.array([[   1,    0,    0],[   0, c[0],-s[0]],[   0, s[0], c[0]]])

		Ry = np.array([[c[1],    0,-s[1]],[   0,    1,    0],[s[1],    0, c[1]]])

		Rz = np.array([[c[2],-s[2],    0],[s[2], c[2],    0],[   0,    0,    1]])



		R  = Rz @ Ry @ Rx



		if return_gradient:

			dRx = np.array([[    0,    0,    0],[   0,-s[0],-c[0]],[   0, c[0],-s[0]]])

			dRy = np.array([[-s[1],    0,-c[1]],[   0,    0,    0],[c[1],    0,-s[1]]])

			dRz = np.array([[-s[2],-c[2],    0],[c[2],-s[2],    0],[   0,    0,    0]])



			dR  = np.stack((Rz@Ry@dRx,Rz@dRy@Rx,dRz@Ry@Rx), axis=0)



		if return_hessian:

			dRxx = np.array([[    0,    0,    0],[    0,-c[0], s[0]],[    0,-s[0],-c[0]]])

			dRyy = np.array([[-c[1],    0, s[1]],[    0,    0,    0],[-s[1],    0,-c[1]]])

			dRzz = np.array([[-c[2], s[2],    0],[-s[2],-c[2],    0],[    0,    0,    0]])



			d2R = np.zeros((3,3,3,3))



			d2R[0,0,...] =  Rz  @  Ry  @ dRxx

			d2R[1,1,...] =  Rz  @ dRyy @  Rx

			d2R[2,2,...] = dRzz @  Ry  @  Rx



			d2R[0,1,...] =  Rz  @ dRy @ dRx

			d2R[0,2,...] = dRz  @  Ry @ dRx

			d2R[1,2,...] = dRz  @ dRy @  Rx



			d2R[1,0,...] = d2R[0,1,...]

			d2R[2,0,...] = d2R[0,2,...]

			d2R[2,1,...] = d2R[1,2,...]



	if return_hessian:

		return R, dR, d2R

	elif return_gradient:

		return R, dR, None

	else:

		return R, None, None

	# 	R = np.array([ [c[1]*c[0],-c[1]*s[0],s[1]], [s[2]*s[1]*c[0]+c[2]*s[0],-s[2]*s[1]*s[0]+c[2]*c[0],-s[2]*c[1]], [-c[2]*s[1]*c[0]+s[2]*s[0],c[2]*s[1]*s[0]+s[2]*c[0],c[2]*c[1]] ])

	# # # Rotation matrix of 2-1-0 rotation

	# # R = np.array([ [c[2]*c[1],-s[2]*c[0]+c[2]*s[1]*s[0],s[2]*s[0]+c[2]*s[1]*c[0]], [s[2]*c[1], c[2]*c[0]+s[2]*s[1]*s[0],-c[2]*s[0]+s[2]*s[1]*c[0]], [-s[1],c[1]*s[0],c[1]*c[0]] ])

	# return R





def squared_mahalanobis_distance(mu, sqrtP, x):