// File: /workspace/github_cpp_makefile_star2_O2/jingdao[P]PointCloudApp/pcd.cpp
// Function: PCD::LoadFromCameraParameter(char const*)
PCD* PCD::LoadFromCameraParameter(const char* dir) {

	DIR* d = opendir(dir);
	if (!d) {
		printf("Invalid directory %s\n",dir);
		return NULL;
	}
	struct dirent *dt;
	int numRead=0; 
	PCD* cloud = new PCD(0);
	char buffer[1024];

	int ndir = strlen(dir);
	strncpy(buffer,dir,ndir);
	char* buffer_c = buffer + ndir;
	*buffer_c++ = '/';

	double focal_length = 600, w = 640, h = 480;
//	double focal_length = 1801, w = 1824, h =2736;
	w = (1 - w) / 2;
	h = (1 - h) / 2;

	while ((dt = readdir(d))) {
		int l = strlen(dt->d_name);
		if (strncmp(dt->d_name + l - 4,".txt",4) == 0) {
			strcpy(buffer_c,dt->d_name);
			FILE* f = fopen(buffer,"r");
			if (f) {
				char line[256];
				double projection[12], *p = projection, translation[3];
				float x,y,z,tmp,rotation[9];

				fgets(line,255,f);
				if (strncmp(line,"CONTOUR",7)!=0) 
					break;
				while (fgets(line,255,f)) {
					if (sscanf(line,"%lf %lf %lf %lf",p,p+1,p+2,p+3) == 4) {
						p += 4;
					}
				}
				translation[2] = -projection[11];
				translation[0] = (projection[3] - translation[2] * w) / focal_length;
				translation[1] = (translation[2] * h - projection[7]) / focal_length;
				rotation[6] = -projection[8];
				rotation[7] = -projection[9];
				rotation[8] = -projection[10];
				rotation[0] = (projection[0] - rotation[6] * w) / focal_length;
				rotation[1] = (projection[1] - rotation[7] * w) / focal_length;
				rotation[2] = (projection[2] - rotation[8] * w) / focal_length;
				rotation[3] = (rotation[6] * h - projection[4]) / focal_length;
				rotation[4] = (rotation[7] * h - projection[5]) / focal_length;
				rotation[5] = (rotation[8] * h - projection[6]) / focal_length;
				//transpose R
				tmp = rotation[1]; rotation[1] = rotation[3]; rotation[3] = tmp;
				tmp = rotation[2]; rotation[2] = rotation[6]; rotation[6] = tmp;
				tmp = rotation[5]; rotation[5] = rotation[7]; rotation[7] = tmp;
				// C = - R' * T
				x = - (rotation[0]*translation[0]+rotation[1]*translation[1]+rotation[2]*translation[2]);
				y = - (rotation[3]*translation[0]+rotation[4]*translation[1]+rotation[5]*translation[2]);
				z = - (rotation[6]*translation[0]+rotation[7]*translation[1]+rotation[8]*translation[2]);
				cloud->drawAxis(x,y,z,rotation,3,1);
				numRead++;
				fclose(f);
			}
		}
	}
	printf("Loaded %d camera parameters\n",numRead);
	closedir(d);

	cloud->drawAxis(0,0,0,NULL,10,1);
	return cloud;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x5b8, %rsp            # imm = 0x5B8
movq %rdi, %rbx
callq 0x22e0
testq %rax, %rax
je 0x5046
movq %rax, %r14
pushq $0x78
popq %rdi
callq 0x2200
movq %rax, 0x10(%rsp)
movq %rax, %rdi
xorl %esi, %esi
callq 0x3330
movq %rbx, %rdi
callq 0x20c0
movslq %eax, %r15
leaq 0x1b0(%rsp), %rdi
movq %rbx, %rsi
movq %r15, %rdx
callq 0x20e0
leaq (%rsp,%r15), %r12
addq $0x1b1, %r12            # imm = 0x1B1
movb $0x2f, -0x1(%r12)
leaq 0x5bd1(%rip), %r13      # 0xa8d1
pushq $0x4
popq %rbp
xorl %ebx, %ebx
movl %ebx, 0x8(%rsp)
movq %r14, %rdi
callq 0x2300
testq %rax, %rax
je 0x5000
movq %rax, %r15
leaq 0x13(%rax), %rbx
movq %rbx, %rdi
callq 0x20c0
cltq
leaq (%r15,%rax), %rdi
addq $0xf, %rdi
movq %r13, %rsi
movq %rbp, %rdx
callq 0x20d0
testl %eax, %eax
jne 0x4d09
movq %r12, %rdi
movq %rbx, %rsi
callq 0x21c0
leaq 0x1b0(%rsp), %rdi
leaq 0x5ac0(%rip), %rsi      # 0xa81c
callq 0x2240
testq %rax, %rax
je 0x4d09
movq %rax, %r15
movq %r12, 0x18(%rsp)
leaq 0xb0(%rsp), %r12
movq %r12, %rdi
movl $0xff, %esi
movq %rax, %rdx
callq 0x2340
movq %r12, %rdi
leaq 0x5aed(%rip), %rsi      # 0xa87d
pushq $0x7
popq %rdx
callq 0x2220
testl %eax, %eax
jne 0x5000
movl %eax, 0xc(%rsp)
leaq 0x50(%rsp), %rbx
movq %r12, %rdi
movl $0xff, %esi
movq %r15, %rdx
callq 0x2340
testq %rax, %rax
je 0x4dee
leaq 0x8(%rbx), %rcx
leaq 0x10(%rbx), %r8
leaq 0x18(%rbx), %r9
movq %r12, %rdi
leaq 0x5ab1(%rip), %rsi      # 0xa885
movq %rbx, %rdx
xorl %eax, %eax
callq 0x21e0
xorl %ecx, %ecx
cmpl $0x4, %eax
sete %cl
shll $0x5, %ecx
addq %rcx, %rbx
jmp 0x4da9
cvtsd2ss 0x90(%rsp), %xmm3
cvtsd2ss 0x98(%rsp), %xmm6
movaps 0x55b8(%rip), %xmm12    # 0xa3c0
xorps %xmm12, %xmm3
cvtsd2ss 0xa0(%rsp), %xmm9
xorps %xmm12, %xmm6
movaps 0x50(%rsp), %xmm7
cvtss2sd %xmm3, %xmm0
movaps %xmm0, %xmm4
movaps 0x51e2(%rip), %xmm11    # 0xa010
xorps %xmm11, %xmm4
movups 0x58(%rsp), %xmm5
movaps 0x60(%rsp), %xmm1
movsd 0x70(%rsp), %xmm2
xorps %xmm11, %xmm2
movlhps %xmm2, %xmm7            # xmm7 = xmm7[0],xmm2[0]
movsd 0x78(%rsp), %xmm2
xorps %xmm11, %xmm2
movlhps %xmm2, %xmm5            # xmm5 = xmm5[0],xmm2[0]
movsd 0x80(%rsp), %xmm2
xorps %xmm11, %xmm2
movlhps %xmm2, %xmm1            # xmm1 = xmm1[0],xmm2[0]
cvtss2sd %xmm6, %xmm2
movlhps %xmm0, %xmm4            # xmm4 = xmm4[0],xmm0[0]
movapd 0x555a(%rip), %xmm13    # 0xa3d0
mulpd %xmm13, %xmm4
addpd %xmm7, %xmm4
movaps %xmm2, %xmm10
xorps %xmm11, %xmm10
movlhps %xmm2, %xmm10           # xmm10 = xmm10[0],xmm2[0]
mulpd %xmm13, %xmm10
addpd %xmm5, %xmm10
movsd 0xa8(%rsp), %xmm8
xorps %xmm12, %xmm9
movss %xmm9, 0x40(%rsp)
movapd 0x552d(%rip), %xmm14    # 0xa3e0
divpd %xmm14, %xmm4
cvtpd2ps %xmm4, %xmm7
movlpd %xmm7, 0x20(%rsp)
divpd %xmm14, %xmm10
cvtpd2ps %xmm10, %xmm5
movlpd %xmm5, 0x2c(%rsp)
movss %xmm3, 0x28(%rsp)
movapd %xmm8, %xmm4
cvtss2sd %xmm9, %xmm3
movss %xmm6, 0x34(%rsp)
movaps %xmm3, %xmm6
xorps %xmm11, %xmm6
movlhps %xmm3, %xmm6            # xmm6 = xmm6[0],xmm3[0]
mulsd %xmm8, %xmm0
mulsd %xmm8, %xmm2
mulsd %xmm8, %xmm3
xorpd %xmm11, %xmm8
mulsd 0x543a(%rip), %xmm4     # 0xa348
addsd 0x68(%rsp), %xmm4
movsd 0x53b3(%rip), %xmm9     # 0xa2d0
divsd %xmm9, %xmm4
mulsd 0x5425(%rip), %xmm8     # 0xa350
subsd 0x88(%rsp), %xmm8
divsd %xmm9, %xmm8
mulpd %xmm13, %xmm6
addpd %xmm1, %xmm6
cvtss2sd %xmm7, %xmm1
shufps $0x55, %xmm7, %xmm7     # xmm7 = xmm7[1,1,1,1]
cvtss2sd %xmm7, %xmm7
mulsd %xmm8, %xmm7
mulsd %xmm4, %xmm1
addsd %xmm7, %xmm1
subsd %xmm0, %xmm1
cvtss2sd %xmm5, %xmm7
shufps $0x55, %xmm5, %xmm5     # xmm5 = xmm5[1,1,1,1]
cvtss2sd %xmm5, %xmm0
mulsd %xmm8, %xmm0
mulsd %xmm4, %xmm7
addsd %xmm0, %xmm7
divpd %xmm14, %xmm6
cvtpd2ps %xmm6, %xmm0
movlpd %xmm0, 0x38(%rsp)
subsd %xmm2, %xmm7
cvtss2sd %xmm0, %xmm2
shufps $0x55, %xmm0, %xmm0     # xmm0 = xmm0[1,1,1,1]
cvtss2sd %xmm0, %xmm0
mulsd %xmm8, %xmm0
mulsd %xmm4, %xmm2
addsd %xmm0, %xmm2
cvtsd2ss %xmm1, %xmm0
subsd %xmm3, %xmm2
xorps %xmm12, %xmm0
cvtsd2ss %xmm7, %xmm1
xorps %xmm12, %xmm1
cvtsd2ss %xmm2, %xmm2
xorps %xmm12, %xmm2
movq 0x10(%rsp), %rdi
leaq 0x20(%rsp), %rsi
movss 0x549d(%rip), %xmm3     # 0xa470
movss 0x5481(%rip), %xmm4     # 0xa45c
callq 0x3532
movl 0x8(%rsp), %ebx
incl %ebx
movq %r15, %rdi
callq 0x21f0
cmpl $0x0, 0xc(%rsp)
movq 0x18(%rsp), %r12
je 0x4d05
jmp 0x5004
movl 0x8(%rsp), %ebx
leaq 0x588a(%rip), %rdi      # 0xa895
movl %ebx, %esi
xorl %eax, %eax
callq 0x2050
movq %r14, %rdi
callq 0x2360
movss 0x5448(%rip), %xmm3     # 0xa46c
movss 0x5430(%rip), %xmm4     # 0xa45c
xorps %xmm0, %xmm0
xorps %xmm1, %xmm1
xorps %xmm2, %xmm2
movq 0x10(%rsp), %r14
movq %r14, %rdi
xorl %esi, %esi
callq 0x3532
jmp 0x505a
leaq 0x581a(%rip), %rdi      # 0xa867
xorl %r14d, %r14d
movq %rbx, %rsi
xorl %eax, %eax
callq 0x2050
movq %r14, %rax
addq $0x5b8, %rsp            # imm = 0x5B8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop