// File: /workspace/github_cpp_makefile_star2_O1/tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelPointSet.cpp
// Function: gaps::R3SurfelPointSet::UpdateNormals(double, int) const
void R3SurfelPointSet::
UpdateNormals(RNScalar max_neighborhood_radius, int max_neighborhood_points) const
{
  // Declare variables (fill it only if needed)
  R3Kdtree<R3SurfelPoint *> *kdtree = NULL;
  RNArray<R3SurfelPoint *> neighbors;
  R3Point pointset_centroid = Centroid();
  R3Point *positions = NULL;

  // Compute normals and radii for all points that don't already have them
  for (int i = 0; i < NPoints(); i++) {
    R3SurfelPoint *point = Point(i);
    if (point->HasNormal() && (point->Radius() > 0)) continue;

    // Allocate kdtree
    if (!kdtree) {
      RNArray<R3SurfelPoint *> points;
      for (int j = 0; j < NPoints(); j++) points.Insert(Point(j));
      if (!kdtree) kdtree = new R3Kdtree<R3SurfelPoint *>(points, SurfelPointPosition, NULL);
      if (!kdtree) RNAbort("Unable to allocate kdtree to update normals");
    }

    // Find neighbors with kdtree
    neighbors.Truncate(0);
    kdtree->FindClosest(point, 0, max_neighborhood_radius, max_neighborhood_points, neighbors);
    if (neighbors.NEntries() < 3) continue;

    // Allocate array of positions
    if (!positions) {
      positions = new R3Point [max_neighborhood_points + 1];
      if (!positions) RNAbort("Unable to allocate positions to update normals");
    }

    // Create array of positions for neighborhood
    int npositions = 0;
    positions[npositions++] = point->Position();
    for (int j = 0; j < neighbors.NEntries(); j++) {
      R3SurfelPoint *neighbor = neighbors.Kth(j);
      positions[npositions++] = neighbor->Position();
    }

    // Compute radius of neighborhood
    int neighbor_index = (npositions <= 6) ? npositions-1 : 6;
    RNScalar radius0 = R3Distance(positions[neighbor_index], point->Position());
    if (radius0 < RN_EPSILON) radius0 = RN_EPSILON;
    
    // Compute normal with PCA of neighborhood
    RNScalar variances[3];
    R3Point centroid = R3Centroid(npositions, positions);
    R3Triad triad = R3PrincipleAxes(centroid, npositions, positions, NULL, variances);
    R3Vector normal = triad[2];
    R3Vector tangent = triad[0];
    RNScalar aspect = (variances[0] > 0) ? sqrt(variances[1]/variances[0]) : 1;
    RNScalar radius1 = aspect * radius0;

    // Flip normal
    R3SurfelBlock *block = point->Block();
    R3SurfelNode *node = block->Node();
    R3SurfelScan *scan = (node) ? node->Scan() : NULL;
    if (scan) {
      // Orient normal towards scan viewpoint
      R3Plane plane(centroid, normal);
      const R3Point& viewpoint = scan->Viewpoint();
      if (R3SignedDistance(plane, viewpoint) < 0) normal.Flip();
    }
    else {
      // Orient normal away from pointset centroid
      R3Vector v = point->Position() - pointset_centroid;
      if (v.Dot(normal) < 0) normal.Flip();
    }

    // Assign normal
    if (!point->HasNormal()) point->SetNormal(normal);

    // Assign normal
    if (!point->HasTangent()) point->SetTangent(tangent);
    
    // Assign radius
    if (point->Radius(0) == 0) point->SetRadius(radius0, radius1);
  }

  // Delete data
  if (positions) delete [] positions;
  if (kdtree) delete kdtree;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x178, %rsp            # imm = 0x178
movq %rsi, 0x48(%rsp)
movsd %xmm0, 0x60(%rsp)
movq %rdi, %r14
leaq 0x28(%rsp), %rdi
callq 0xaaf34
leaq 0xe0(%rsp), %rdi
movq %r14, %rsi
callq 0x32b74
cmpl $0x0, 0x10(%r14)
jle 0x35945
movq 0x48(%rsp), %rax
incl %eax
movl %eax, 0x3c(%rsp)
movslq %eax, %rcx
movl $0x18, %edx
movq %rcx, %rax
mulq %rdx
movq $-0x1, %rdx
cmovnoq %rax, %rdx
movq %rdx, 0x40(%rsp)
shlq $0x3, %rcx
leaq (%rcx,%rcx,2), %rax
movq %rax, 0x70(%rsp)
xorl %ebx, %ebx
leaq 0x118(%rsp), %r13
movq $0x0, 0x18(%rsp)
movq $0x0, 0x10(%rsp)
movq 0x8(%r14), %rbp
movq %rbx, 0x68(%rsp)
shlq $0x4, %rbx
movq 0x8(%rbp,%rbx), %rax
testb $0x40, 0x2f(%rax)
je 0x3551c
movzwl 0x1c(%rax), %eax
xorps %xmm0, %xmm0
cvtsi2ss %eax, %xmm0
mulss 0xa9901(%rip), %xmm0    # 0xdee10
ucomiss 0xaa0f2(%rip), %xmm0    # 0xdf608
ja 0x3592e
cmpq $0x0, 0x18(%rsp)
jne 0x3558a
movq %r13, %rdi
callq 0xaaf34
cmpl $0x0, 0x10(%r14)
jle 0x3555f
xorl %r15d, %r15d
xorl %r12d, %r12d
movq 0x8(%r14), %rsi
addq %r15, %rsi
movl 0x124(%rsp), %edx
movq %r13, %rdi
callq 0xaafca
incq %r12
movslq 0x10(%r14), %rax
addq $0x10, %r15
cmpq %rax, %r12
jl 0x35539
movl $0x60, %edi
callq 0x134e0
movq %rax, 0x18(%rsp)
movq %rax, %rdi
movq %r13, %rsi
leaq -0x2bf6(%rip), %rdx     # 0x32985
xorl %ecx, %ecx
callq 0x35a0e
movq %r13, %rdi
callq 0xaaf9e
leaq 0x28(%rsp), %r12
movq %r12, %rdi
xorl %esi, %esi
callq 0xab1ac
addq %rbx, %rbp
movq $0x0, (%rsp)
xorps %xmm0, %xmm0
movq 0x18(%rsp), %rdi
movq %rbp, %rsi
movsd 0x60(%rsp), %xmm1
movq 0x48(%rsp), %rdx
xorl %ecx, %ecx
xorl %r8d, %r8d
movq %r12, %r9
callq 0x35fea
cmpl $0x3, 0x34(%rsp)
jl 0x3592e
cmpq $0x0, 0x10(%rsp)
jne 0x3560b
movq 0x40(%rsp), %rdi
callq 0x13030
movq %rax, 0x10(%rsp)
cmpl $0x0, 0x3c(%rsp)
je 0x3560b
xorl %ebx, %ebx
movq 0x10(%rsp), %rax
leaq (%rax,%rbx), %rdi
callq 0x84d7c
addq $0x18, %rbx
cmpq %rbx, 0x70(%rsp)
jne 0x355f2
movq (%rbp), %rax
movq 0x8(%rbp), %rcx
xorps %xmm0, %xmm0
cvtss2sd (%rcx), %xmm0
addsd 0x10(%rax), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rcx), %xmm1
addsd 0x18(%rax), %xmm1
cvtss2sd 0x8(%rcx), %xmm2
addsd 0x20(%rax), %xmm2
movq %r13, %rdi
callq 0x84d7e
movq 0x10(%rsp), %rdi
movq %r13, %rsi
callq 0x850ec
cmpl $0x0, 0x34(%rsp)
jle 0x356bb
movq 0x10(%rsp), %rax
leaq 0x18(%rax), %r15
xorl %ebx, %ebx
movq 0x28(%rsp), %rax
movq (%rax,%rbx,8), %rax
movq (%rax), %rcx
movq 0x8(%rax), %rax
xorps %xmm0, %xmm0
cvtss2sd (%rax), %xmm0
addsd 0x10(%rcx), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rax), %xmm1
addsd 0x18(%rcx), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rax), %xmm2
addsd 0x20(%rcx), %xmm2
movq %r13, %rdi
callq 0x84d7e
movq %r15, %rdi
movq %r13, %rsi
callq 0x850ec
incq %rbx
movslq 0x34(%rsp), %rax
addq $0x18, %r15
cmpq %rax, %rbx
jl 0x3565d
incl %ebx
jmp 0x356c0
movl $0x1, %ebx
movq (%rbp), %rax
movq 0x8(%rbp), %rcx
xorps %xmm0, %xmm0
cvtss2sd (%rcx), %xmm0
addsd 0x10(%rax), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rcx), %xmm1
addsd 0x18(%rax), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rcx), %xmm2
addsd 0x20(%rax), %xmm2
movq %r13, %rdi
callq 0x84d7e
movq 0x10(%rsp), %r15
leal -0x1(%rbx), %eax
cmpl $0x7, %ebx
movl $0x6, %ecx
cmovael %ecx, %eax
cltq
leaq (%rax,%rax,2), %rax
leaq (%r15,%rax,8), %rdi
movq %r13, %rsi
callq 0x508bc
movsd %xmm0, 0x58(%rsp)
leaq 0xff4e0(%rip), %rax     # 0x134c08
movsd (%rax), %xmm0
movsd %xmm0, 0x50(%rsp)
leaq 0xc8(%rsp), %r12
movq %r12, %rdi
movl %ebx, %esi
movq %r15, %rdx
xorl %ecx, %ecx
callq 0x5501d
movq %r13, %rdi
movq %r12, %rsi
movl %ebx, %edx
movq %r15, %rcx
xorl %r8d, %r8d
leaq 0x90(%rsp), %r9
callq 0x550a7
leaq 0x78(%rsp), %rdi
leaq 0x148(%rsp), %rsi
callq 0x85826
leaq 0xb0(%rsp), %rdi
movq %r13, %rsi
callq 0x85826
movsd 0x90(%rsp), %xmm1
ucomisd 0xa8bf1(%rip), %xmm1    # 0xde388
movsd 0xa89b1(%rip), %xmm0    # 0xde150
movsd %xmm0, 0x20(%rsp)
jbe 0x357cf
movsd 0x98(%rsp), %xmm0
divsd %xmm1, %xmm0
ucomisd 0xa8bcc(%rip), %xmm0    # 0xde388
jb 0x357c4
sqrtsd %xmm0, %xmm0
jmp 0x357c9
callq 0x133c0
movsd %xmm0, 0x20(%rsp)
movq (%rbp), %rax
movq 0xb8(%rax), %rdi
testq %rdi, %rdi
je 0x357ee
movl $0x1, %esi
callq 0x37aec
movq %rax, %rbx
jmp 0x357f0
xorl %ebx, %ebx
testq %rbx, %rbx
je 0x3583b
leaq 0xf8(%rsp), %r15
movq %r15, %rdi
leaq 0xc8(%rsp), %rsi
leaq 0x78(%rsp), %rdx
callq 0x838fe
addq $0x28, %rbx
movq %r15, %rdi
movq %rbx, %rsi
callq 0x50acf
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x358b2
leaq 0x78(%rsp), %rdi
callq 0x85ba4
jmp 0x358b2
movq (%rbp), %rax
movq 0x8(%rbp), %rcx
xorps %xmm0, %xmm0
cvtss2sd (%rcx), %xmm0
addsd 0x10(%rax), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rcx), %xmm1
addsd 0x18(%rax), %xmm1
cvtss2sd 0x8(%rcx), %xmm2
addsd 0x20(%rax), %xmm2
leaq 0x160(%rsp), %rbx
movq %rbx, %rdi
callq 0x84d7e
leaq 0xf8(%rsp), %r15
movq %r15, %rdi
movq %rbx, %rsi
leaq 0xe0(%rsp), %rdx
callq 0x84fe0
movq %r15, %rdi
leaq 0x78(%rsp), %rsi
callq 0x85aae
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x358b2
leaq 0x78(%rsp), %rdi
callq 0x85ba4
movq 0x8(%rbp), %rax
testb $0x40, 0x2f(%rax)
jne 0x358c9
movq %rbp, %rdi
leaq 0x78(%rsp), %rsi
callq 0x326e2
movq 0x8(%rbp), %rax
testb $0x1, 0x2f(%rax)
jne 0x358e3
movq %rbp, %rdi
leaq 0xb0(%rsp), %rsi
callq 0x32710
movq 0x8(%rbp), %rax
movzwl 0x1c(%rax), %eax
xorps %xmm0, %xmm0
cvtsi2ss %eax, %xmm0
mulss 0xa9516(%rip), %xmm0    # 0xdee10
ucomiss 0xa9d07(%rip), %xmm0    # 0xdf608
jne 0x3592e
jp 0x3592e
movsd 0x50(%rsp), %xmm0
maxsd 0x58(%rsp), %xmm0
movsd 0x20(%rsp), %xmm1
mulsd %xmm0, %xmm1
cvttsd2si %xmm0, %esi
xorps %xmm0, %xmm0
cvtsd2ss %xmm1, %xmm0
movq %rbp, %rdi
callq 0x3276a
movq 0x68(%rsp), %rbx
incq %rbx
movslq 0x10(%r14), %rax
cmpq %rax, %rbx
jl 0x354e4
jmp 0x35957
movq $0x0, 0x10(%rsp)
movq $0x0, 0x18(%rsp)
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x35966
callq 0x13680
movq 0x18(%rsp), %rbx
testq %rbx, %rbx
je 0x35985
movq %rbx, %rdi
callq 0x35b4a
movl $0x60, %esi
movq %rbx, %rdi
callq 0x134f0
leaq 0x28(%rsp), %rdi
callq 0xaaf9e
addq $0x178, %rsp            # imm = 0x178
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x359f9
jmp 0x359f9
jmp 0x359f9
jmp 0x359f9
jmp 0x359f9
movq %rax, %rbx
movl $0x60, %esi
movq 0x18(%rsp), %rdi
callq 0x134f0
jmp 0x359ea
jmp 0x359e7
jmp 0x359f9
jmp 0x359f9
jmp 0x359f9
jmp 0x359f9
jmp 0x359f9
jmp 0x359f9
jmp 0x359f9
jmp 0x359f9
jmp 0x359f9
movq %rax, %rbx
movq 0x10(%rsp), %rdi
movq 0x40(%rsp), %rsi
callq 0x139b0
jmp 0x359fc
movq %rax, %rbx
leaq 0x118(%rsp), %rdi
callq 0xaaf9e
jmp 0x359fc
movq %rax, %rbx
leaq 0x28(%rsp), %rdi
callq 0xaaf9e
movq %rbx, %rdi
callq 0x138b0