// File: /workspace/github_cpp_makefile_star2_O3/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 0xa9ca8
leaq 0xe0(%rsp), %rdi
movq %r14, %rsi
callq 0x327dc
cmpl $0x0, 0x10(%r14)
jle 0x35690
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 0x35238
movzwl 0x1c(%rax), %eax
xorps %xmm0, %xmm0
cvtsi2ss %eax, %xmm0
mulss 0xa8bd5(%rip), %xmm0    # 0xdde00
ucomiss 0xa93d6(%rip), %xmm0    # 0xde608
ja 0x3564d
cmpq $0x0, 0x18(%rsp)
jne 0x352a6
movq %r13, %rdi
callq 0xa9ca8
cmpl $0x0, 0x10(%r14)
jle 0x3527b
xorl %r15d, %r15d
xorl %r12d, %r12d
movq 0x8(%r14), %rsi
addq %r15, %rsi
movl 0x124(%rsp), %edx
movq %r13, %rdi
callq 0xa9d3e
incq %r12
movslq 0x10(%r14), %rax
addq $0x10, %r15
cmpq %rax, %r12
jl 0x35255
movl $0x60, %edi
callq 0x134e0
movq %rax, 0x18(%rsp)
movq %rax, %rdi
movq %r13, %rsi
leaq -0x2cac(%rip), %rdx     # 0x325eb
xorl %ecx, %ecx
callq 0x3571a
movq %r13, %rdi
callq 0xa9d12
leaq 0x28(%rsp), %r12
movq %r12, %rdi
xorl %esi, %esi
callq 0xa9f40
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 0x35cf4
cmpl $0x3, 0x34(%rsp)
jl 0x3564d
cmpq $0x0, 0x10(%rsp)
jne 0x35327
movq 0x40(%rsp), %rdi
callq 0x13030
movq %rax, 0x10(%rsp)
cmpl $0x0, 0x3c(%rsp)
je 0x35327
xorl %ebx, %ebx
movq 0x10(%rsp), %rax
leaq (%rax,%rbx), %rdi
callq 0x83dc0
addq $0x18, %rbx
cmpq %rbx, 0x70(%rsp)
jne 0x3530e
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 0x83dc2
movq 0x10(%rsp), %rdi
movq %r13, %rsi
callq 0x8412a
cmpl $0x0, 0x34(%rsp)
jle 0x353d7
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 0x83dc2
movq %r15, %rdi
movq %r13, %rsi
callq 0x8412a
incq %rbx
movslq 0x34(%rsp), %rax
addq $0x18, %r15
cmpq %rax, %rbx
jl 0x35379
incl %ebx
jmp 0x353dc
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 0x83dc2
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 0x4ff14
movsd %xmm0, 0x58(%rsp)
leaq 0xfe7c4(%rip), %rax     # 0x133c08
movsd (%rax), %xmm0
movsd %xmm0, 0x50(%rsp)
leaq 0xc8(%rsp), %r12
movq %r12, %rdi
movl %ebx, %esi
movq %r15, %rdx
xorl %ecx, %ecx
callq 0x545c1
movq %r13, %rdi
movq %r12, %rsi
movl %ebx, %edx
movq %r15, %rcx
xorl %r8d, %r8d
leaq 0x90(%rsp), %r9
callq 0x5464b
leaq 0x78(%rsp), %rdi
leaq 0x148(%rsp), %rsi
callq 0x84866
leaq 0xb0(%rsp), %rdi
movq %r13, %rsi
callq 0x84866
movsd 0x90(%rsp), %xmm1
ucomisd 0xa7ed5(%rip), %xmm1    # 0xdd388
movsd 0xa7c8d(%rip), %xmm0    # 0xdd148
movsd %xmm0, 0x20(%rsp)
jbe 0x354eb
movsd 0x98(%rsp), %xmm0
divsd %xmm1, %xmm0
ucomisd 0xa7eb0(%rip), %xmm0    # 0xdd388
jb 0x354e0
sqrtsd %xmm0, %xmm0
jmp 0x354e5
callq 0x133c0
movsd %xmm0, 0x20(%rsp)
movq (%rbp), %rax
movq 0xb8(%rax), %rdi
testq %rdi, %rdi
je 0x35557
movl $0x1, %esi
callq 0x37798
movq %rax, %rbx
testq %rax, %rax
je 0x35553
leaq 0xf8(%rsp), %r15
movq %r15, %rdi
leaq 0xc8(%rsp), %rsi
leaq 0x78(%rsp), %rdx
callq 0x82942
addq $0x28, %rbx
movq %r15, %rdi
movq %rbx, %rsi
callq 0x50127
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x355cd
leaq 0x78(%rsp), %rdi
callq 0x84bc4
jmp 0x355cd
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
leaq 0x160(%rsp), %rbx
movq %rbx, %rdi
callq 0x83dc2
leaq 0xf8(%rsp), %r15
movq %r15, %rdi
movq %rbx, %rsi
leaq 0xe0(%rsp), %rdx
callq 0x8401e
movq %r15, %rdi
leaq 0x78(%rsp), %rsi
callq 0x84ae2
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x355cd
leaq 0x78(%rsp), %rdi
callq 0x84bc4
movq 0x8(%rbp), %rax
movb 0x2f(%rax), %cl
testb $0x40, %cl
jne 0x355ed
movq %rbp, %rdi
leaq 0x78(%rsp), %rsi
callq 0x32348
movq 0x8(%rbp), %rax
movb 0x2f(%rax), %cl
testb $0x1, %cl
jne 0x35606
movq %rbp, %rdi
leaq 0xb0(%rsp), %rsi
callq 0x32376
movq 0x8(%rbp), %rax
movzwl 0x1c(%rax), %eax
xorps %xmm0, %xmm0
cvtsi2ss %eax, %xmm0
mulss 0xa87e7(%rip), %xmm0    # 0xdde00
ucomiss 0xa8fe8(%rip), %xmm0    # 0xde608
jne 0x3564d
jp 0x3564d
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 0x323d0
movq 0x68(%rsp), %rbx
incq %rbx
movslq 0x10(%r14), %rax
cmpq %rax, %rbx
jl 0x35200
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x35671
callq 0x13680
movq 0x18(%rsp), %rbx
testq %rbx, %rbx
je 0x35690
movq %rbx, %rdi
callq 0x35852
movl $0x60, %esi
movq %rbx, %rdi
callq 0x134f0
leaq 0x28(%rsp), %rdi
callq 0xa9d12
addq $0x178, %rsp            # imm = 0x178
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x35704
jmp 0x35704
jmp 0x35704
jmp 0x356f2
jmp 0x35704
movq %rax, %rbx
movl $0x60, %esi
movq 0x18(%rsp), %rdi
callq 0x134f0
jmp 0x356f5
jmp 0x35704
jmp 0x35704
jmp 0x35704
jmp 0x35704
jmp 0x35704
jmp 0x35704
jmp 0x35704
jmp 0x35704
jmp 0x35704
jmp 0x35704
movq %rax, %rbx
movq 0x10(%rsp), %rdi
movq 0x40(%rsp), %rsi
callq 0x139b0
jmp 0x35707
movq %rax, %rbx
leaq 0x118(%rsp), %rdi
callq 0xa9d12
jmp 0x35707
movq %rax, %rbx
leaq 0x28(%rsp), %rdi
callq 0xa9d12
movq %rbx, %rdi
callq 0x138b0
nop