// File: /workspace/github_cpp_makefile_star2_O2/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 $0x168, %rsp            # imm = 0x168
movl %esi, %ebx
movsd %xmm0, 0x48(%rsp)
movq %rdi, %r14
leaq 0x20(%rsp), %rdi
callq 0x9a7ae
leaq 0xd0(%rsp), %rdi
movq %r14, %rsi
callq 0x2f6e6
movq %rbx, 0x50(%rsp)
leal 0x1(%rbx), %eax
movl %eax, 0x1c(%rsp)
movslq %eax, %rcx
pushq $0x18
popq %rdx
movq %rcx, %rax
mulq %rdx
pushq $-0x1
popq %rdx
cmovnoq %rax, %rdx
movq %rdx, 0x30(%rsp)
imulq $0x18, %rcx, %rax
movq %rax, 0x60(%rsp)
xorl %r15d, %r15d
leaq 0x120(%rsp), %r13
xorl %ebp, %ebp
movq $0x0, 0x8(%rsp)
movslq 0x10(%r14), %rax
cmpq %rax, %r15
jge 0x31c86
movq 0x8(%r14), %rax
movq %r15, 0x58(%rsp)
shlq $0x4, %r15
movq %rax, %rbx
movq 0x8(%rax,%r15), %rax
testb $0x40, 0x2f(%rax)
je 0x31930
movzwl 0x1c(%rax), %eax
cvtsi2ss %eax, %xmm0
mulss 0x974dd(%rip), %xmm0    # 0xc8e00
ucomiss 0x97cce(%rip), %xmm0    # 0xc95f8
ja 0x31c79
cmpq $0x0, 0x8(%rsp)
jne 0x31997
movq %rbp, (%rsp)
movq %r13, %rdi
callq 0x9a7ae
xorl %ebp, %ebp
xorl %r12d, %r12d
movslq 0x10(%r14), %rax
cmpq %rax, %r12
jge 0x3196a
movq 0x8(%r14), %rsi
addq %rbp, %rsi
movq %r13, %rdi
callq 0x1784c
incq %r12
addq $0x10, %rbp
jmp 0x31949
pushq $0x60
popq %rdi
callq 0x134f0
movq %rax, 0x8(%rsp)
movq %rax, %rdi
movq %r13, %rsi
leaq -0x2466(%rip), %rdx     # 0x2f51e
xorl %ecx, %ecx
callq 0x31d36
movq (%rsp), %rbp
movq %r13, %rdi
callq 0x9a81c
leaq 0x20(%rsp), %r12
movq %r12, %rdi
xorl %esi, %esi
callq 0x9a9f8
addq %r15, %rbx
xorps %xmm0, %xmm0
movq 0x8(%rsp), %rdi
movq %rbx, (%rsp)
movq %rbx, %rsi
movsd 0x48(%rsp), %xmm1
movq 0x50(%rsp), %rdx
movq %r12, %rcx
xorl %r8d, %r8d
callq 0x31e52
cmpl $0x3, 0x2c(%rsp)
jl 0x31c79
testq %rbp, %rbp
jne 0x31a09
movq 0x30(%rsp), %rdi
callq 0x13030
movq %rax, %rbp
cmpl $0x0, 0x1c(%rsp)
je 0x31a09
xorl %r15d, %r15d
leaq (%r15,%rbp), %rdi
callq 0x781ba
addq $0x18, %r15
cmpq %r15, 0x60(%rsp)
jne 0x319f5
movq %r13, %rdi
movq (%rsp), %rsi
callq 0x16310
movq %rbp, %rdi
movq %r13, %rsi
callq 0x784a6
movq %rbp, %rbx
leaq 0x18(%rbp), %r15
pushq $0x1
popq %rbp
leaq -0x1(%rbp), %r12
movslq 0x2c(%rsp), %rax
cmpq %rax, %r12
jge 0x31a5e
movq 0x20(%rsp), %rax
movq -0x8(%rax,%rbp,8), %rsi
movq %r13, %rdi
callq 0x16310
movq %r15, %rdi
movq %r13, %rsi
callq 0x784a6
incq %rbp
addq $0x18, %r15
jmp 0x31a2a
movq %r13, %rdi
movq (%rsp), %rsi
callq 0x16310
cmpq $0x7, %rbp
pushq $0x6
popq %rax
cmovaeq %rax, %r12
imulq $0x18, %r12, %rdi
addq %rbx, %rdi
movq %r13, %rsi
callq 0x495ca
movsd %xmm0, 0x40(%rsp)
leaq 0xf3197(%rip), %rax     # 0x124c28
movsd (%rax), %xmm0
movsd %xmm0, 0x38(%rsp)
leaq 0xb8(%rsp), %r15
movq %r15, %rdi
movl %ebp, %esi
movq %rbx, %rdx
xorl %ecx, %ecx
callq 0x4d640
movq %r13, %rdi
movq %r15, %rsi
movl %ebp, %edx
movq %rbx, %rcx
xorl %r8d, %r8d
leaq 0x80(%rsp), %r9
callq 0x4d6c9
movq %rbx, %rbp
leaq 0x68(%rsp), %rdi
leaq 0x150(%rsp), %rsi
callq 0x7898c
leaq 0xa0(%rsp), %rdi
movq %r13, %rsi
callq 0x7898c
movsd 0x80(%rsp), %xmm1
ucomisd 0x9687d(%rip), %xmm1    # 0xc8380
movsd 0x9663d(%rip), %xmm0    # 0xc8148
movsd %xmm0, 0x10(%rsp)
jbe 0x31b3b
movsd 0x88(%rsp), %xmm0
divsd %xmm1, %xmm0
ucomisd 0x96858(%rip), %xmm0    # 0xc8380
jb 0x31b30
sqrtsd %xmm0, %xmm0
jmp 0x31b35
callq 0x133d0
movsd %xmm0, 0x10(%rsp)
movq (%rsp), %rax
movq (%rax), %rax
movq 0xb8(%rax), %rdi
testq %rdi, %rdi
je 0x31ba0
pushq $0x1
popq %rsi
callq 0x3398a
movq %rax, %r15
testq %rax, %rax
je 0x31ba0
leaq 0xe8(%rsp), %rbx
movq %rbx, %rdi
leaq 0xb8(%rsp), %rsi
leaq 0x68(%rsp), %rdx
callq 0x76e1c
addq $0x28, %r15
movq %rbx, %rdi
movq %r15, %rsi
callq 0x497d0
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x31bf0
leaq 0x68(%rsp), %rdi
callq 0x78cd2
jmp 0x31bf0
leaq 0x108(%rsp), %rbx
movq %rbx, %rdi
movq (%rsp), %rsi
callq 0x16310
leaq 0xe8(%rsp), %r15
movq %r15, %rdi
movq %rbx, %rsi
leaq 0xd0(%rsp), %rdx
callq 0x78399
movq %r15, %rdi
leaq 0x68(%rsp), %rsi
callq 0x78bf6
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x31bf0
leaq 0x68(%rsp), %rdi
callq 0x78cd2
movq (%rsp), %rax
movq 0x8(%rax), %rax
movb 0x2f(%rax), %cl
testb $0x40, %cl
jne 0x31c19
movq (%rsp), %rdi
leaq 0x68(%rsp), %rsi
callq 0x2f290
movq (%rsp), %rax
movq 0x8(%rax), %rax
movb 0x2f(%rax), %cl
testb $0x1, %cl
jne 0x31c37
movq (%rsp), %rdi
leaq 0xa0(%rsp), %rsi
callq 0x2f2c0
movq (%rsp), %rax
movq 0x8(%rax), %rax
movzwl 0x1c(%rax), %eax
cvtsi2ss %eax, %xmm0
mulss 0x971b9(%rip), %xmm0    # 0xc8e00
ucomiss 0x979aa(%rip), %xmm0    # 0xc95f8
jne 0x31c79
jp 0x31c79
movsd 0x38(%rsp), %xmm0
maxsd 0x40(%rsp), %xmm0
movsd 0x10(%rsp), %xmm1
mulsd %xmm0, %xmm1
cvttsd2si %xmm0, %esi
cvtsd2ss %xmm1, %xmm0
movq (%rsp), %rdi
callq 0x2f31e
movq 0x58(%rsp), %r15
incq %r15
jmp 0x318eb
testq %rbp, %rbp
je 0x31c93
movq %rbp, %rdi
callq 0x136a0
movq 0x8(%rsp), %rbx
testq %rbx, %rbx
je 0x31cb0
movq %rbx, %rdi
callq 0x31e66
pushq $0x60
popq %rsi
movq %rbx, %rdi
callq 0x13500
leaq 0x20(%rsp), %rdi
callq 0x9a81c
addq $0x168, %rsp            # imm = 0x168
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x31d20
jmp 0x31d20
jmp 0x31d20
jmp 0x31d0e
movq %rax, %rbx
pushq $0x60
popq %rsi
movq 0x8(%rsp), %rdi
callq 0x13500
jmp 0x31d11
jmp 0x31d20
jmp 0x31d20
jmp 0x31d20
jmp 0x31d20
jmp 0x31d20
jmp 0x31d20
jmp 0x31d20
jmp 0x31d20
jmp 0x31d20
jmp 0x31d20
jmp 0x31d20
movq %rax, %rbx
movq %rbp, %rdi
movq 0x30(%rsp), %rsi
callq 0x139f0
jmp 0x31d23
movq %rax, %rbx
leaq 0x120(%rsp), %rdi
callq 0x9a81c
jmp 0x31d23
movq %rax, %rbx
leaq 0x20(%rsp), %rdi
callq 0x9a81c
movq %rbx, %rdi
callq 0x138f0
nop