// File: jingdao[P]PointCloudApp/normal.cpp
// Function: Normal::Normal(PCD*, float)
Normal::Normal(PCD* pointcloud, float radius) {
	if (!pointcloud || pointcloud->numPoints == 0)
		return;

	//set up data structures
	cloud = pointcloud;
	norm = new float[pointcloud->numPoints * 3];
	curvature = new float[pointcloud->numPoints]();
	if (!pointcloud->kdtree)
		pointcloud->kdtree = new KdTree(pointcloud);
	
	for (int i=0;i<pointcloud->numPoints;i++) {
		float P[3] = {
			pointcloud->float_data[i * 4],
			pointcloud->float_data[i * 4 + 1],
			pointcloud->float_data[i * 4 + 2]
		};
		//form covariance matrix
		std::vector<int> neighbors;
		pointcloud->kdtree->search(&neighbors,P[0],P[1],P[2],radius);
		PCD::Quaternion center = pointcloud->getCentroid(&neighbors);
		double cov[9] = {}; //column major
		for (size_t j=0;j<neighbors.size();j++) {
			float deltaP[3] = {
				pointcloud->float_data[neighbors[j] * 4] - center.i,
				pointcloud->float_data[neighbors[j] * 4 + 1] - center.j,
				pointcloud->float_data[neighbors[j] * 4 + 2] - center.k,
			};
			cov[0] += deltaP[0] * deltaP[0];
			cov[1] += deltaP[1] * deltaP[0];
			cov[2] += deltaP[2] * deltaP[0];
			cov[3] += deltaP[0] * deltaP[1];
			cov[4] += deltaP[1] * deltaP[1];
			cov[5] += deltaP[2] * deltaP[1];
			cov[6] += deltaP[0] * deltaP[2];
			cov[7] += deltaP[1] * deltaP[2];
			cov[8] += deltaP[2] * deltaP[2];
		}
		//compute PCA
		double lambda_real[3], lambda_imag[3], v[9];
		eigenvalue(3,cov,lambda_real,lambda_imag,v);
		//get normal and curvature
		double minLambda;
		int minIndex;
		for (int j=0;j<3;j++) {
			curvature[i] += lambda_real[j];
			if (j==0 || lambda_real[j] < minLambda) {
				minIndex = j;
				minLambda = lambda_real[j];
			}
		}
		curvature[i] = minLambda / curvature[i];
		double N[3] = {
			v[minIndex * 3],
			v[minIndex * 3 + 1],
			v[minIndex * 3 + 2]
		};
		//normal points to origin?
		if (P[0] * N[0] + P[1] * N[1] + P[2] * N[2] < 0) {
			norm[i * 3] = N[0];
			norm[i * 3 + 1] = N[1];
			norm[i * 3 + 2] = N[2];
		} else {
			norm[i * 3] = -N[0];
			norm[i * 3 + 1] = -N[1];
			norm[i * 3 + 2] = -N[2];
		}
//		printf("%4f,%4f,%4f: %lu neighbors %4f,%4f,%4f %4f\n",
//			P[0],P[1],P[2],neighbors.size(),N[0],N[1],N[2],curvature[i]);
	}
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x1f0, %rsp            # imm = 0x1F0
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movss %xmm0, -0x14(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x188(%rbp)
cmpq $0x0, -0x10(%rbp)
je 0x11594
movq -0x10(%rbp), %rax
cmpl $0x0, 0x60(%rax)
jne 0x11599
jmp 0x11cba
movq -0x188(%rbp), %rax
movq -0x10(%rbp), %rcx
movq %rcx, (%rax)
movq -0x10(%rbp), %rax
imull $0x3, 0x60(%rax), %eax
cltq
movl $0x4, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
callq 0x2040
movq %rax, %rcx
movq -0x188(%rbp), %rax
movq %rcx, 0x8(%rax)
movq -0x10(%rbp), %rax
movslq 0x60(%rax), %rax
movl $0x4, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
movq %rdi, -0x198(%rbp)
callq 0x2040
movq -0x198(%rbp), %rdx
movq %rax, %rdi
movq %rdi, -0x190(%rbp)
xorl %esi, %esi
callq 0x2100
movq -0x190(%rbp), %rcx
movq -0x188(%rbp), %rax
movq %rcx, 0x10(%rax)
movq -0x10(%rbp), %rax
cmpq $0x0, 0x70(%rax)
jne 0x1169a
movl $0x40, %edi
callq 0x2200
movq %rax, %rdi
movq %rdi, %rax
movq %rax, -0x1a0(%rbp)
movq -0x10(%rbp), %rsi
callq 0xde90
jmp 0x11667
movq -0x1a0(%rbp), %rcx
movq -0x10(%rbp), %rax
movq %rcx, 0x70(%rax)
jmp 0x1169a
movq -0x1a0(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x20(%rbp)
movl %eax, -0x24(%rbp)
movl $0x40, %esi
callq 0x2210
jmp 0x11cc3
movl $0x0, -0x28(%rbp)
movl -0x28(%rbp), %eax
movq -0x10(%rbp), %rcx
cmpl 0x60(%rcx), %eax
jge 0x11cba
movq -0x10(%rbp), %rax
movq 0x58(%rax), %rax
movl -0x28(%rbp), %ecx
shll $0x2, %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm0
movss %xmm0, -0x34(%rbp)
movq -0x10(%rbp), %rax
movq 0x58(%rax), %rax
movl -0x28(%rbp), %ecx
shll $0x2, %ecx
movslq %ecx, %rcx
movss 0x4(%rax,%rcx,4), %xmm0
movss %xmm0, -0x30(%rbp)
movq -0x10(%rbp), %rax
movq 0x58(%rax), %rax
movl -0x28(%rbp), %ecx
shll $0x2, %ecx
movslq %ecx, %rcx
movss 0x8(%rax,%rcx,4), %xmm0
movss %xmm0, -0x2c(%rbp)
leaq -0x50(%rbp), %rdi
movq %rdi, -0x1a8(%rbp)
callq 0x3130
movq -0x1a8(%rbp), %rsi
movq -0x10(%rbp), %rax
movq 0x70(%rax), %rdi
movss -0x34(%rbp), %xmm0
movss -0x30(%rbp), %xmm1
movss -0x2c(%rbp), %xmm2
movss -0x14(%rbp), %xmm3
callq 0xe3e0
jmp 0x1173e
movq -0x10(%rbp), %rdi
leaq -0x50(%rbp), %rsi
callq 0x8a00
movaps %xmm1, -0x1d0(%rbp)
movaps %xmm0, -0x1c0(%rbp)
jmp 0x1175b
movaps -0x1d0(%rbp), %xmm0
movaps -0x1c0(%rbp), %xmm1
movlpd %xmm1, -0x60(%rbp)
movlpd %xmm0, -0x58(%rbp)
leaq -0xb0(%rbp), %rdi
xorl %esi, %esi
movl $0x48, %edx
callq 0x2100
movq $0x0, -0xb8(%rbp)
movq -0xb8(%rbp), %rax
movq %rax, -0x1d8(%rbp)
leaq -0x50(%rbp), %rdi
callq 0xaca0
movq %rax, %rcx
movq -0x1d8(%rbp), %rax
cmpq %rcx, %rax
jae 0x119ed
movq -0x10(%rbp), %rax
movq 0x58(%rax), %rax
movq %rax, -0x1f0(%rbp)
movq -0xb8(%rbp), %rsi
leaq -0x50(%rbp), %rdi
callq 0xacc0
movq %rax, %rcx
movq -0x1f0(%rbp), %rax
movl (%rcx), %ecx
shll $0x2, %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm0
subss -0x5c(%rbp), %xmm0
movss %xmm0, -0xc4(%rbp)
movq -0x10(%rbp), %rax
movq 0x58(%rax), %rax
movq %rax, -0x1e8(%rbp)
movq -0xb8(%rbp), %rsi
leaq -0x50(%rbp), %rdi
callq 0xacc0
movq %rax, %rcx
movq -0x1e8(%rbp), %rax
movl (%rcx), %ecx
shll $0x2, %ecx
addl $0x1, %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm0
subss -0x58(%rbp), %xmm0
movss %xmm0, -0xc0(%rbp)
movq -0x10(%rbp), %rax
movq 0x58(%rax), %rax
movq %rax, -0x1e0(%rbp)
movq -0xb8(%rbp), %rsi
leaq -0x50(%rbp), %rdi
callq 0xacc0
movq %rax, %rcx
movq -0x1e0(%rbp), %rax
movl (%rcx), %ecx
shll $0x2, %ecx
addl $0x2, %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm0
subss -0x54(%rbp), %xmm0
movss %xmm0, -0xbc(%rbp)
movss -0xc4(%rbp), %xmm0
mulss -0xc4(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0xb0(%rbp), %xmm0
movsd %xmm0, -0xb0(%rbp)
movss -0xc0(%rbp), %xmm0
mulss -0xc4(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0xa8(%rbp), %xmm0
movsd %xmm0, -0xa8(%rbp)
movss -0xbc(%rbp), %xmm0
mulss -0xc4(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0xa0(%rbp), %xmm0
movsd %xmm0, -0xa0(%rbp)
movss -0xc4(%rbp), %xmm0
mulss -0xc0(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x98(%rbp), %xmm0
movsd %xmm0, -0x98(%rbp)
movss -0xc0(%rbp), %xmm0
mulss -0xc0(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x90(%rbp), %xmm0
movsd %xmm0, -0x90(%rbp)
movss -0xbc(%rbp), %xmm0
mulss -0xc0(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x88(%rbp), %xmm0
movsd %xmm0, -0x88(%rbp)
movss -0xc4(%rbp), %xmm0
mulss -0xbc(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x80(%rbp), %xmm0
movsd %xmm0, -0x80(%rbp)
movss -0xc0(%rbp), %xmm0
mulss -0xbc(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x78(%rbp), %xmm0
movsd %xmm0, -0x78(%rbp)
movss -0xbc(%rbp), %xmm0
mulss -0xbc(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x70(%rbp), %xmm0
movsd %xmm0, -0x70(%rbp)
movq -0xb8(%rbp), %rax
addq $0x1, %rax
movq %rax, -0xb8(%rbp)
jmp 0x11791
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x20(%rbp)
movl %eax, -0x24(%rbp)
leaq -0x50(%rbp), %rdi
callq 0x31d0
jmp 0x11cc3
movl $0x3, %edi
leaq -0xb0(%rbp), %rsi
leaq -0xe0(%rbp), %rdx
leaq -0x100(%rbp), %rcx
leaq -0x150(%rbp), %r8
callq 0x11cd0
jmp 0x11a15
movl $0x0, -0x160(%rbp)
cmpl $0x3, -0x160(%rbp)
jge 0x11ac2
movq -0x188(%rbp), %rax
movslq -0x160(%rbp), %rcx
movsd -0xe0(%rbp,%rcx,8), %xmm1
movq 0x10(%rax), %rax
movslq -0x28(%rbp), %rcx
movss (%rax,%rcx,4), %xmm0
cvtss2sd %xmm0, %xmm0
addsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, (%rax,%rcx,4)
cmpl $0x0, -0x160(%rbp)
je 0x11a88
movslq -0x160(%rbp), %rax
movsd -0xe0(%rbp,%rax,8), %xmm1
movsd -0x158(%rbp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x11aac
movl -0x160(%rbp), %eax
movl %eax, -0x15c(%rbp)
movslq -0x160(%rbp), %rax
movsd -0xe0(%rbp,%rax,8), %xmm0
movsd %xmm0, -0x158(%rbp)
jmp 0x11aae
movl -0x160(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x160(%rbp)
jmp 0x11a1f
movq -0x188(%rbp), %rax
movsd -0x158(%rbp), %xmm0
movq 0x10(%rax), %rcx
movslq -0x28(%rbp), %rdx
movss (%rcx,%rdx,4), %xmm1
cvtss2sd %xmm1, %xmm1
divsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movq 0x10(%rax), %rax
movslq -0x28(%rbp), %rcx
movss %xmm0, (%rax,%rcx,4)
imull $0x3, -0x15c(%rbp), %eax
cltq
movsd -0x150(%rbp,%rax,8), %xmm0
movsd %xmm0, -0x180(%rbp)
imull $0x3, -0x15c(%rbp), %eax
addl $0x1, %eax
cltq
movsd -0x150(%rbp,%rax,8), %xmm0
movsd %xmm0, -0x178(%rbp)
imull $0x3, -0x15c(%rbp), %eax
addl $0x2, %eax
cltq
movsd -0x150(%rbp,%rax,8), %xmm0
movsd %xmm0, -0x170(%rbp)
movss -0x34(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0x180(%rbp), %xmm2
movss -0x30(%rbp), %xmm1
cvtss2sd %xmm1, %xmm1
mulsd -0x178(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movss -0x2c(%rbp), %xmm1
cvtss2sd %xmm1, %xmm1
movsd -0x170(%rbp), %xmm2
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
xorps %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x11bfd
movq -0x188(%rbp), %rax
movsd -0x180(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movq 0x8(%rax), %rcx
imull $0x3, -0x28(%rbp), %edx
movslq %edx, %rdx
movss %xmm0, (%rcx,%rdx,4)
movsd -0x178(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movq 0x8(%rax), %rcx
imull $0x3, -0x28(%rbp), %edx
addl $0x1, %edx
movslq %edx, %rdx
movss %xmm0, (%rcx,%rdx,4)
movsd -0x170(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movq 0x8(%rax), %rax
imull $0x3, -0x28(%rbp), %ecx
addl $0x2, %ecx
movslq %ecx, %rcx
movss %xmm0, (%rax,%rcx,4)
jmp 0x11ca3
movq -0x188(%rbp), %rax
movsd -0x180(%rbp), %xmm0
movq %xmm0, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm0
cvtsd2ss %xmm0, %xmm0
movq 0x8(%rax), %rcx
imull $0x3, -0x28(%rbp), %edx
movslq %edx, %rdx
movss %xmm0, (%rcx,%rdx,4)
movsd -0x178(%rbp), %xmm0
movq %xmm0, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm0
cvtsd2ss %xmm0, %xmm0
movq 0x8(%rax), %rcx
imull $0x3, -0x28(%rbp), %edx
addl $0x1, %edx
movslq %edx, %rdx
movss %xmm0, (%rcx,%rdx,4)
movsd -0x170(%rbp), %xmm0
movq %xmm0, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm0
cvtsd2ss %xmm0, %xmm0
movq 0x8(%rax), %rax
imull $0x3, -0x28(%rbp), %ecx
addl $0x2, %ecx
movslq %ecx, %rcx
movss %xmm0, (%rax,%rcx,4)
leaq -0x50(%rbp), %rdi
callq 0x31d0
movl -0x28(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x28(%rbp)
jmp 0x116a1
addq $0x1f0, %rsp            # imm = 0x1F0
popq %rbp
retq
movq -0x20(%rbp), %rdi
callq 0x2380
nopl (%rax)