// File: 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:
pushq %rbp
movq %rsp, %rbp
subq $0xe0, %rsp
movq %rdi, -0x18(%rbp)
movss %xmm0, -0x1c(%rbp)
movss %xmm1, -0x20(%rbp)
movss %xmm2, -0x24(%rbp)
movss -0x24(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0xb2f9(%rip), %xmm1     # 0x132d8
mulsd %xmm1, %xmm0
movsd 0xb2e5(%rip), %xmm1     # 0x132d0
divsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x38(%rbp)
movss -0x20(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0xb2cf(%rip), %xmm1     # 0x132d8
mulsd %xmm1, %xmm0
movsd 0xb2bb(%rip), %xmm1     # 0x132d0
divsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x3c(%rbp)
movss -0x1c(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0xb2a5(%rip), %xmm1     # 0x132d8
mulsd %xmm1, %xmm0
movsd 0xb291(%rip), %xmm1     # 0x132d0
divsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x40(%rbp)
movss -0x3c(%rbp), %xmm0
callq 0xace0
movss %xmm0, -0xd8(%rbp)
movss -0x40(%rbp), %xmm0
callq 0xace0
movaps %xmm0, %xmm1
movss -0xd8(%rbp), %xmm0
mulss %xmm1, %xmm0
movss %xmm0, -0x70(%rbp)
movss -0x3c(%rbp), %xmm0
callq 0xace0
movd %xmm0, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm0
movss %xmm0, -0xd4(%rbp)
movss -0x40(%rbp), %xmm0
callq 0xad00
movaps %xmm0, %xmm1
movss -0xd4(%rbp), %xmm0
mulss %xmm1, %xmm0
movss %xmm0, -0x6c(%rbp)
movss -0x3c(%rbp), %xmm0
callq 0xad00
movss %xmm0, -0x68(%rbp)
movss -0x38(%rbp), %xmm0
callq 0xace0
movss %xmm0, -0xc4(%rbp)
movss -0x40(%rbp), %xmm0
callq 0xad00
movss %xmm0, -0xc8(%rbp)
movss -0x40(%rbp), %xmm0
callq 0xace0
movss %xmm0, -0xd0(%rbp)
movss -0x38(%rbp), %xmm0
callq 0xad00
movaps %xmm0, %xmm1
movss -0xd0(%rbp), %xmm0
mulss %xmm1, %xmm0
movss %xmm0, -0xcc(%rbp)
movss -0x3c(%rbp), %xmm0
callq 0xad00
movss -0xcc(%rbp), %xmm1
movss -0xc8(%rbp), %xmm2
movaps %xmm0, %xmm3
movss -0xc4(%rbp), %xmm0
mulss %xmm3, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, -0x64(%rbp)
movss -0x38(%rbp), %xmm0
callq 0xace0
movss %xmm0, -0xb4(%rbp)
movss -0x40(%rbp), %xmm0
callq 0xace0
movss %xmm0, -0xb8(%rbp)
movss -0x38(%rbp), %xmm0
callq 0xad00
movss %xmm0, -0xc0(%rbp)
movss -0x3c(%rbp), %xmm0
callq 0xad00
movaps %xmm0, %xmm1
movss -0xc0(%rbp), %xmm0
mulss %xmm1, %xmm0
movss %xmm0, -0xbc(%rbp)
movss -0x40(%rbp), %xmm0
callq 0xad00
movss -0xbc(%rbp), %xmm1
movss -0xb8(%rbp), %xmm2
movaps %xmm0, %xmm3
movss -0xb4(%rbp), %xmm0
mulss %xmm3, %xmm1
movd %xmm1, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, -0x60(%rbp)
movss -0x3c(%rbp), %xmm0
callq 0xace0
movd %xmm0, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm0
movss %xmm0, -0xb0(%rbp)
movss -0x38(%rbp), %xmm0
callq 0xad00
movaps %xmm0, %xmm1
movss -0xb0(%rbp), %xmm0
mulss %xmm1, %xmm0
movss %xmm0, -0x5c(%rbp)
movss -0x38(%rbp), %xmm0
callq 0xad00
movss %xmm0, -0xa0(%rbp)
movss -0x40(%rbp), %xmm0
callq 0xad00
movss %xmm0, -0xa4(%rbp)
movss -0x38(%rbp), %xmm0
callq 0xace0
movss %xmm0, -0xac(%rbp)
movss -0x40(%rbp), %xmm0
callq 0xace0
movaps %xmm0, %xmm1
movss -0xac(%rbp), %xmm0
mulss %xmm1, %xmm0
movss %xmm0, -0xa8(%rbp)
movss -0x3c(%rbp), %xmm0
callq 0xad00
movss -0xa8(%rbp), %xmm1
movss -0xa4(%rbp), %xmm2
movaps %xmm0, %xmm3
movss -0xa0(%rbp), %xmm0
mulss %xmm3, %xmm1
movd %xmm1, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, -0x58(%rbp)
movss -0x40(%rbp), %xmm0
callq 0xace0
movss %xmm0, -0x90(%rbp)
movss -0x38(%rbp), %xmm0
callq 0xad00
movss %xmm0, -0x94(%rbp)
movss -0x38(%rbp), %xmm0
callq 0xace0
movss %xmm0, -0x9c(%rbp)
movss -0x3c(%rbp), %xmm0
callq 0xad00
movaps %xmm0, %xmm1
movss -0x9c(%rbp), %xmm0
mulss %xmm1, %xmm0
movss %xmm0, -0x98(%rbp)
movss -0x40(%rbp), %xmm0
callq 0xad00
movss -0x98(%rbp), %xmm1
movss -0x94(%rbp), %xmm2
movaps %xmm0, %xmm3
movss -0x90(%rbp), %xmm0
mulss %xmm3, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, -0x54(%rbp)
movss -0x38(%rbp), %xmm0
callq 0xace0
movss %xmm0, -0x8c(%rbp)
movss -0x3c(%rbp), %xmm0
callq 0xace0
movaps %xmm0, %xmm1
movss -0x8c(%rbp), %xmm0
mulss %xmm1, %xmm0
movss %xmm0, -0x50(%rbp)
movss -0x70(%rbp), %xmm0
addss -0x60(%rbp), %xmm0
addss -0x50(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd %xmm0, -0x78(%rbp)
movsd -0x78(%rbp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x8433
movsd 0xaea3(%rip), %xmm0     # 0x13258
addsd -0x78(%rbp), %xmm0
callq 0x2170
movsd 0xaee9(%rip), %xmm1     # 0x132b0
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x7c(%rbp)
movss -0x7c(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd 0xaed3(%rip), %xmm0     # 0x132b8
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x28(%rbp)
movss -0x54(%rbp), %xmm0
subss -0x5c(%rbp), %xmm0
divss -0x7c(%rbp), %xmm0
movss %xmm0, -0x2c(%rbp)
movss -0x68(%rbp), %xmm0
subss -0x58(%rbp), %xmm0
divss -0x7c(%rbp), %xmm0
movss %xmm0, -0x30(%rbp)
movss -0x64(%rbp), %xmm0
subss -0x6c(%rbp), %xmm0
divss -0x7c(%rbp), %xmm0
movss %xmm0, -0x34(%rbp)
jmp 0x8675
movss -0x70(%rbp), %xmm0
ucomiss -0x60(%rbp), %xmm0
jbe 0x84f9
movss -0x70(%rbp), %xmm0
ucomiss -0x50(%rbp), %xmm0
jbe 0x84f9
movss -0x70(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd 0xadf6(%rip), %xmm0     # 0x13258
addsd %xmm1, %xmm0
movss -0x60(%rbp), %xmm1
cvtss2sd %xmm1, %xmm1
subsd %xmm1, %xmm0
movss -0x50(%rbp), %xmm1
cvtss2sd %xmm1, %xmm1
subsd %xmm1, %xmm0
callq 0x2170
movsd 0xae23(%rip), %xmm1     # 0x132b0
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x80(%rbp)
movss -0x80(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd 0xae0d(%rip), %xmm0     # 0x132b8
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x2c(%rbp)
movss -0x54(%rbp), %xmm0
subss -0x5c(%rbp), %xmm0
divss -0x80(%rbp), %xmm0
movss %xmm0, -0x28(%rbp)
movss -0x68(%rbp), %xmm0
addss -0x58(%rbp), %xmm0
divss -0x80(%rbp), %xmm0
movss %xmm0, -0x34(%rbp)
movss -0x64(%rbp), %xmm0
addss -0x6c(%rbp), %xmm0
divss -0x80(%rbp), %xmm0
movss %xmm0, -0x30(%rbp)
jmp 0x8673
movss -0x60(%rbp), %xmm0
ucomiss -0x50(%rbp), %xmm0
jbe 0x85bf
movss -0x70(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd 0xad3f(%rip), %xmm0     # 0x13258
subsd %xmm1, %xmm0
movss -0x60(%rbp), %xmm1
cvtss2sd %xmm1, %xmm1
addsd %xmm1, %xmm0
movss -0x50(%rbp), %xmm1
cvtss2sd %xmm1, %xmm1
subsd %xmm1, %xmm0
callq 0x2170
movsd 0xad6c(%rip), %xmm1     # 0x132b0
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x84(%rbp)
movss -0x84(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd 0xad50(%rip), %xmm0     # 0x132b8
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x30(%rbp)
movss -0x54(%rbp), %xmm0
addss -0x5c(%rbp), %xmm0
divss -0x84(%rbp), %xmm0
movss %xmm0, -0x34(%rbp)
movss -0x68(%rbp), %xmm0
subss -0x58(%rbp), %xmm0
divss -0x84(%rbp), %xmm0
movss %xmm0, -0x28(%rbp)
movss -0x64(%rbp), %xmm0
addss -0x6c(%rbp), %xmm0
divss -0x84(%rbp), %xmm0
movss %xmm0, -0x2c(%rbp)
jmp 0x8671
movss -0x70(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd 0xac88(%rip), %xmm0     # 0x13258
subsd %xmm1, %xmm0
movss -0x60(%rbp), %xmm1
cvtss2sd %xmm1, %xmm1
subsd %xmm1, %xmm0
movss -0x50(%rbp), %xmm1
cvtss2sd %xmm1, %xmm1
addsd %xmm1, %xmm0
callq 0x2170
movsd 0xacb5(%rip), %xmm1     # 0x132b0
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x88(%rbp)
movss -0x88(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd 0xac99(%rip), %xmm0     # 0x132b8
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x34(%rbp)
movss -0x54(%rbp), %xmm0
addss -0x5c(%rbp), %xmm0
divss -0x88(%rbp), %xmm0
movss %xmm0, -0x30(%rbp)
movss -0x68(%rbp), %xmm0
addss -0x58(%rbp), %xmm0
divss -0x88(%rbp), %xmm0
movss %xmm0, -0x2c(%rbp)
movss -0x64(%rbp), %xmm0
subss -0x6c(%rbp), %xmm0
divss -0x88(%rbp), %xmm0
movss %xmm0, -0x28(%rbp)
jmp 0x8673
jmp 0x8675
movss -0x28(%rbp), %xmm0
movss %xmm0, -0x10(%rbp)
movss -0x2c(%rbp), %xmm0
movss %xmm0, -0xc(%rbp)
movss -0x30(%rbp), %xmm0
movss %xmm0, -0x8(%rbp)
movss -0x34(%rbp), %xmm0
movss %xmm0, -0x4(%rbp)
movsd -0x10(%rbp), %xmm0
movsd -0x8(%rbp), %xmm1
addq $0xe0, %rsp
popq %rbp
retq