// File: /workspace/github_cpp_makefile_star2_O2/jingdao[P]PointCloudApp/pcd.cpp
// Function: PCD::quaternionFromAngle(float, float, float)
PCD::Quaternion PCD::quaternionFromAngle(float rx,float ry, float rz) {
	float r,i,j,k;
	float r1 = rz * M_PI / 180;
	float r2 = ry * M_PI / 180;
	float r3 = rx * M_PI / 180;
	float M[9];
	M[0] = cos(r2)*cos(r3);
	M[1] = -cos(r2) * sin(r3);
	M[2] = sin(r2);
	M[3] = cos(r1) * sin(r3) + cos(r3) * sin(r1) * sin(r2);
	M[4] = cos(r1) * cos(r3) - sin(r1) * sin(r2) * sin(r3);
	M[5] = -cos(r2) * sin(r1);
	M[6] = sin(r1) * sin(r3) - cos(r1) * cos(r3) * sin(r2);
	M[7] = cos(r3) * sin(r1) + cos(r1) * sin(r2) * sin(r3);
	M[8] = cos(r1) * cos(r2);

	double tr = M[0] + M[4] + M[8];

	if (tr > 0) { 
		float S = sqrt(tr+1.0) * 2;
		r = (float)(0.25 * S);
		i = (float)((M[7] - M[5]) / S);
		j = (float)((M[2] - M[6]) / S);
		k = (float)((M[3] - M[1]) / S);
	} else if ((M[0] > M[4])&&(M[0]> M[8])) {
		float S = sqrt(1.0 + M[0]- M[4] - M[8]) * 2;
		i = (float)(0.25 * S);
		r = (float)((M[7] - M[5]) / S);
		k = (float)((M[2] + M[6]) / S);
		j = (float)((M[3] + M[1]) / S);
	} else if (M[4]>M[8]) {
		float S = sqrt(1.0 - M[0] + M[4] - M[8]) * 2; 
		j = (float)(0.25 * S);
		k = (float)((M[7] + M[5]) / S);
		r = (float)((M[2] - M[6]) / S);
		i = (float)((M[3] + M[1]) / S); 
	} else {
		float S = sqrt(1.0 - M[0] - M[4] + M[8]) * 2; 
		k = (float)(0.25 * S);
		j = (float)((M[7] + M[5]) / S);
		i = (float)((M[2] + M[6]) / S);
		r = (float)((M[3] - M[1]) / S); 
	}
	Quaternion res = {r,i,j,k};
	return res;
}

