// File: /workspace/github_cpp_makefile_star2_O1/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 $0x68, %rsp
cvtss2sd %xmm2, %xmm2
movsd 0x568c(%rip), %xmm3     # 0xb238
mulsd %xmm3, %xmm2
movsd 0x5678(%rip), %xmm4     # 0xb230
divsd %xmm4, %xmm2
cvtsd2ss %xmm2, %xmm2
movss %xmm2, (%rsp)
cvtss2sd %xmm1, %xmm1
mulsd %xmm3, %xmm1
divsd %xmm4, %xmm1
cvtsd2ss %xmm1, %xmm1
movss %xmm1, 0xc(%rsp)
cvtss2sd %xmm0, %xmm0
mulsd %xmm3, %xmm0
divsd %xmm4, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x8(%rsp)
movaps %xmm1, %xmm0
callq 0x2130
movss %xmm0, 0x40(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2130
mulss 0x40(%rsp), %xmm0
movss %xmm0, 0x64(%rsp)
movss 0xc(%rsp), %xmm0
callq 0x2130
xorps 0x5788(%rip), %xmm0     # 0xb3b0
movaps %xmm0, 0x40(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2190
mulss 0x40(%rsp), %xmm0
movss %xmm0, 0x14(%rsp)
movss 0xc(%rsp), %xmm0
callq 0x2190
movaps %xmm0, 0x50(%rsp)
movss (%rsp), %xmm0
callq 0x2130
movss %xmm0, 0x30(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2190
movaps %xmm0, 0x40(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2130
movss %xmm0, 0x20(%rsp)
movss (%rsp), %xmm0
callq 0x2190
mulss 0x20(%rsp), %xmm0
movss %xmm0, 0x20(%rsp)
movss 0xc(%rsp), %xmm0
callq 0x2190
mulss 0x20(%rsp), %xmm0
movaps 0x40(%rsp), %xmm1
mulss 0x30(%rsp), %xmm1
addss %xmm0, %xmm1
movaps %xmm1, 0x40(%rsp)
movss (%rsp), %xmm0
callq 0x2130
movss %xmm0, 0x30(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2130
movss %xmm0, 0x1c(%rsp)
movss (%rsp), %xmm0
callq 0x2190
movss %xmm0, 0x20(%rsp)
movss 0xc(%rsp), %xmm0
callq 0x2190
movss %xmm0, 0x4(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2190
movss 0x4(%rsp), %xmm1
mulss 0x20(%rsp), %xmm1
mulss %xmm0, %xmm1
movss 0x1c(%rsp), %xmm0
mulss 0x30(%rsp), %xmm0
subss %xmm1, %xmm0
movss %xmm0, 0x1c(%rsp)
movss 0xc(%rsp), %xmm0
callq 0x2130
xorps 0x566b(%rip), %xmm0     # 0xb3b0
movaps %xmm0, 0x30(%rsp)
movss (%rsp), %xmm0
callq 0x2190
mulss 0x30(%rsp), %xmm0
movss %xmm0, 0x4(%rsp)
movss (%rsp), %xmm0
callq 0x2190
movss %xmm0, 0x20(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2190
movss %xmm0, 0x30(%rsp)
movss (%rsp), %xmm0
callq 0x2130
movss %xmm0, 0x18(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2130
movss %xmm0, 0x10(%rsp)
movss 0xc(%rsp), %xmm0
callq 0x2190
movss 0x10(%rsp), %xmm1
mulss 0x18(%rsp), %xmm1
mulss %xmm0, %xmm1
movss 0x30(%rsp), %xmm0
mulss 0x20(%rsp), %xmm0
subss %xmm1, %xmm0
movss %xmm0, 0x30(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2130
movss %xmm0, 0x18(%rsp)
movss (%rsp), %xmm0
callq 0x2190
movaps %xmm0, 0x20(%rsp)
movss (%rsp), %xmm0
callq 0x2130
movss %xmm0, 0x10(%rsp)
movss 0xc(%rsp), %xmm0
callq 0x2190
mulss 0x10(%rsp), %xmm0
movss %xmm0, 0x10(%rsp)
movss 0x8(%rsp), %xmm0
callq 0x2190
mulss 0x10(%rsp), %xmm0
movaps 0x20(%rsp), %xmm1
mulss 0x18(%rsp), %xmm1
addss %xmm0, %xmm1
movaps %xmm1, 0x20(%rsp)
movss (%rsp), %xmm0
callq 0x2130
movss %xmm0, (%rsp)
movss 0xc(%rsp), %xmm0
callq 0x2130
movss 0x1c(%rsp), %xmm4
movss 0x64(%rsp), %xmm3
mulss (%rsp), %xmm0
movaps %xmm3, %xmm1
addss %xmm4, %xmm1
addss %xmm0, %xmm1
xorps %xmm2, %xmm2
ucomiss %xmm2, %xmm1
jbe 0x5ea2
xorps %xmm0, %xmm0
cvtss2sd %xmm1, %xmm0
addsd 0x53bc(%rip), %xmm0     # 0xb248
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x5f32
sqrtsd %xmm0, %xmm0
jmp 0x5f37
ucomiss %xmm4, %xmm3
xorps %xmm1, %xmm1
cvtss2sd %xmm0, %xmm1
jbe 0x5ee8
ucomiss %xmm0, %xmm3
jbe 0x5ee8
xorps %xmm0, %xmm0
cvtss2sd %xmm3, %xmm0
addsd 0x5386(%rip), %xmm0     # 0xb248
xorps %xmm2, %xmm2
cvtss2sd %xmm4, %xmm2
subsd %xmm2, %xmm0
subsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x5fa6
sqrtsd %xmm0, %xmm0
jmp 0x5fab
ucomiss %xmm0, %xmm4
xorps %xmm2, %xmm2
cvtss2sd %xmm3, %xmm2
movsd 0x534e(%rip), %xmm0     # 0xb248
subsd %xmm2, %xmm0
xorps %xmm2, %xmm2
cvtss2sd %xmm4, %xmm2
movss 0x4(%rsp), %xmm5
addss 0x20(%rsp), %xmm5
jbe 0x5f87
addsd %xmm2, %xmm0
subsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x5fff
sqrtsd %xmm0, %xmm0
jmp 0x6010
callq 0x2170
movaps 0x50(%rsp), %xmm1
movaps 0x40(%rsp), %xmm3
movss 0x4(%rsp), %xmm4
addsd %xmm0, %xmm0
xorps %xmm2, %xmm2
cvtsd2ss %xmm0, %xmm2
movss 0x5476(%rip), %xmm0     # 0xb3d0
mulss %xmm2, %xmm0
movaps 0x20(%rsp), %xmm5
subss %xmm4, %xmm5
movaps %xmm5, %xmm4
divss %xmm2, %xmm4
subss 0x30(%rsp), %xmm1
divss %xmm2, %xmm1
subss 0x14(%rsp), %xmm3
divss %xmm2, %xmm3
jmp 0x60a8
subsd %xmm2, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x6055
sqrtsd %xmm0, %xmm0
jmp 0x6066
callq 0x2170
movaps 0x50(%rsp), %xmm3
movss 0x4(%rsp), %xmm4
addsd %xmm0, %xmm0
xorps %xmm1, %xmm1
cvtsd2ss %xmm0, %xmm1
movss 0x5407(%rip), %xmm2     # 0xb3d0
mulss %xmm1, %xmm2
movaps 0x20(%rsp), %xmm0
subss %xmm4, %xmm0
divss %xmm1, %xmm0
addss 0x30(%rsp), %xmm3
divss %xmm1, %xmm3
movss 0x14(%rsp), %xmm4
addss 0x40(%rsp), %xmm4
divss %xmm1, %xmm4
movaps %xmm4, %xmm1
movaps %xmm2, %xmm4
jmp 0x60a8
movss %xmm5, 0x4(%rsp)
callq 0x2170
movss 0x4(%rsp), %xmm5
movaps 0x50(%rsp), %xmm3
addsd %xmm0, %xmm0
xorps %xmm1, %xmm1
cvtsd2ss %xmm0, %xmm1
movss 0x53a8(%rip), %xmm2     # 0xb3d0
mulss %xmm1, %xmm2
divss %xmm1, %xmm5
subss 0x30(%rsp), %xmm3
movaps %xmm3, %xmm0
divss %xmm1, %xmm0
movss 0x14(%rsp), %xmm4
addss 0x40(%rsp), %xmm4
divss %xmm1, %xmm4
movaps %xmm5, %xmm3
movaps %xmm2, %xmm1
jmp 0x60a8
movss %xmm5, 0x4(%rsp)
callq 0x2170
movss 0x4(%rsp), %xmm5
movaps 0x50(%rsp), %xmm3
addsd %xmm0, %xmm0
xorps %xmm1, %xmm1
cvtsd2ss %xmm0, %xmm1
movss 0x5352(%rip), %xmm2     # 0xb3d0
mulss %xmm1, %xmm2
divss %xmm1, %xmm5
addss 0x30(%rsp), %xmm3
movaps %xmm3, %xmm4
divss %xmm1, %xmm4
movaps 0x40(%rsp), %xmm0
subss 0x14(%rsp), %xmm0
divss %xmm1, %xmm0
movaps %xmm2, %xmm3
movaps %xmm5, %xmm1
unpcklps %xmm3, %xmm1            # xmm1 = xmm1[0],xmm3[0],xmm1[1],xmm3[1]
unpcklps %xmm4, %xmm0            # xmm0 = xmm0[0],xmm4[0],xmm0[1],xmm4[1]
addq $0x68, %rsp
retq
nop