// File: /workspace/github_cpp_makefile_star2_O2/jingdao[P]PointCloudApp/pcd.cpp
// Function: PCD::get3DProjection(unsigned int*, int, int, int, bool)
int PCD::get3DProjection(unsigned int* pixels, int screenWidth, int screenHeight, int pointSize, bool zoomToFit) {
	int i, j, k;
	Quaternion q, newPoint;
	std::vector<float> x_data;
	std::vector<float> y_data;
	std::vector<unsigned int> rgb_data;
	q.r = 0;
	printf("Using camera: (%f %f %f %f %f %f %f)\n", camera.position.i, camera.position.j, camera.position.k,
		camera.rotation.r, camera.rotation.i, camera.rotation.j, camera.rotation.k);
	for (i = 0; i < numPoints; i++) {
		//first transform point cloud to camera coordinates
		q.i = float_data[i * 4] - camera.position.i;
		q.j = float_data[i * 4 + 1] - camera.position.j;
		q.k = float_data[i * 4 + 2] - camera.position.k;
		newPoint = quaternionMult(quaternionMult(camera.rotation, q), quaternionInv(camera.rotation));
		//next map points to image plane based on camera focal length
		if ( - newPoint.k > camera.focal_length) { //if not behind camera
			x_data.push_back(newPoint.i * camera.focal_length / - newPoint.k);
			y_data.push_back(newPoint.j * camera.focal_length / - newPoint.k);
			rgb_data.push_back((unsigned int) float_data[i * 4 + 3]);
		}
	}
	//lastly plot these points on xy plane of bitmap
	int pixelsDrawn = 0;
	float maxX = -1;
	float maxY = -1;
	float x, y, scaleX, scaleY;
	float margin = 0.05f;
	if (zoomToFit){
		for (size_t n = 0; n < x_data.size(); n++) {
			if (fabs(x_data[n]) > maxX) maxX = fabs(x_data[n]);
			if (fabs(y_data[n]) > maxY) maxY = fabs(y_data[n]);
		}
	} else {
		maxX = 1;
		maxY = 1;
	}
//	printf("display rectangle: %f x %f\n", maxX, maxY);
	if (maxX > maxY) {
		scaleX = (screenWidth - 1) * (1 - margin * 2) / (maxX * 2);
		scaleY = (screenHeight - 1) * (1 - margin * 2) / (maxX * 2);
	}
	else {
		scaleX = (screenWidth - 1) * (1 - margin * 2) / (maxY * 2);
		scaleY = (screenHeight - 1) * (1 - margin * 2) / (maxY * 2);
	}
	for (size_t n = 0; n < x_data.size(); n++) {
		x = x_data[n] * scaleX + screenWidth / 2;
		y = y_data[n] * scaleY + screenHeight / 2;
		x -= (pointSize - 1) / 2;
		y -= (pointSize - 1) / 2;
		for (j = 0; j < pointSize; j++) {
			for (k = 0; k < pointSize; k++) {
				if (x+j>=0 && x+j<screenWidth && y+k>=0 && y+k<screenHeight) {
					pixelsDrawn++;
					pixels[((int)y+k)*screenWidth + ((int)x+j)] = rgb_data[n];
				}
			}
		}
	}
	return pixelsDrawn;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xc8, %rsp
movl %r9d, 0xc(%rsp)
movq %r8, 0x50(%rsp)
movq %rcx, 0x58(%rsp)
movq %rdx, 0x60(%rsp)
movq %rsi, 0x68(%rsp)
movq %rdi, %r13
xorps %xmm0, %xmm0
leaq 0x30(%rsp), %rax
movaps %xmm0, (%rax)
andq $0x0, 0x10(%rax)
leaq 0x70(%rsp), %rax
andq $0x0, 0x10(%rax)
leaq 0x90(%rsp), %r14
andq $0x0, 0x10(%r14)
movq %rax, %rbx
movaps %xmm0, (%rax)
movaps %xmm0, (%r14)
cvtss2sd 0x20(%rdi), %xmm0
cvtss2sd 0x24(%rdi), %xmm1
cvtss2sd 0x28(%rdi), %xmm2
cvtss2sd 0x2c(%rdi), %xmm3
cvtss2sd 0x30(%rdi), %xmm4
cvtss2sd 0x34(%rdi), %xmm5
cvtss2sd 0x38(%rdi), %xmm6
leaq 0x4d0a(%rip), %rdi      # 0xa8d6
movb $0x7, %al
callq 0x2050
xorps %xmm0, %xmm0
xorl %ebp, %ebp
leaq 0x8(%rsp), %r15
xorl %r12d, %r12d
movslq 0x60(%r13), %rax
cmpq %rax, %r12
jge 0x5cf1
movq 0x58(%r13), %rax
movss (%rax,%rbp), %xmm2
subss 0x20(%r13), %xmm2
movlhps %xmm0, %xmm2            # xmm2 = xmm2[0],xmm0[0]
shufps $0xe2, %xmm0, %xmm2     # xmm2 = xmm2[2,0],xmm0[2,3]
movsd 0x4(%rax,%rbp), %xmm3
movsd 0x24(%r13), %xmm0
subps %xmm0, %xmm3
movsd 0x2c(%r13), %xmm0
movaps %xmm0, 0x10(%rsp)
movsd 0x34(%r13), %xmm1
movaps %xmm1, 0x20(%rsp)
movaps %xmm2, 0xb0(%rsp)
callq 0x595e
movaps 0x10(%rsp), %xmm4
movaps %xmm4, %xmm2
movaps 0x477c(%rip), %xmm3     # 0xa3c0
xorps %xmm3, %xmm2
shufps $0x1, %xmm4, %xmm2      # xmm2 = xmm2[1,0],xmm4[0,0]
shufps $0xe2, %xmm4, %xmm2     # xmm2 = xmm2[2,0],xmm4[2,3]
movaps 0x20(%rsp), %xmm4
xorps %xmm3, %xmm4
movaps %xmm4, %xmm3
callq 0x595e
movaps %xmm1, %xmm3
shufps $0x55, %xmm1, %xmm3     # xmm3 = xmm3[1,1],xmm1[1,1]
xorps 0x4753(%rip), %xmm3     # 0xa3c0
movss 0x3c(%r13), %xmm2
ucomiss %xmm2, %xmm3
jbe 0x5cdd
movaps %xmm1, 0x20(%rsp)
shufps $0x55, %xmm0, %xmm0     # xmm0 = xmm0[1,1,1,1]
mulss %xmm0, %xmm2
movaps %xmm3, 0x10(%rsp)
divss %xmm3, %xmm2
movss %xmm2, 0x8(%rsp)
leaq 0x30(%rsp), %rdi
movq %r15, %rsi
callq 0x6f2a
movaps 0x20(%rsp), %xmm0
mulss 0x3c(%r13), %xmm0
divss 0x10(%rsp), %xmm0
movss %xmm0, 0x8(%rsp)
movq %rbx, %rdi
movq %r15, %rsi
callq 0x6f2a
movq 0x58(%r13), %rax
cvttss2si 0xc(%rax,%rbp), %rax
movl %eax, 0x8(%rsp)
movq %r14, %rdi
movq %r15, %rsi
callq 0x7196
incq %r12
addq $0x10, %rbp
movaps 0xb0(%rsp), %xmm0
jmp 0x5be0
movq 0x30(%rsp), %rcx
movq 0x38(%rsp), %rsi
cmpb $0x0, 0xc(%rsp)
je 0x5d3e
subq %rcx, %rsi
sarq $0x2, %rsi
movaps 0x4720(%rip), %xmm0     # 0xa430
movq 0x70(%rsp), %rdi
xorl %eax, %eax
movaps 0x4722(%rip), %xmm1     # 0xa440
cmpq %rax, %rsi
je 0x5d54
movss (%rcx,%rax,4), %xmm2
movss (%rdi,%rax,4), %xmm3
unpcklps %xmm2, %xmm3            # xmm3 = xmm3[0],xmm2[0],xmm3[1],xmm2[1]
andps %xmm1, %xmm3
maxps %xmm0, %xmm3
incq %rax
movaps %xmm3, %xmm0
jmp 0x5d1e
movq 0x70(%rsp), %rdi
subq %rcx, %rsi
sarq $0x2, %rsi
movss 0x470a(%rip), %xmm7     # 0xa45c
jmp 0x5d63
movaps %xmm0, %xmm7
shufps $0x55, %xmm0, %xmm7     # xmm7 = xmm7[1,1],xmm0[1,1]
ucomiss %xmm0, %xmm7
ja 0x5d63
movaps %xmm0, %xmm7
movq 0x60(%rsp), %r9
leal -0x1(%r9), %eax
cvtsi2ss %eax, %xmm0
movss 0x46fc(%rip), %xmm6     # 0xa474
mulss %xmm6, %xmm0
movq 0x58(%rsp), %r10
leal -0x1(%r10), %eax
cvtsi2ss %eax, %xmm1
pushq $0x2
popq %r8
movl %r9d, %eax
cltd
idivl %r8d
cvtsi2ss %eax, %xmm2
addss %xmm7, %xmm7
movl %r10d, %eax
cltd
idivl %r8d
cvtsi2ss %eax, %xmm3
movq 0x50(%rsp), %r11
leal -0x1(%r11), %eax
cltd
idivl %r8d
cvtsi2ss %eax, %xmm4
divss %xmm7, %xmm0
cvtsi2ss %r9d, %xmm5
mulss %xmm6, %xmm1
cvtsi2ss %r10d, %xmm6
divss %xmm7, %xmm1
movq 0x90(%rsp), %rax
movslq %r9d, %rdx
xorl %r15d, %r15d
testl %r11d, %r11d
movl $0x0, %r8d
cmovgl %r11d, %r8d
movl %r11d, %r9d
shlq $0x2, %rdx
xorps %xmm7, %xmm7
xorl %r10d, %r10d
cmpq %rsi, %r10
je 0x5eba
movss (%rcx,%r10,4), %xmm8
mulss %xmm0, %xmm8
addss %xmm2, %xmm8
movss (%rdi,%r10,4), %xmm9
mulss %xmm1, %xmm9
addss %xmm3, %xmm9
subss %xmm4, %xmm8
subss %xmm4, %xmm9
cvttss2si %xmm9, %r11d
cvttss2si %xmm8, %ebp
movslq %r11d, %r11
movslq %ebp, %r14
imulq %rdx, %r11
leaq (%r11,%r14,4), %r11
addq 0x68(%rsp), %r11
xorl %r14d, %r14d
cmpq %r8, %r14
je 0x5eb2
cvtsi2ss %r14d, %xmm10
addss %xmm8, %xmm10
xorps %xmm11, %xmm11
cmpleps %xmm10, %xmm11
cmpltps %xmm5, %xmm10
andps %xmm11, %xmm10
movd %xmm10, %ebp
movq %r11, %r12
xorl %r13d, %r13d
cmpq %r13, %r9
je 0x5ea9
testb $0x1, %bpl
je 0x5ea1
cvtsi2ss %r13d, %xmm10
addss %xmm9, %xmm10
ucomiss %xmm7, %xmm10
jb 0x5ea1
ucomiss %xmm10, %xmm6
jbe 0x5ea1
incl %r15d
movl (%rax,%r10,4), %ebx
movl %ebx, (%r12)
incq %r13
addq %rdx, %r12
jmp 0x5e75
incq %r14
addq $0x4, %r11
jmp 0x5e49
incq %r10
jmp 0x5df6
leaq 0x90(%rsp), %rdi
callq 0x7162
leaq 0x70(%rsp), %rdi
callq 0x6db0
leaq 0x30(%rsp), %rdi
callq 0x6db0
movl %r15d, %eax
addq $0xc8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x5ef4
jmp 0x5ef4
movq %rax, %rbx
leaq 0x90(%rsp), %rdi
callq 0x7162
leaq 0x70(%rsp), %rdi
callq 0x6db0
leaq 0x30(%rsp), %rdi
callq 0x6db0
movq %rbx, %rdi
callq 0x23b0