// Assembly:
subq $0xe8, %rsp
cvtss2sd %xmm2, %xmm2
movsd 0x4e17(%rip), %xmm3     # 0xa238
mulsd %xmm3, %xmm2
movsd 0x4e03(%rip), %xmm4     # 0xa230
divsd %xmm4, %xmm2
cvtsd2ss %xmm2, %xmm2
movss %xmm2, (%rsp)
cvtss2sd %xmm1, %xmm1
mulsd %xmm3, %xmm1
divsd %xmm4, %xmm1
cvtsd2ss %xmm1, %xmm1
movss %xmm1, 0x8(%rsp)
cvtss2sd %xmm0, %xmm0
mulsd %xmm3, %xmm0
divsd %xmm4, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x4(%rsp)
movaps %xmm1, %xmm0
callq 0x2130
movss %xmm0, 0x20(%rsp)
movss 0x4(%rsp), %xmm0
callq 0x2130
mulss 0x20(%rsp), %xmm0
movss %xmm0, 0x4c(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2130
xorps 0x4f23(%rip), %xmm0     # 0xa3c0
movaps %xmm0, 0x20(%rsp)
movss 0x4(%rsp), %xmm0
callq 0x2190
mulss 0x20(%rsp), %xmm0
movaps %xmm0, 0x70(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2190
movaps %xmm0, 0x60(%rsp)
movss (%rsp), %xmm0
callq 0x2130
movaps %xmm0, 0x30(%rsp)
movss 0x4(%rsp), %xmm0
callq 0x2190
movaps %xmm0, 0x20(%rsp)
movss 0x4(%rsp), %xmm0
callq 0x2130
movaps %xmm0, 0xa0(%rsp)
movss (%rsp), %xmm0
callq 0x2190
movaps %xmm0, 0x50(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2190
movaps %xmm0, 0xd0(%rsp)
movss (%rsp), %xmm0
callq 0x2130
movss %xmm0, 0x10(%rsp)
movss 0x4(%rsp), %xmm0
callq 0x2130
movss %xmm0, 0xc(%rsp)
movss (%rsp), %xmm0
callq 0x2190
movss %xmm0, 0x90(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2190
movss %xmm0, 0x80(%rsp)
movss 0x4(%rsp), %xmm0
callq 0x2190
movss 0x80(%rsp), %xmm1
mulss 0x90(%rsp), %xmm1
mulss %xmm0, %xmm1
movss 0xc(%rsp), %xmm0
mulss 0x10(%rsp), %xmm0
subss %xmm1, %xmm0
movss %xmm0, 0xc(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2130
xorps 0x4e13(%rip), %xmm0     # 0xa3c0
movaps %xmm0, 0x10(%rsp)
movss (%rsp), %xmm0
callq 0x2190
mulss 0x10(%rsp), %xmm0
movaps %xmm0, 0x10(%rsp)
movss (%rsp), %xmm0
callq 0x2190
movaps %xmm0, 0x90(%rsp)
movss 0x4(%rsp), %xmm0
callq 0x2190
movaps %xmm0, 0x80(%rsp)
movss (%rsp), %xmm0
callq 0x2130
movaps %xmm0, 0xc0(%rsp)
movss 0x4(%rsp), %xmm0
callq 0x2130
movaps %xmm0, 0xb0(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2190
movaps 0xc0(%rsp), %xmm3
xorps 0x4d95(%rip), %xmm3     # 0xa3c0
movaps 0xa0(%rsp), %xmm1
unpcklps 0xb0(%rsp), %xmm1       # xmm1 = xmm1[0],mem[0],xmm1[1],mem[1]
movaps 0x50(%rsp), %xmm2
unpcklps %xmm3, %xmm2            # xmm2 = xmm2[0],xmm3[0],xmm2[1],xmm3[1]
mulps %xmm1, %xmm2
movaps 0xd0(%rsp), %xmm1
unpcklps %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0],xmm1[1],xmm0[1]
mulps %xmm2, %xmm1
movaps %xmm1, %xmm2
movaps 0x30(%rsp), %xmm0
unpcklps 0x90(%rsp), %xmm0       # xmm0 = xmm0[0],mem[0],xmm0[1],mem[1]
movaps 0x20(%rsp), %xmm1
unpcklps 0x80(%rsp), %xmm1       # xmm1 = xmm1[0],mem[0],xmm1[1],mem[1]
mulps %xmm0, %xmm1
addps %xmm2, %xmm1
movaps %xmm1, 0x20(%rsp)
movss 0x4(%rsp), %xmm0
callq 0x2130
movss %xmm0, 0xa0(%rsp)
movss (%rsp), %xmm0
callq 0x2190
movaps %xmm0, 0x30(%rsp)
movss (%rsp), %xmm0
callq 0x2130
movss %xmm0, 0x50(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2190
mulss 0x50(%rsp), %xmm0
movss %xmm0, 0x50(%rsp)
movss 0x4(%rsp), %xmm0
callq 0x2190
mulss 0x50(%rsp), %xmm0
movaps 0x30(%rsp), %xmm1
mulss 0xa0(%rsp), %xmm1
addss %xmm0, %xmm1
movaps %xmm1, 0x30(%rsp)
movss (%rsp), %xmm0
callq 0x2130
movss %xmm0, (%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2130
movss 0xc(%rsp), %xmm4
movss 0x4c(%rsp), %xmm3
mulss (%rsp), %xmm0
movaps %xmm3, %xmm1
addss %xmm4, %xmm1
addss %xmm0, %xmm1
xorps %xmm2, %xmm2
ucomiss %xmm2, %xmm1
jbe 0x574e
cvtss2sd %xmm1, %xmm0
addsd 0x4b10(%rip), %xmm0     # 0xa248
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x57ce
sqrtsd %xmm0, %xmm0
jmp 0x57d3
ucomiss %xmm4, %xmm3
cvtss2sd %xmm0, %xmm1
jbe 0x578b
ucomiss %xmm0, %xmm3
jbe 0x578b
cvtss2sd %xmm3, %xmm0
addsd 0x4ae0(%rip), %xmm0     # 0xa248
cvtss2sd %xmm4, %xmm2
subsd %xmm2, %xmm0
subsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x5844
sqrtsd %xmm0, %xmm0
jmp 0x5849
ucomiss %xmm0, %xmm4
cvtss2sd %xmm3, %xmm2
movsd 0x4aae(%rip), %xmm0     # 0xa248
subsd %xmm2, %xmm0
cvtss2sd %xmm4, %xmm2
movaps 0x10(%rsp), %xmm4
addss 0x30(%rsp), %xmm4
jbe 0x5825
addsd %xmm2, %xmm0
subsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x589a
sqrtsd %xmm0, %xmm0
jmp 0x58a9
callq 0x2170
movaps 0x10(%rsp), %xmm1
movaps 0x30(%rsp), %xmm4
addsd %xmm0, %xmm0
cvtsd2ss %xmm0, %xmm0
subss %xmm1, %xmm4
movaps 0x60(%rsp), %xmm2
movaps 0x20(%rsp), %xmm3
unpcklps %xmm3, %xmm2            # xmm2 = xmm2[0],xmm3[0],xmm2[1],xmm3[1]
movaps 0x70(%rsp), %xmm1
shufps $0xd4, %xmm3, %xmm1     # xmm1 = xmm1[0,1],xmm3[1,3]
shufps $0xe2, %xmm1, %xmm1     # xmm1 = xmm1[2,0,2,3]
subps %xmm1, %xmm2
shufps $0x40, %xmm2, %xmm4     # xmm4 = xmm4[0,0],xmm2[0,1]
movaps %xmm0, %xmm1
shufps $0x0, %xmm0, %xmm1      # xmm1 = xmm1[0,0],xmm0[0,0]
divps %xmm1, %xmm4
mulss 0x4c34(%rip), %xmm0     # 0xa450
movss %xmm0, %xmm4            # xmm4 = xmm0[0],xmm4[1,2,3]
jmp 0x594b
subsd %xmm2, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x58f2
sqrtsd %xmm0, %xmm0
jmp 0x5901
callq 0x2170
movaps 0x10(%rsp), %xmm1
movaps 0x30(%rsp), %xmm4
addsd %xmm0, %xmm0
cvtsd2ss %xmm0, %xmm0
subss %xmm1, %xmm4
movaps 0x70(%rsp), %xmm3
unpcklps 0x60(%rsp), %xmm3       # xmm3 = xmm3[0],mem[0],xmm3[1],mem[1]
addps 0x20(%rsp), %xmm3
movaps 0x4b9b(%rip), %xmm1     # 0xa410
movaps %xmm1, %xmm2
shufps $0x1, %xmm4, %xmm2      # xmm2 = xmm2[1,0],xmm4[0,0]
shufps $0x42, %xmm3, %xmm2     # xmm2 = xmm2[2,0],xmm3[0,1]
shufps $0x0, %xmm0, %xmm0      # xmm0 = xmm0[0,0,0,0]
divps %xmm0, %xmm2
mulps %xmm1, %xmm0
shufps $0x1, %xmm2, %xmm0      # xmm0 = xmm0[1,0],xmm2[0,0]
shufps $0xe2, %xmm2, %xmm0     # xmm0 = xmm0[2,0],xmm2[2,3]
movaps %xmm0, %xmm4
jmp 0x594b
movaps %xmm4, 0x10(%rsp)
callq 0x2170
movaps 0x10(%rsp), %xmm4
addsd %xmm0, %xmm0
cvtsd2ss %xmm0, %xmm0
movaps 0x70(%rsp), %xmm2
movaps 0x20(%rsp), %xmm3
addss %xmm3, %xmm2
shufps $0x55, %xmm3, %xmm3     # xmm3 = xmm3[1,1,1,1]
movaps 0x60(%rsp), %xmm1
subss %xmm3, %xmm1
movlhps %xmm1, %xmm2            # xmm2 = xmm2[0],xmm1[0]
movaps 0x4b2a(%rip), %xmm1     # 0xa400
shufps $0xe4, %xmm1, %xmm4     # xmm4 = xmm4[0,1],xmm1[2,3]
shufps $0x22, %xmm4, %xmm2     # xmm2 = xmm2[2,0],xmm4[2,0]
shufps $0x0, %xmm0, %xmm0      # xmm0 = xmm0[0,0,0,0]
divps %xmm0, %xmm2
mulps %xmm1, %xmm0
shufps $0x32, %xmm2, %xmm0     # xmm0 = xmm0[2,0],xmm2[3,0]
shufps $0x84, %xmm0, %xmm2     # xmm2 = xmm2[0,1],xmm0[0,2]
jmp 0x5948
movaps %xmm4, 0x10(%rsp)
callq 0x2170
movaps 0x10(%rsp), %xmm4
addsd %xmm0, %xmm0
cvtsd2ss %xmm0, %xmm0
movaps 0x20(%rsp), %xmm3
movaps %xmm3, %xmm1
shufps $0x55, %xmm3, %xmm1     # xmm1 = xmm1[1,1],xmm3[1,1]
movaps 0x60(%rsp), %xmm2
addss %xmm1, %xmm2
subss 0x70(%rsp), %xmm3
movlhps %xmm3, %xmm2            # xmm2 = xmm2[0],xmm3[0]
movaps 0x4ac2(%rip), %xmm1     # 0xa3f0
shufps $0x30, %xmm1, %xmm4     # xmm4 = xmm4[0,0],xmm1[3,0]
shufps $0x82, %xmm4, %xmm2     # xmm2 = xmm2[2,0],xmm4[0,2]
shufps $0x0, %xmm0, %xmm0      # xmm0 = xmm0[0,0,0,0]
divps %xmm0, %xmm2
mulps %xmm1, %xmm0
shufps $0x23, %xmm2, %xmm0     # xmm0 = xmm0[3,0],xmm2[2,0]
shufps $0x24, %xmm0, %xmm2     # xmm2 = xmm2[0,1],xmm0[2,0]
movaps %xmm2, %xmm4
movaps %xmm4, %xmm1
unpckhpd %xmm4, %xmm1            # xmm1 = xmm1[1],xmm4[1]
movaps %xmm4, %xmm0
addq $0xe8, %rsp
retq
nop