// File: /workspace/github_cpp_makefile_star2_O3/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 $0x128, %rsp            # imm = 0x128
movl %r9d, 0x48(%rsp)
movl %r8d, %ebx
movq %rcx, 0x70(%rsp)
movl %edx, %r14d
movq %rsi, 0x78(%rsp)
movq %rdi, %r13
xorps %xmm0, %xmm0
movaps %xmm0, 0x30(%rsp)
xorl %eax, %eax
movq %rax, 0x40(%rsp)
movq %rax, 0x20(%rsp)
movaps %xmm0, 0x10(%rsp)
movq %rax, 0x60(%rsp)
movaps %xmm0, 0x50(%rsp)
xorps %xmm0, %xmm0
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 0x53bd(%rip), %rdi      # 0xb92e
movb $0x7, %al
callq 0x2050
xorps %xmm11, %xmm11
movl 0x60(%r13), %eax
testl %eax, %eax
jle 0x688a
xorl %r12d, %r12d
leaq 0xc(%rsp), %rbp
xorl %r15d, %r15d
movq 0x58(%r13), %rcx
movss (%rcx,%r12), %xmm3
movss 0x4(%rcx,%r12), %xmm4
subss 0x20(%r13), %xmm3
subss 0x24(%r13), %xmm4
movss 0x8(%rcx,%r12), %xmm0
subss 0x28(%r13), %xmm0
movsd 0x2c(%r13), %xmm5
movsd 0x34(%r13), %xmm7
movaps %xmm5, %xmm6
shufps $0x55, %xmm5, %xmm6     # xmm6 = xmm6[1,1],xmm5[1,1]
movaps %xmm3, %xmm1
mulss %xmm6, %xmm1
movaps %xmm5, %xmm8
mulss %xmm11, %xmm8
subss %xmm1, %xmm8
movaps %xmm7, %xmm1
mulss %xmm4, %xmm1
subss %xmm1, %xmm8
movaps %xmm7, %xmm10
shufps $0x55, %xmm7, %xmm10    # xmm10 = xmm10[1,1],xmm7[1,1]
movaps %xmm10, %xmm1
mulss %xmm0, %xmm1
subss %xmm1, %xmm8
movaps %xmm11, %xmm1
mulss %xmm6, %xmm1
movaps %xmm5, %xmm2
mulss %xmm3, %xmm2
addss %xmm1, %xmm2
movaps %xmm7, %xmm9
mulss %xmm0, %xmm9
addss %xmm2, %xmm9
movaps %xmm10, %xmm1
mulss %xmm4, %xmm1
subss %xmm1, %xmm9
movaps %xmm11, %xmm1
mulss %xmm7, %xmm1
movaps %xmm5, %xmm2
mulss %xmm4, %xmm2
addss %xmm1, %xmm2
movaps %xmm10, %xmm12
mulss %xmm3, %xmm12
addss %xmm2, %xmm12
movaps %xmm6, %xmm1
mulss %xmm0, %xmm1
subss %xmm1, %xmm12
movaps %xmm11, 0x110(%rsp)
movaps %xmm11, %xmm1
mulss %xmm10, %xmm1
mulss %xmm5, %xmm0
addss %xmm1, %xmm0
mulss %xmm6, %xmm4
addss %xmm0, %xmm4
movaps %xmm7, %xmm0
movaps %xmm3, 0x100(%rsp)
mulss %xmm3, %xmm0
subss %xmm0, %xmm4
movaps %xmm5, %xmm0
mulss %xmm4, %xmm0
movaps %xmm8, %xmm1
mulss %xmm10, %xmm1
subss %xmm1, %xmm0
movaps %xmm9, %xmm1
mulss %xmm7, %xmm1
subss %xmm1, %xmm0
movaps %xmm12, %xmm11
mulss %xmm6, %xmm11
addss %xmm0, %xmm11
xorps 0x4d33(%rip), %xmm11    # 0xb400
movss 0x3c(%r13), %xmm0
ucomiss %xmm0, %xmm11
jbe 0x685c
movaps %xmm6, 0xe0(%rsp)
movaps %xmm6, %xmm1
movaps 0x4d11(%rip), %xmm3     # 0xb400
xorps %xmm3, %xmm1
movaps %xmm10, 0xa0(%rsp)
movaps %xmm10, %xmm2
xorps %xmm3, %xmm2
movaps %xmm5, 0xf0(%rsp)
movaps %xmm5, %xmm3
movaps %xmm9, 0xb0(%rsp)
mulss %xmm9, %xmm3
movaps %xmm8, 0xc0(%rsp)
mulss %xmm8, %xmm1
addss %xmm3, %xmm1
movaps %xmm12, 0x90(%rsp)
mulss %xmm12, %xmm2
addss %xmm1, %xmm2
movss %xmm4, 0x4c(%rsp)
movaps %xmm4, %xmm1
movaps %xmm7, 0xd0(%rsp)
mulss %xmm7, %xmm1
addss %xmm2, %xmm1
mulss %xmm1, %xmm0
movaps %xmm11, 0x80(%rsp)
divss %xmm11, %xmm0
movss %xmm0, 0xc(%rsp)
movq 0x38(%rsp), %rsi
cmpq 0x40(%rsp), %rsi
je 0x678b
movss %xmm0, (%rsi)
addq $0x4, %rsi
movq %rsi, 0x38(%rsp)
jmp 0x6798
leaq 0x30(%rsp), %rdi
movq %rbp, %rdx
callq 0x7dd6
movaps 0xf0(%rsp), %xmm1
mulss 0x90(%rsp), %xmm1
movaps 0xc0(%rsp), %xmm0
mulss 0xd0(%rsp), %xmm0
subss %xmm0, %xmm1
movss 0x4c(%rsp), %xmm0
mulss 0xe0(%rsp), %xmm0
subss %xmm0, %xmm1
movaps 0xb0(%rsp), %xmm0
mulss 0xa0(%rsp), %xmm0
addss %xmm1, %xmm0
mulss 0x3c(%r13), %xmm0
divss 0x80(%rsp), %xmm0
movss %xmm0, 0xc(%rsp)
movq 0x18(%rsp), %rsi
cmpq 0x20(%rsp), %rsi
je 0x6816
movss %xmm0, (%rsi)
addq $0x4, %rsi
movq %rsi, 0x18(%rsp)
jmp 0x6823
leaq 0x10(%rsp), %rdi
movq %rbp, %rdx
callq 0x7dd6
movq 0x58(%r13), %rax
cvttss2si 0xc(%rax,%r12), %rax
movl %eax, 0xc(%rsp)
movq 0x58(%rsp), %rsi
cmpq 0x60(%rsp), %rsi
je 0x684b
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0x58(%rsp)
jmp 0x6858
leaq 0x50(%rsp), %rdi
movq %rbp, %rdx
callq 0x7fd6
movl 0x60(%r13), %eax
movaps 0x110(%rsp), %xmm0
movaps 0x100(%rsp), %xmm1
movlhps %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
shufps $0xe2, %xmm0, %xmm1     # xmm1 = xmm1[2,0],xmm0[2,3]
incq %r15
movslq %eax, %rcx
addq $0x10, %r12
movaps %xmm1, %xmm11
cmpq %rcx, %r15
jl 0x6593
movq 0x30(%rsp), %rax
movq 0x38(%rsp), %rdx
movq %rdx, %rcx
subq %rax, %rcx
sarq $0x2, %rcx
cmpb $0x0, 0x48(%rsp)
je 0x68fc
cmpq %rax, %rdx
je 0x6906
cmpq $0x1, %rcx
movq %rcx, %rsi
adcq $0x0, %rsi
movq 0x10(%rsp), %rdi
movaps 0x4bbf(%rip), %xmm0     # 0xb480
xorl %r8d, %r8d
movaps 0x4bc5(%rip), %xmm1     # 0xb490
movaps %xmm0, %xmm2
movss (%rax,%r8,4), %xmm3
movss (%rdi,%r8,4), %xmm0
unpcklps %xmm3, %xmm0            # xmm0 = xmm0[0],xmm3[0],xmm0[1],xmm3[1]
andps %xmm1, %xmm0
maxps %xmm2, %xmm0
incq %r8
cmpq %r8, %rsi
jne 0x68cb
movaps %xmm0, %xmm4
shufps $0x55, %xmm0, %xmm4     # xmm4 = xmm4[1,1],xmm0[1,1]
ucomiss %xmm0, %xmm4
ja 0x690e
movaps %xmm0, %xmm4
jmp 0x690e
movss 0x4ba8(%rip), %xmm4     # 0xb4ac
jmp 0x690e
movss 0x4bba(%rip), %xmm4     # 0xb4c8
movq 0x50(%rsp), %rdi
cmpq %rax, %rdx
je 0x6a81
leal -0x1(%r14), %edx
xorps %xmm0, %xmm0
cvtsi2ss %edx, %xmm0
movss 0x4b9d(%rip), %xmm3     # 0xb4cc
mulss %xmm3, %xmm0
addss %xmm4, %xmm4
movq 0x70(%rsp), %rsi
leal -0x1(%rsi), %edx
xorps %xmm1, %xmm1
cvtsi2ss %edx, %xmm1
divss %xmm4, %xmm0
movl %r14d, %edx
shrl $0x1f, %edx
addl %r14d, %edx
sarl %edx
xorps %xmm2, %xmm2
cvtsi2ss %edx, %xmm2
mulss %xmm3, %xmm1
movl %esi, %edx
shrl $0x1f, %edx
addl %esi, %edx
sarl %edx
xorps %xmm3, %xmm3
cvtsi2ss %edx, %xmm3
divss %xmm4, %xmm1
leal -0x1(%rbx), %edx
shrl $0x1f, %edx
addl %ebx, %edx
decl %edx
sarl %edx
xorps %xmm4, %xmm4
cvtsi2ss %edx, %xmm4
movq 0x10(%rsp), %rdx
xorps %xmm5, %xmm5
cvtsi2ss %r14d, %xmm5
xorps %xmm6, %xmm6
cvtsi2ss %esi, %xmm6
movslq %r14d, %rsi
cmpq $0x1, %rcx
adcq $0x0, %rcx
movl %ebx, %r8d
shlq $0x2, %rsi
xorl %r9d, %r9d
xorps %xmm7, %xmm7
xorl %ebp, %ebp
testl %ebx, %ebx
jle 0x6a73
movss (%rax,%r9,4), %xmm8
mulss %xmm0, %xmm8
addss %xmm2, %xmm8
movss (%rdx,%r9,4), %xmm9
mulss %xmm1, %xmm9
addss %xmm3, %xmm9
subss %xmm4, %xmm8
subss %xmm4, %xmm9
cvttss2si %xmm9, %r10d
cvttss2si %xmm8, %r11d
movslq %r10d, %r10
movslq %r11d, %r11
imulq %rsi, %r10
leaq (%r10,%r11,4), %r10
addq 0x78(%rsp), %r10
xorl %r11d, %r11d
xorps %xmm10, %xmm10
cvtsi2ss %r11d, %xmm10
addss %xmm8, %xmm10
xorps %xmm11, %xmm11
cmpleps %xmm10, %xmm11
cmpltps %xmm5, %xmm10
andps %xmm11, %xmm10
movd %xmm10, %r15d
movq %r10, %r12
xorl %r13d, %r13d
testb $0x1, %r15b
je 0x6a5c
xorps %xmm10, %xmm10
cvtsi2ss %r13d, %xmm10
addss %xmm9, %xmm10
ucomiss %xmm7, %xmm10
jb 0x6a5c
ucomiss %xmm10, %xmm6
jbe 0x6a5c
incl %ebp
movl (%rdi,%r9,4), %r14d
movl %r14d, (%r12)
incq %r13
addq %rsi, %r12
cmpq %r13, %r8
jne 0x6a32
incq %r11
addq $0x4, %r10
cmpq %r8, %r11
jne 0x6a07
incq %r9
cmpq %rcx, %r9
jne 0x69b5
jmp 0x6a83
xorl %ebp, %ebp
testq %rdi, %rdi
je 0x6a95
movq 0x60(%rsp), %rsi
subq %rdi, %rsi
callq 0x2210
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x6aac
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x2210
movq 0x30(%rsp), %rdi
testq %rdi, %rdi
je 0x6ac3
movq 0x40(%rsp), %rsi
subq %rdi, %rsi
callq 0x2210
movl %ebp, %eax
addq $0x128, %rsp            # imm = 0x128
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x6adb
jmp 0x6adb
movq %rax, %rbx
movq 0x50(%rsp), %rdi
testq %rdi, %rdi
je 0x6af5
movq 0x60(%rsp), %rsi
subq %rdi, %rsi
callq 0x2210
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x6b0c
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x2210
movq 0x30(%rsp), %rdi
testq %rdi, %rdi
je 0x6b23
movq 0x40(%rsp), %rsi
subq %rdi, %rsi
callq 0x2210
movq %rbx, %rdi
callq 0x23a0
nop