// File: /workspace/github_cpp_makefile_star2_O3/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 0x9ef3
movq %rsi, %rbx
movslq 0x60(%rsi), %r13
testq %r13, %r13
je 0x9ef3
movq %rdi, %r14
movq %rbx, (%rdi)
movq %r13, %r15
shlq $0x2, %r15
testl %r13d, %r13d
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 0x9bf6
movl $0x40, %edi
callq 0x2200
movq %rax, %r15
movq %rax, %rdi
movq %rbx, %rsi
callq 0x83d4
movq %r15, 0x70(%rbx)
movl 0x60(%rbx), %r13d
testl %r13d, %r13d
jle 0x9ef3
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 0x878c
movq %rbx, %rdi
movq %r15, %rsi
callq 0x6448
xorps %xmm2, %xmm2
movaps %xmm2, 0x60(%rsp)
movaps %xmm2, 0x50(%rsp)
movaps %xmm2, 0x40(%rsp)
movaps %xmm2, 0x30(%rsp)
movq $0x0, 0x70(%rsp)
movq 0x10(%rsp), %rax
movq 0x18(%rsp), %rcx
subq %rax, %rcx
je 0x9d8d
sarq $0x2, %rcx
movq 0x58(%rbx), %rdx
movaps %xmm1, %xmm2
shufps $0x55, %xmm1, %xmm2     # xmm2 = xmm2[1,1],xmm1[1,1]
cmpq $0x1, %rcx
adcq $0x0, %rcx
shufps $0x10, %xmm0, %xmm1     # xmm1 = xmm1[0,0],xmm0[1,0]
shufps $0xe2, %xmm0, %xmm1     # xmm1 = xmm1[2,0],xmm0[2,3]
xorpd %xmm3, %xmm3
xorps %xmm0, %xmm0
xorl %esi, %esi
xorpd %xmm4, %xmm4
xorpd %xmm5, %xmm5
xorpd %xmm6, %xmm6
movslq (%rax,%rsi,4), %rdi
leaq (,%rdi,4), %r8
shlq $0x4, %rdi
orl $0x2, %r8d
movslq %r8d, %r8
movss (%rdx,%r8,4), %xmm7
subss %xmm2, %xmm7
movsd (%rdx,%rdi), %xmm8
subps %xmm1, %xmm8
movaps %xmm8, %xmm9
shufps $0x0, %xmm8, %xmm9      # xmm9 = xmm9[0,0],xmm8[0,0]
mulps %xmm8, %xmm9
cvtps2pd %xmm9, %xmm9
addpd %xmm9, %xmm3
movaps %xmm8, %xmm10
movaps %xmm7, %xmm11
shufps $0xd4, %xmm8, %xmm11    # xmm11 = xmm11[0,1],xmm8[1,3]
shufps $0x52, %xmm8, %xmm11    # xmm11 = xmm11[2,0],xmm8[1,1]
mulss %xmm7, %xmm8
cvtss2sd %xmm8, %xmm8
movsd %xmm8, %xmm9            # xmm9 = xmm8[0],xmm9[1]
addpd %xmm9, %xmm4
shufps $0x55, %xmm10, %xmm10   # xmm10 = xmm10[1,1,1,1]
mulps %xmm10, %xmm11
cvtps2pd %xmm11, %xmm9
addpd %xmm9, %xmm5
movsd %xmm8, %xmm9            # xmm9 = xmm8[0],xmm9[1]
addpd %xmm9, %xmm6
mulss %xmm7, %xmm7
cvtss2sd %xmm7, %xmm7
addsd %xmm7, %xmm0
incq %rsi
cmpq %rsi, %rcx
jne 0x9cd4
movapd %xmm3, 0x30(%rsp)
movapd %xmm4, 0x40(%rsp)
movapd %xmm5, 0x50(%rsp)
movapd %xmm6, 0x60(%rsp)
movsd %xmm0, 0x70(%rsp)
movl $0x3, %edi
leaq 0x30(%rsp), %rsi
leaq 0x80(%rsp), %rdx
leaq 0xf0(%rsp), %rcx
movq %r12, %r8
callq 0x9f38
movq 0x10(%r14), %rax
movss (%rax,%r13,4), %xmm0
xorl %ecx, %ecx
movsd 0x28(%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 0x9de8
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 0x9dc1
cvtss2sd %xmm0, %xmm0
movsd %xmm3, 0x28(%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 0x9ea4
movq 0x8(%r14), %rcx
leaq (%rcx,%rax,4), %rax
cvtsd2ss %xmm1, %xmm1
movss %xmm2, (%rax)
movss %xmm1, 0x4(%rax)
cvtsd2ss %xmm0, %xmm0
jmp 0x9ec7
movq 0x8(%r14), %rcx
leaq (%rcx,%rax,4), %rax
cvtsd2ss %xmm1, %xmm1
unpcklps %xmm1, %xmm2            # xmm2 = xmm2[0],xmm1[0],xmm2[1],xmm1[1]
movaps 0x1546(%rip), %xmm1     # 0xb400
xorps %xmm1, %xmm2
movlps %xmm2, (%rax)
cvtsd2ss %xmm0, %xmm0
xorps %xmm1, %xmm0
movss %xmm0, 0x8(%rax)
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x9ee3
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x2210
incq %r13
movslq 0x60(%rbx), %rax
cmpq %rax, %r13
jl 0x9c0f
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 0x9f2b
jmp 0x9f16
jmp 0x9f16
movq %rax, %rbx
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x9f30
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x2210
movq %rbx, %rdi
callq 0x23a0