// File: 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
movq %rsp, %rbp
subq $0x180, %rsp            # imm = 0x180
movb %r9b, %al
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movl %edx, -0x14(%rbp)
movl %ecx, -0x18(%rbp)
movl %r8d, -0x1c(%rbp)
andb $0x1, %al
movb %al, -0x1d(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x158(%rbp)
leaq -0x68(%rbp), %rdi
callq 0xab10
leaq -0x80(%rbp), %rdi
callq 0xab10
leaq -0x98(%rbp), %rdi
callq 0xad20
movq -0x158(%rbp), %rax
movl $0x0, -0x3c(%rbp)
movss 0x20(%rax), %xmm0
movss 0x24(%rax), %xmm1
cvtss2sd %xmm0, %xmm0
cvtss2sd %xmm1, %xmm1
movss 0x28(%rax), %xmm2
cvtss2sd %xmm2, %xmm2
movss 0x2c(%rax), %xmm3
cvtss2sd %xmm3, %xmm3
movss 0x30(%rax), %xmm4
cvtss2sd %xmm4, %xmm4
movss 0x34(%rax), %xmm5
cvtss2sd %xmm5, %xmm5
movss 0x38(%rax), %xmm6
cvtss2sd %xmm6, %xmm6
leaq 0xabba(%rip), %rdi      # 0x1385c
movb $0x7, %al
callq 0x2050
jmp 0x8cab
movl $0x0, -0x24(%rbp)
movq -0x158(%rbp), %rcx
movl -0x24(%rbp), %eax
cmpl 0x60(%rcx), %eax
jge 0x8f29
movq -0x158(%rbp), %rdi
movq 0x58(%rdi), %rax
movl -0x24(%rbp), %ecx
shll $0x2, %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm0
movss 0x20(%rdi), %xmm1
subss %xmm1, %xmm0
movss %xmm0, -0x38(%rbp)
movq 0x58(%rdi), %rax
movl -0x24(%rbp), %ecx
shll $0x2, %ecx
movslq %ecx, %rcx
movss 0x4(%rax,%rcx,4), %xmm0
movss 0x24(%rdi), %xmm1
subss %xmm1, %xmm0
movss %xmm0, -0x34(%rbp)
movq 0x58(%rdi), %rax
movl -0x24(%rbp), %ecx
shll $0x2, %ecx
movslq %ecx, %rcx
movss 0x8(%rax,%rcx,4), %xmm0
movss 0x28(%rdi), %xmm1
subss %xmm1, %xmm0
movss %xmm0, -0x30(%rbp)
movups 0x2c(%rdi), %xmm0
movaps %xmm0, -0xe0(%rbp)
movups -0x3c(%rbp), %xmm0
movaps %xmm0, -0xf0(%rbp)
movsd -0xe0(%rbp), %xmm0
movsd -0xd8(%rbp), %xmm1
movsd -0xf0(%rbp), %xmm2
movsd -0xe8(%rbp), %xmm3
callq 0x86b0
movq -0x158(%rbp), %rdi
movlpd %xmm0, -0xc4(%rbp)
movlpd %xmm1, -0xbc(%rbp)
movups 0x2c(%rdi), %xmm0
movaps %xmm0, -0x110(%rbp)
movsd -0x110(%rbp), %xmm0
movsd -0x108(%rbp), %xmm1
callq 0x8800
movq -0x158(%rbp), %rdi
movlpd %xmm0, -0x100(%rbp)
movlpd %xmm1, -0xf8(%rbp)
movsd -0xc4(%rbp), %xmm0
movsd -0xbc(%rbp), %xmm1
movsd -0x100(%rbp), %xmm2
movsd -0xf8(%rbp), %xmm3
callq 0x86b0
movq -0x158(%rbp), %rax
movlpd %xmm0, -0xb4(%rbp)
movlpd %xmm1, -0xac(%rbp)
movq -0xb4(%rbp), %rcx
movq %rcx, -0x4c(%rbp)
movq -0xac(%rbp), %rcx
movq %rcx, -0x44(%rbp)
movss -0x40(%rbp), %xmm0
movd %xmm0, %ecx
xorl $0x80000000, %ecx       # imm = 0x80000000
movd %ecx, %xmm0
ucomiss 0x3c(%rax), %xmm0
jbe 0x8f19
movq -0x158(%rbp), %rax
movss -0x48(%rbp), %xmm0
movss 0x3c(%rax), %xmm1
mulss %xmm1, %xmm0
movaps 0xa54e(%rip), %xmm2     # 0x13390
movss -0x40(%rbp), %xmm1
pxor %xmm2, %xmm1
divss %xmm1, %xmm0
movss %xmm0, -0x114(%rbp)
leaq -0x68(%rbp), %rdi
leaq -0x114(%rbp), %rsi
callq 0xac40
jmp 0x8e69
movq -0x158(%rbp), %rax
movss -0x44(%rbp), %xmm0
movss 0x3c(%rax), %xmm1
mulss %xmm1, %xmm0
movaps 0xa50b(%rip), %xmm2     # 0x13390
movss -0x40(%rbp), %xmm1
pxor %xmm2, %xmm1
divss %xmm1, %xmm0
movss %xmm0, -0x118(%rbp)
leaq -0x80(%rbp), %rdi
leaq -0x118(%rbp), %rsi
callq 0xac40
jmp 0x8eac
movq -0x158(%rbp), %rax
movq 0x58(%rax), %rax
movl -0x24(%rbp), %ecx
shll $0x2, %ecx
movslq %ecx, %rcx
cvttss2si 0xc(%rax,%rcx,4), %rax
movl %eax, -0x11c(%rbp)
leaq -0x98(%rbp), %rdi
leaq -0x11c(%rbp), %rsi
callq 0xad40
jmp 0x8ee2
jmp 0x8f19
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
leaq -0x98(%rbp), %rdi
callq 0xadb0
leaq -0x80(%rbp), %rdi
callq 0xabe0
leaq -0x68(%rbp), %rdi
callq 0xabe0
jmp 0x9446
jmp 0x8f1b
movl -0x24(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x24(%rbp)
jmp 0x8cb2
movl $0x0, -0x120(%rbp)
movss 0xa445(%rip), %xmm0     # 0x13380
movss %xmm0, -0x124(%rbp)
movss 0xa435(%rip), %xmm0     # 0x13380
movss %xmm0, -0x128(%rbp)
movss 0xa421(%rip), %xmm0     # 0x1337c
movss %xmm0, -0x13c(%rbp)
testb $0x1, -0x1d(%rbp)
je 0x908b
movq $0x0, -0x148(%rbp)
movq -0x148(%rbp), %rax
movq %rax, -0x160(%rbp)
leaq -0x68(%rbp), %rdi
callq 0xaba0
movq %rax, %rcx
movq -0x160(%rbp), %rax
cmpq %rcx, %rax
jae 0x9089
movq -0x148(%rbp), %rsi
leaq -0x68(%rbp), %rdi
callq 0xabc0
movss (%rax), %xmm0
callq 0xad70
movss %xmm0, -0x164(%rbp)
jmp 0x8fc5
movss -0x164(%rbp), %xmm0
ucomiss -0x124(%rbp), %xmm0
jbe 0x9009
movq -0x148(%rbp), %rsi
leaq -0x68(%rbp), %rdi
callq 0xabc0
movss (%rax), %xmm0
callq 0xad70
movss %xmm0, -0x168(%rbp)
jmp 0x8ff9
movss -0x168(%rbp), %xmm0
movss %xmm0, -0x124(%rbp)
movq -0x148(%rbp), %rsi
leaq -0x80(%rbp), %rdi
callq 0xabc0
movss (%rax), %xmm0
callq 0xad70
movss %xmm0, -0x16c(%rbp)
jmp 0x902c
movss -0x16c(%rbp), %xmm0
ucomiss -0x128(%rbp), %xmm0
jbe 0x9070
movq -0x148(%rbp), %rsi
leaq -0x80(%rbp), %rdi
callq 0xabc0
movss (%rax), %xmm0
callq 0xad70
movss %xmm0, -0x170(%rbp)
jmp 0x9060
movss -0x170(%rbp), %xmm0
movss %xmm0, -0x128(%rbp)
jmp 0x9072
movq -0x148(%rbp), %rax
addq $0x1, %rax
movq %rax, -0x148(%rbp)
jmp 0x8f78
jmp 0x90ab
movss 0xa2cd(%rip), %xmm0     # 0x13360
movss %xmm0, -0x124(%rbp)
movss 0xa2bd(%rip), %xmm0     # 0x13360
movss %xmm0, -0x128(%rbp)
movss -0x124(%rbp), %xmm0
ucomiss -0x128(%rbp), %xmm0
jbe 0x9163
movl -0x14(%rbp), %eax
subl $0x1, %eax
cvtsi2ss %eax, %xmm0
movss -0x13c(%rbp), %xmm1
movd %xmm1, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm1
addss %xmm1, %xmm1
movss 0xa275(%rip), %xmm2     # 0x13360
addss %xmm2, %xmm1
mulss %xmm1, %xmm0
movss 0xa271(%rip), %xmm1     # 0x1336c
mulss -0x124(%rbp), %xmm1
divss %xmm1, %xmm0
movss %xmm0, -0x134(%rbp)
movl -0x18(%rbp), %eax
subl $0x1, %eax
cvtsi2ss %eax, %xmm0
movss -0x13c(%rbp), %xmm1
movd %xmm1, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm1
addss %xmm1, %xmm1
movss 0xa226(%rip), %xmm2     # 0x13360
addss %xmm2, %xmm1
mulss %xmm1, %xmm0
movss 0xa222(%rip), %xmm1     # 0x1336c
mulss -0x124(%rbp), %xmm1
divss %xmm1, %xmm0
movss %xmm0, -0x138(%rbp)
jmp 0x9201
movl -0x14(%rbp), %eax
subl $0x1, %eax
cvtsi2ss %eax, %xmm0
movss -0x13c(%rbp), %xmm1
movd %xmm1, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm1
addss %xmm1, %xmm1
movss 0xa1d2(%rip), %xmm2     # 0x13360
addss %xmm2, %xmm1
mulss %xmm1, %xmm0
movss 0xa1ce(%rip), %xmm1     # 0x1336c
mulss -0x128(%rbp), %xmm1
divss %xmm1, %xmm0
movss %xmm0, -0x134(%rbp)
movl -0x18(%rbp), %eax
subl $0x1, %eax
cvtsi2ss %eax, %xmm0
movss -0x13c(%rbp), %xmm1
movd %xmm1, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm1
addss %xmm1, %xmm1
movss 0xa183(%rip), %xmm2     # 0x13360
addss %xmm2, %xmm1
mulss %xmm1, %xmm0
movss 0xa17f(%rip), %xmm1     # 0x1336c
mulss -0x128(%rbp), %xmm1
divss %xmm1, %xmm0
movss %xmm0, -0x138(%rbp)
movq $0x0, -0x150(%rbp)
movq -0x150(%rbp), %rax
movq %rax, -0x178(%rbp)
leaq -0x68(%rbp), %rdi
callq 0xaba0
movq %rax, %rcx
movq -0x178(%rbp), %rax
cmpq %rcx, %rax
jae 0x940d
movq -0x150(%rbp), %rsi
leaq -0x68(%rbp), %rdi
callq 0xabc0
movss (%rax), %xmm0
movss -0x134(%rbp), %xmm2
movl -0x14(%rbp), %eax
movl $0x2, %ecx
cltd
idivl %ecx
cvtsi2ss %eax, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, -0x12c(%rbp)
movq -0x150(%rbp), %rsi
leaq -0x80(%rbp), %rdi
callq 0xabc0
movss (%rax), %xmm0
movss -0x138(%rbp), %xmm2
movl -0x18(%rbp), %eax
movl $0x2, %ecx
cltd
idivl %ecx
cvtsi2ss %eax, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, -0x130(%rbp)
movl -0x1c(%rbp), %eax
subl $0x1, %eax
movl $0x2, %ecx
cltd
idivl %ecx
cvtsi2ss %eax, %xmm1
movss -0x12c(%rbp), %xmm0
subss %xmm1, %xmm0
movss %xmm0, -0x12c(%rbp)
movl -0x1c(%rbp), %eax
subl $0x1, %eax
movl $0x2, %ecx
cltd
idivl %ecx
cvtsi2ss %eax, %xmm1
movss -0x130(%rbp), %xmm0
subss %xmm1, %xmm0
movss %xmm0, -0x130(%rbp)
movl $0x0, -0x28(%rbp)
movl -0x28(%rbp), %eax
cmpl -0x1c(%rbp), %eax
jge 0x93f4
movl $0x0, -0x2c(%rbp)
movl -0x2c(%rbp), %eax
cmpl -0x1c(%rbp), %eax
jge 0x93e4
movss -0x12c(%rbp), %xmm0
cvtsi2ssl -0x28(%rbp), %xmm1
addss %xmm1, %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jb 0x93d4
movss -0x12c(%rbp), %xmm1
cvtsi2ssl -0x28(%rbp), %xmm0
addss %xmm0, %xmm1
cvtsi2ssl -0x14(%rbp), %xmm0
ucomiss %xmm1, %xmm0
jbe 0x93d4
movss -0x130(%rbp), %xmm0
cvtsi2ssl -0x2c(%rbp), %xmm1
addss %xmm1, %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jb 0x93d4
movss -0x130(%rbp), %xmm1
cvtsi2ssl -0x2c(%rbp), %xmm0
addss %xmm0, %xmm1
cvtsi2ssl -0x18(%rbp), %xmm0
ucomiss %xmm1, %xmm0
jbe 0x93d4
movl -0x120(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x120(%rbp)
movq -0x150(%rbp), %rsi
leaq -0x98(%rbp), %rdi
callq 0xad90
movl (%rax), %edx
movq -0x10(%rbp), %rax
cvttss2si -0x130(%rbp), %ecx
addl -0x2c(%rbp), %ecx
imull -0x14(%rbp), %ecx
cvttss2si -0x12c(%rbp), %esi
addl -0x28(%rbp), %esi
addl %esi, %ecx
movslq %ecx, %rcx
movl %edx, (%rax,%rcx,4)
jmp 0x93d6
movl -0x2c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x2c(%rbp)
jmp 0x9312
jmp 0x93e6
movl -0x28(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x28(%rbp)
jmp 0x92ff
jmp 0x93f6
movq -0x150(%rbp), %rax
addq $0x1, %rax
movq %rax, -0x150(%rbp)
jmp 0x920c
movl -0x120(%rbp), %eax
movl %eax, -0x17c(%rbp)
leaq -0x98(%rbp), %rdi
callq 0xadb0
leaq -0x80(%rbp), %rdi
callq 0xabe0
leaq -0x68(%rbp), %rdi
callq 0xabe0
movl -0x17c(%rbp), %eax
addq $0x180, %rsp            # imm = 0x180
popq %rbp
retq
movq -0xa0(%rbp), %rdi
callq 0x2380
nopw %cs:(%rax,%rax)