// File: /workspace/github_cpp_makefile_star2_O2/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 $0x118, %rsp            # imm = 0x118
movss %xmm0, 0x8(%rsp)
testq %rsi, %rsi
je 0x8f77
movq %rsi, %rbx
movslq 0x60(%rsi), %rax
testq %rax, %rax
je 0x8f77
movq %rdi, %r14
movq %rbx, (%rdi)
imulq $0xc, %rax, %rdi
movq %rax, %r15
shlq $0x2, %r15
testl %eax, %eax
pushq $-0x1
popq %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 0x8ca4
pushq $0x40
popq %rdi
callq 0x2200
movq %rax, %r15
movq %rax, %rdi
movq %rbx, %rsi
callq 0x75d8
movq %r15, 0x70(%rbx)
xorl %r13d, %r13d
leaq 0x20(%rsp), %r15
leaq 0xb0(%rsp), %r12
movslq 0x60(%rbx), %rax
cmpq %rax, %r13
jge 0x8f77
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, 0x20(%rsp)
andq $0x0, 0x30(%rsp)
movq 0x70(%rbx), %rdi
movq %r15, %rsi
movss %xmm3, 0x10(%rsp)
movaps %xmm3, %xmm0
movss %xmm1, 0xc(%rsp)
movss %xmm2, 0x14(%rsp)
movss 0x8(%rsp), %xmm3
callq 0x7928
movq %rbx, %rdi
movq %r15, %rsi
callq 0x5aa2
xorps %xmm2, %xmm2
movaps %xmm2, 0x70(%rsp)
movaps %xmm2, 0x60(%rsp)
andq $0x0, 0x80(%rsp)
movaps %xmm2, 0x50(%rsp)
movaps %xmm2, 0x40(%rsp)
movq 0x20(%rsp), %rax
movq 0x28(%rsp), %rcx
subq %rax, %rcx
sarq $0x2, %rcx
movaps %xmm1, %xmm2
shufps $0x55, %xmm1, %xmm2     # xmm2 = xmm2[1,1],xmm1[1,1]
shufps $0x10, %xmm0, %xmm1     # xmm1 = xmm1[0,0],xmm0[1,0]
shufps $0xe2, %xmm0, %xmm1     # xmm1 = xmm1[2,0],xmm0[2,3]
xorps %xmm0, %xmm0
xorpd %xmm3, %xmm3
xorl %edx, %edx
xorpd %xmm4, %xmm4
xorpd %xmm5, %xmm5
xorpd %xmm6, %xmm6
cmpq %rdx, %rcx
je 0x8e37
movq 0x58(%rbx), %rsi
movslq (%rax,%rdx,4), %rdi
leaq (,%rdi,4), %r8
shlq $0x4, %rdi
orl $0x2, %r8d
movslq %r8d, %r8
movss (%rsi,%r8,4), %xmm7
subss %xmm2, %xmm7
movsd (%rsi,%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, %xmm0
movapd %xmm0, 0x40(%rsp)
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
movapd %xmm4, 0x50(%rsp)
shufps $0x55, %xmm10, %xmm10   # xmm10 = xmm10[1,1,1,1]
mulps %xmm10, %xmm11
cvtps2pd %xmm11, %xmm9
addpd %xmm9, %xmm5
movapd %xmm5, 0x60(%rsp)
movsd %xmm8, %xmm9            # xmm9 = xmm8[0],xmm9[1]
addpd %xmm9, %xmm6
movapd %xmm6, 0x70(%rsp)
mulss %xmm7, %xmm7
cvtss2sd %xmm7, %xmm7
addsd %xmm7, %xmm3
movsd %xmm3, 0x80(%rsp)
incq %rdx
jmp 0x8d72
pushq $0x3
popq %rdi
leaq 0x40(%rsp), %rsi
leaq 0x90(%rsp), %rdx
leaq 0x100(%rsp), %rcx
movq %r12, %r8
callq 0x8fb2
movq 0x10(%r14), %rax
movss (%rax,%r13,4), %xmm0
xorl %ecx, %ecx
cvtss2sd %xmm0, %xmm0
cmpq $0x3, %rcx
je 0x8eb0
movsd 0x90(%rsp,%rcx,8), %xmm1
testq %rcx, %rcx
sete %dl
movsd 0x18(%rsp), %xmm3
ucomisd %xmm1, %xmm3
seta %sil
orb %dl, %sil
movapd %xmm1, %xmm2
testb %sil, %sil
jne 0x8e9a
movapd %xmm3, %xmm2
addsd %xmm0, %xmm1
cvtsd2ss %xmm1, %xmm0
cmovnel %ecx, %ebp
incq %rcx
movsd %xmm2, 0x18(%rsp)
jmp 0x8e63
movsd 0x18(%rsp), %xmm1
divsd %xmm0, %xmm1
cvtsd2ss %xmm1, %xmm0
movss %xmm0, (%rax,%r13,4)
leal (%rbp,%rbp,2), %eax
cltq
movsd 0xb0(%rsp,%rax,8), %xmm2
movsd 0xb8(%rsp,%rax,8), %xmm1
movsd 0xc0(%rsp,%rax,8), %xmm0
cvtss2sd 0x10(%rsp), %xmm3
cvtss2sd 0xc(%rsp), %xmm4
mulsd %xmm1, %xmm4
mulsd %xmm2, %xmm3
addsd %xmm4, %xmm3
cvtss2sd 0x14(%rsp), %xmm4
mulsd %xmm0, %xmm4
addsd %xmm3, %xmm4
xorpd %xmm3, %xmm3
ucomisd %xmm4, %xmm3
cvtsd2ss %xmm2, %xmm2
jbe 0x8f3a
movq 0x8(%r14), %rcx
imulq $0xc, %r13, %rdx
leaq (%rcx,%rdx), %rax
cvtsd2ss %xmm1, %xmm1
movss %xmm2, (%rcx,%rdx)
movss %xmm1, 0x4(%rcx,%rdx)
cvtsd2ss %xmm0, %xmm0
jmp 0x8f62
movq 0x8(%r14), %rcx
imulq $0xc, %r13, %rdx
leaq (%rcx,%rdx), %rax
cvtsd2ss %xmm1, %xmm1
unpcklps %xmm1, %xmm2            # xmm2 = xmm2[0],xmm1[0],xmm2[1],xmm1[1]
movaps 0x146c(%rip), %xmm1     # 0xa3c0
xorps %xmm1, %xmm2
movlps %xmm2, (%rcx,%rdx)
cvtsd2ss %xmm0, %xmm0
xorps %xmm1, %xmm0
movss %xmm0, 0x8(%rax)
movq %r15, %rdi
callq 0x2d00
incq %r13
jmp 0x8cb4
addq $0x118, %rsp            # imm = 0x118
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
pushq $0x40
popq %rsi
movq %r15, %rdi
callq 0x2210
jmp 0x8faa
jmp 0x8f9d
jmp 0x8f9d
movq %rax, %rbx
leaq 0x20(%rsp), %rdi
callq 0x2d00
movq %rbx, %rdi
callq 0x23b0