// File: /workspace/github_cpp_makefile_star2_O1/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
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x108, %rsp            # imm = 0x108
movss %xmm0, (%rsp)
testq %rsi, %rsi
je 0xa049
movq %rsi, %rbx
movslq 0x60(%rsi), %rax
testq %rax, %rax
je 0xa049
movq %rdi, %r14
movq %rbx, (%rdi)
movq %rax, %r15
shlq $0x2, %r15
testl %eax, %eax
leaq (%r15,%r15,2), %rdi
movq $-0x1, %rax
cmovsq %rax, %rdi
cmovsq %rax, %r15
callq 0x2040
movq %rax, 0x8(%r14)
movq %r15, %rdi
callq 0x2040
movq %rax, %r12
movq %rax, %rdi
xorl %esi, %esi
movq %r15, %rdx
callq 0x2100
movq %r12, 0x10(%r14)
cmpq $0x0, 0x70(%rbx)
jne 0x9ccd
movl $0x40, %edi
callq 0x2200
movq %rax, %r15
movq %rax, %rdi
movq %rbx, %rsi
callq 0x83c0
movq %r15, 0x70(%rbx)
cmpl $0x0, 0x60(%rbx)
jle 0xa049
xorl %r13d, %r13d
leaq 0x10(%rsp), %r15
leaq 0xa0(%rsp), %r12
movq 0x58(%rbx), %rax
movq %r13, %rcx
shlq $0x4, %rcx
movss (%rax,%rcx), %xmm3
movss 0x4(%rax,%rcx), %xmm1
movss 0x8(%rax,%rcx), %xmm2
xorps %xmm0, %xmm0
movaps %xmm0, 0x10(%rsp)
movq $0x0, 0x20(%rsp)
movq 0x70(%rbx), %rdi
movq %r15, %rsi
movss %xmm3, 0x8(%rsp)
movaps %xmm3, %xmm0
movss %xmm1, 0x4(%rsp)
movss %xmm2, 0xc(%rsp)
movss (%rsp), %xmm3
callq 0x876a
movq %rbx, %rdi
movq %r15, %rsi
callq 0x635c
xorpd %xmm2, %xmm2
movapd %xmm2, 0x60(%rsp)
movapd %xmm2, 0x50(%rsp)
movapd %xmm2, 0x40(%rsp)
movapd %xmm2, 0x30(%rsp)
movq $0x0, 0x70(%rsp)
movq 0x10(%rsp), %rax
movq 0x18(%rsp), %rcx
subq %rax, %rcx
je 0x9ece
sarq $0x2, %rcx
movsd 0x30(%rsp), %xmm3
movsd 0x38(%rsp), %xmm2
movq 0x58(%rbx), %rdx
shufps $0x55, %xmm0, %xmm0     # xmm0 = xmm0[1,1,1,1]
movaps %xmm1, %xmm11
shufps $0x55, %xmm1, %xmm11    # xmm11 = xmm11[1,1],xmm1[1,1]
movsd 0x40(%rsp), %xmm10
movsd 0x48(%rsp), %xmm9
movsd 0x50(%rsp), %xmm8
movsd 0x58(%rsp), %xmm7
movsd 0x60(%rsp), %xmm6
movsd 0x68(%rsp), %xmm5
movsd 0x70(%rsp), %xmm4
cmpq $0x1, %rcx
adcq $0x0, %rcx
xorl %esi, %esi
movslq (%rax,%rsi,4), %rdi
leaq (,%rdi,4), %r8
shlq $0x4, %rdi
movss (%rdx,%rdi), %xmm14
subss %xmm0, %xmm14
leal 0x1(%r8), %edi
movslq %edi, %rdi
movss (%rdx,%rdi,4), %xmm13
subss %xmm1, %xmm13
orl $0x2, %r8d
movslq %r8d, %rdi
movss (%rdx,%rdi,4), %xmm12
subss %xmm11, %xmm12
movaps %xmm14, %xmm15
mulss %xmm14, %xmm15
cvtss2sd %xmm15, %xmm15
addsd %xmm15, %xmm3
movaps %xmm14, %xmm15
mulss %xmm13, %xmm15
cvtss2sd %xmm15, %xmm15
addsd %xmm15, %xmm2
mulss %xmm12, %xmm14
cvtss2sd %xmm14, %xmm14
addsd %xmm14, %xmm10
addsd %xmm15, %xmm9
movaps %xmm13, %xmm15
mulss %xmm13, %xmm15
cvtss2sd %xmm15, %xmm15
addsd %xmm15, %xmm8
mulss %xmm12, %xmm13
cvtss2sd %xmm13, %xmm13
addsd %xmm13, %xmm7
addsd %xmm14, %xmm6
addsd %xmm13, %xmm5
mulss %xmm12, %xmm12
cvtss2sd %xmm12, %xmm12
addsd %xmm12, %xmm4
incq %rsi
cmpq %rsi, %rcx
jne 0x9dd5
movsd %xmm3, 0x30(%rsp)
movsd %xmm2, 0x38(%rsp)
movsd %xmm10, 0x40(%rsp)
movsd %xmm9, 0x48(%rsp)
movsd %xmm8, 0x50(%rsp)
movsd %xmm7, 0x58(%rsp)
movsd %xmm6, 0x60(%rsp)
movsd %xmm5, 0x68(%rsp)
movsd %xmm4, 0x70(%rsp)
movl $0x3, %edi
leaq 0x30(%rsp), %rsi
leaq 0x80(%rsp), %rdx
leaq 0xf0(%rsp), %rcx
movq %r12, %r8
callq 0xa08e
movq 0x10(%r14), %rax
movss (%rax,%r13,4), %xmm0
xorl %ecx, %ecx
movsd 0x78(%rsp), %xmm1
movsd 0x80(%rsp,%rcx,8), %xmm2
testq %rcx, %rcx
sete %dl
ucomisd %xmm2, %xmm1
seta %sil
orb %dl, %sil
movapd %xmm2, %xmm3
testb %sil, %sil
jne 0x9f29
movapd %xmm1, %xmm3
cvtss2sd %xmm0, %xmm0
addsd %xmm0, %xmm2
xorps %xmm0, %xmm0
cvtsd2ss %xmm2, %xmm0
cmovnel %ecx, %ebp
incq %rcx
movapd %xmm3, %xmm1
cmpq $0x3, %rcx
jne 0x9f02
movss %xmm0, (%rax,%r13,4)
movq 0x10(%r14), %rax
xorps %xmm0, %xmm0
cvtss2sd (%rax,%r13,4), %xmm0
movsd %xmm3, 0x78(%rsp)
movapd %xmm3, %xmm1
divsd %xmm0, %xmm1
xorps %xmm0, %xmm0
cvtsd2ss %xmm1, %xmm0
movss %xmm0, (%rax,%r13,4)
leal (%rbp,%rbp,2), %eax
cltq
movsd 0xa0(%rsp,%rax,8), %xmm2
movsd 0xa8(%rsp,%rax,8), %xmm1
movsd 0xb0(%rsp,%rax,8), %xmm0
xorps %xmm3, %xmm3
cvtss2sd 0x8(%rsp), %xmm3
xorps %xmm4, %xmm4
cvtss2sd 0x4(%rsp), %xmm4
mulsd %xmm1, %xmm4
mulsd %xmm2, %xmm3
addsd %xmm4, %xmm3
xorps %xmm4, %xmm4
cvtss2sd 0xc(%rsp), %xmm4
mulsd %xmm0, %xmm4
addsd %xmm3, %xmm4
xorpd %xmm3, %xmm3
ucomisd %xmm4, %xmm3
cvtsd2ss %xmm2, %xmm2
leaq (%r13,%r13,2), %rax
jbe 0x9ff4
movq 0x8(%r14), %rcx
leaq (%rcx,%rax,4), %rax
cvtsd2ss %xmm1, %xmm1
movss %xmm2, (%rax)
movss %xmm1, 0x4(%rax)
cvtsd2ss %xmm0, %xmm0
jmp 0xa01d
movaps 0x13b5(%rip), %xmm3     # 0xb3b0
xorps %xmm3, %xmm2
movq 0x8(%r14), %rcx
leaq (%rcx,%rax,4), %rax
movss %xmm2, (%rax)
cvtsd2ss %xmm1, %xmm1
xorps %xmm3, %xmm1
movss %xmm1, 0x4(%rax)
cvtsd2ss %xmm0, %xmm0
xorps %xmm3, %xmm0
movss %xmm0, 0x8(%rax)
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0xa039
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x2210
incq %r13
movslq 0x60(%rbx), %rax
cmpq %rax, %r13
jl 0x9ce7
addq $0x108, %rsp            # imm = 0x108
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
movl $0x40, %esi
movq %r15, %rdi
jmp 0xa081
jmp 0xa06c
jmp 0xa06c
movq %rax, %rbx
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0xa086
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x2210
movq %rbx, %rdi
callq 0x23a0