// File: 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
movq %rsp, %rbp
subq $0x650, %rsp            # imm = 0x650
movq %rdi, -0x10(%rbp)
movq -0x10(%rbp), %rdi
callq 0x22b0
movq %rax, -0x18(%rbp)
cmpq $0x0, -0x18(%rbp)
jne 0x70f2
movq -0x10(%rbp), %rsi
leaq 0xc70f(%rip), %rdi      # 0x137ed
movb $0x0, %al
callq 0x2050
movq $0x0, -0x8(%rbp)
jmp 0x78f1
movl $0x0, -0x24(%rbp)
movl $0x78, %edi
callq 0x2200
movq %rax, %rdi
movq %rdi, %rax
movq %rax, -0x648(%rbp)
xorl %esi, %esi
callq 0x41e0
jmp 0x7119
movq -0x648(%rbp), %rax
movq %rax, -0x30(%rbp)
movq -0x10(%rbp), %rdi
callq 0x20c0
movl %eax, -0x444(%rbp)
leaq -0x440(%rbp), %rdi
movq -0x10(%rbp), %rsi
movslq -0x444(%rbp), %rdx
callq 0x20e0
leaq -0x440(%rbp), %rax
movslq -0x444(%rbp), %rcx
addq %rcx, %rax
movq %rax, -0x450(%rbp)
movq -0x450(%rbp), %rax
movq %rax, %rcx
addq $0x1, %rcx
movq %rcx, -0x450(%rbp)
movb $0x2f, (%rax)
movsd 0xc096(%rip), %xmm0     # 0x13218
movsd %xmm0, -0x458(%rbp)
movsd 0xc1c6(%rip), %xmm0     # 0x13358
movsd %xmm0, -0x460(%rbp)
movsd 0xc1ae(%rip), %xmm0     # 0x13350
movsd %xmm0, -0x468(%rbp)
movsd 0xc0a6(%rip), %xmm0     # 0x13258
subsd -0x460(%rbp), %xmm0
movsd 0xc0ee(%rip), %xmm1     # 0x132b0
divsd %xmm1, %xmm0
movsd %xmm0, -0x460(%rbp)
movsd 0xc082(%rip), %xmm0     # 0x13258
subsd -0x468(%rbp), %xmm0
movsd 0xc0ca(%rip), %xmm1     # 0x132b0
divsd %xmm1, %xmm0
movsd %xmm0, -0x468(%rbp)
movq -0x18(%rbp), %rdi
callq 0x22d0
movq %rax, -0x20(%rbp)
cmpq $0x0, %rax
je 0x78a9
movq -0x20(%rbp), %rdi
addq $0x13, %rdi
callq 0x20c0
movl %eax, -0x46c(%rbp)
movq -0x20(%rbp), %rdi
addq $0x13, %rdi
movslq -0x46c(%rbp), %rax
addq %rax, %rdi
addq $-0x4, %rdi
leaq 0xc61e(%rip), %rsi      # 0x13857
movl $0x4, %edx
callq 0x20d0
cmpl $0x0, %eax
jne 0x78a4
movq -0x450(%rbp), %rdi
movq -0x20(%rbp), %rsi
addq $0x13, %rsi
callq 0x21c0
leaq -0x440(%rbp), %rdi
leaq 0xc51e(%rip), %rsi      # 0x1378c
callq 0x2230
movq %rax, -0x478(%rbp)
cmpq $0x0, -0x478(%rbp)
je 0x78a2
leaq -0x5e0(%rbp), %rax
movq %rax, -0x5e8(%rbp)
leaq -0x580(%rbp), %rdi
movq -0x478(%rbp), %rdx
movl $0xff, %esi
callq 0x2300
leaq -0x580(%rbp), %rdi
leaq 0xc547(%rip), %rsi      # 0x13803
movl $0x7, %edx
callq 0x20d0
cmpl $0x0, %eax
je 0x72f2
jmp 0x78a9
movq -0x648(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x38(%rbp)
movl %eax, -0x3c(%rbp)
movl $0x78, %esi
callq 0x2210
jmp 0x78fe
jmp 0x72f4
leaq -0x580(%rbp), %rdi
movq -0x478(%rbp), %rdx
movl $0xff, %esi
callq 0x2300
cmpq $0x0, %rax
je 0x7368
leaq -0x580(%rbp), %rdi
movq -0x5e8(%rbp), %rdx
movq -0x5e8(%rbp), %rcx
addq $0x8, %rcx
movq -0x5e8(%rbp), %r8
addq $0x10, %r8
movq -0x5e8(%rbp), %r9
addq $0x18, %r9
leaq 0xc4c3(%rip), %rsi      # 0x1380b
movb $0x0, %al
callq 0x21e0
cmpl $0x4, %eax
jne 0x7366
movq -0x5e8(%rbp), %rax
addq $0x20, %rax
movq %rax, -0x5e8(%rbp)
jmp 0x72f4
movsd -0x588(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, -0x5f0(%rbp)
movsd -0x5c8(%rbp), %xmm1
movsd -0x5f0(%rbp), %xmm0
movsd -0x460(%rbp), %xmm2
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x458(%rbp), %xmm0
movsd %xmm0, -0x600(%rbp)
movsd -0x5f0(%rbp), %xmm0
movsd -0x468(%rbp), %xmm2
movsd -0x5a8(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x458(%rbp), %xmm0
movsd %xmm0, -0x5f8(%rbp)
movsd -0x5a0(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x628(%rbp)
movsd -0x598(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x624(%rbp)
movsd -0x590(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x620(%rbp)
movsd -0x5e0(%rbp), %xmm1
movss -0x628(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0x460(%rbp), %xmm2
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x458(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x640(%rbp)
movsd -0x5d8(%rbp), %xmm1
movss -0x624(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0x460(%rbp), %xmm2
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x458(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x63c(%rbp)
movsd -0x5d0(%rbp), %xmm1
movss -0x620(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0x460(%rbp), %xmm2
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x458(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x638(%rbp)
movss -0x628(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0x468(%rbp), %xmm2
movsd -0x5c0(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x458(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x634(%rbp)
movss -0x624(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0x468(%rbp), %xmm2
movsd -0x5b8(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x458(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x630(%rbp)
movss -0x620(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0x468(%rbp), %xmm2
movsd -0x5b0(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x458(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x62c(%rbp)
movss -0x63c(%rbp), %xmm0
movss %xmm0, -0x610(%rbp)
movss -0x634(%rbp), %xmm0
movss %xmm0, -0x63c(%rbp)
movss -0x610(%rbp), %xmm0
movss %xmm0, -0x634(%rbp)
movss -0x638(%rbp), %xmm0
movss %xmm0, -0x610(%rbp)
movss -0x628(%rbp), %xmm0
movss %xmm0, -0x638(%rbp)
movss -0x610(%rbp), %xmm0
movss %xmm0, -0x628(%rbp)
movss -0x62c(%rbp), %xmm0
movss %xmm0, -0x610(%rbp)
movss -0x624(%rbp), %xmm0
movss %xmm0, -0x62c(%rbp)
movss -0x610(%rbp), %xmm0
movss %xmm0, -0x624(%rbp)
movss -0x640(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd -0x600(%rbp), %xmm2
movss -0x63c(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
mulsd -0x5f8(%rbp), %xmm0
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movss -0x638(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0x5f0(%rbp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x604(%rbp)
movss -0x634(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd -0x600(%rbp), %xmm2
movss -0x630(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
mulsd -0x5f8(%rbp), %xmm0
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movss -0x62c(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0x5f0(%rbp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x608(%rbp)
movss -0x628(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd -0x600(%rbp), %xmm2
movss -0x624(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
mulsd -0x5f8(%rbp), %xmm0
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movss -0x620(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0x5f0(%rbp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, -0x60c(%rbp)
movq -0x30(%rbp), %rdi
movss -0x604(%rbp), %xmm0
movss -0x608(%rbp), %xmm1
movss -0x60c(%rbp), %xmm2
leaq -0x640(%rbp), %rsi
movss 0xbaf8(%rip), %xmm3     # 0x13378
movss 0xbad8(%rip), %xmm4     # 0x13360
callq 0x4640
movl -0x24(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x24(%rbp)
movq -0x478(%rbp), %rdi
callq 0x21f0
jmp 0x78a4
jmp 0x71f2
movl -0x24(%rbp), %esi
leaq 0xbf68(%rip), %rdi      # 0x1381b
movb $0x0, %al
callq 0x2050
movq -0x18(%rbp), %rdi
callq 0x2320
movq -0x30(%rbp), %rdi
xorps %xmm2, %xmm2
xorl %eax, %eax
movl %eax, %esi
movss 0xba9a(%rip), %xmm3     # 0x13370
movss 0xba82(%rip), %xmm4     # 0x13360
movaps %xmm2, %xmm0
movaps %xmm2, %xmm1
callq 0x4640
movq -0x30(%rbp), %rax
movq %rax, -0x8(%rbp)
movq -0x8(%rbp), %rax
addq $0x650, %rsp            # imm = 0x650
popq %rbp
retq
movq -0x38(%rbp), %rdi
callq 0x2380
nopw (%rax,%rax)