// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/GSV/GSVSegment.cpp
// Function: gaps::GSVSegment::UpdateNormals()
void GSVSegment::
UpdateNormals(void)
{
  // Parameters
  const int max_iter = 4;
  const double min_radius = 0.25;
  const int max_neighbors = 32;

  // Create kdtree of points
  RNArray<const GSVPoint *> points;
  CreateArrayOfPoints(this, points);
  R3Kdtree<const GSVPoint *> kdtree(points, GSVPointPosition);
  
  // Estimate normals for all points
  for (int iu = 0; iu < NSurveys(); iu++) {
    GSVSurvey *survey = Survey(iu);
    for (int ia = 0; ia < survey->NScans(); ia++) {
      GSVScan *scan = survey->Scan(ia);
      for (int ie = 0; ie < scan->NScanlines(); ie++) {
        GSVScanline *scanline = scan->Scanline(ie);
        if (!scanline->ArePointsResident()) continue;
        R3Point viewpoint = scanline->Pose().Viewpoint();
        for (int ik = 0; ik < scanline->NPoints(); ik++) {
          GSVPoint& point = (GSVPoint&) scanline->Point(ik);
          if (!point.Normal().IsZero()) continue;

          // Get point info
          R3Point position = point.Position();
          R3Vector vector = position - viewpoint;

          // Iterate over increasing max distances
          for (int iter = 0; iter <= max_iter; iter++) {
            // Compute max distance
            double max_distance = min_radius * pow(2, iter);
            double min_stddev = 0.1 * max_distance;
            if (min_stddev > 0.1) min_stddev = 0.1;
            if (min_stddev < 0.05) min_stddev = 0.05;
            double min_variance = min_stddev * min_stddev;
                
            // Find neighbors
            RNArray<const GSVPoint *> neighbors;
            if (!kdtree.FindAll(&point, 0, max_distance,
                                IsGSVPointCompatible, NULL, neighbors)) 
              continue;
              
            // Check if too few neighbors
            if (neighbors.NEntries() < 3) continue;

            // Sample point positions 
            int npositions = 0;
            static R3Point positions[ max_neighbors ];
            int dk = neighbors.NEntries() / max_neighbors + 1;
            for (int k = 0; k < neighbors.NEntries(); k += dk) {
              if (npositions >= max_neighbors) break;
              positions[npositions++] = neighbors[k]->Position();
            }

            // Compute normal from neighbor positions
            RNScalar variances[3];
            R3Point centroid = R3Centroid(npositions, positions);
            R3Triad triad = R3PrincipleAxes(centroid, npositions, positions, NULL, variances);
            R3Vector normal = triad[2];
            normal.Normalize();

            // Check variances
            if (variances[0] < min_variance) continue;
            if (variances[1] < min_variance) continue;

            // Flip to face laser
            if (normal.Dot(vector) > 0) normal.Flip();

            // Update normal
            point.SetNormal(normal);

            // Update tangent
            if (point.Tangent() == R3zero_vector) {
              R3Vector tangent = triad[0];
              tangent.Normalize();
              point.SetTangent(tangent);
            }

            // Update radius
            if ((point.Radius(0) == 0) || (point.Radius(1) == 0)) {
              RNScalar radius0 = 0.25 * max_distance;
              if (radius0 <= 0) radius0 = 0.01;
              else if (radius0 > 1) radius0 = 1;
              RNLength aspect = sqrt(variances[1] / variances[0]);
              RNScalar radius1 = aspect * radius0;
              point.SetRadius(0, radius0);
              point.SetRadius(1, radius1);
            }

            // Set normal for point
            break;
          }
              
          // Set normal to default, if otherwise missed
          if (point.Normal().IsZero()) {
            point.SetNormal((vector.Z() > 0) ? R3posz_vector : R3negz_vector);
            point.SetTangent(R3posx_vector);
            point.SetRadius(0.1);
          }
        }
      }
    }
  }
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1f8, %rsp            # imm = 0x1F8
movq %rdi, 0x20(%rsp)
leaq 0x80(%rsp), %rbx
movq %rbx, %rdi
callq 0xe1cd2
xorl %edx, %edx
movq 0x20(%rsp), %rcx
movslq 0x2c(%rcx), %rax
cmpq %rax, %rdx
jge 0x38330
movq 0x20(%rcx), %rax
movq %rdx, 0x30(%rsp)
movq (%rax,%rdx,8), %rcx
xorl %edx, %edx
movq %rcx, 0x10(%rsp)
movslq 0x34(%rcx), %rax
cmpq %rax, %rdx
jge 0x38323
movq 0x28(%rcx), %rax
movq %rdx, 0x18(%rsp)
movq (%rax,%rdx,8), %r13
xorl %ebp, %ebp
movslq 0x34(%r13), %rax
cmpq %rax, %rbp
jge 0x38314
movq 0x28(%r13), %rax
movq (%rax,%rbp,8), %r14
cmpq $0x0, 0x28(%r14)
je 0x3830f
xorl %r15d, %r15d
xorl %r12d, %r12d
movslq 0x30(%r14), %rax
cmpq %rax, %r12
jge 0x3830f
movq 0x28(%r14), %rsi
addq %r15, %rsi
movq %rbx, %rdi
callq 0x3110a
incq %r12
addq $0x50, %r15
jmp 0x382ee
incq %rbp
jmp 0x382d0
movq 0x18(%rsp), %rdx
incq %rdx
movq 0x10(%rsp), %rcx
jmp 0x382b8
movq 0x30(%rsp), %rdx
incq %rdx
jmp 0x38292
leaq 0x6b5(%rip), %rdx       # 0x389ec
leaq 0x168(%rsp), %rdi
leaq 0x80(%rsp), %rsi
xorl %ecx, %ecx
callq 0x38b62
xorl %ecx, %ecx
leaq 0xf0(%rsp), %r15
leaq 0x38(%rsp), %r12
leaq 0x696(%rip), %r13       # 0x389fa
leaq 0x168(%rsp), %rbp
movq 0x20(%rsp), %rbx
movslq 0x2c(%rbx), %rax
cmpq %rax, %rcx
jge 0x3895c
movq 0x20(%rbx), %rax
movq %rcx, 0x48(%rsp)
movq (%rax,%rcx,8), %rax
xorl %edx, %edx
movq %rax, 0x50(%rsp)
movq %rax, %rcx
movslq 0x34(%rax), %rax
cmpq %rax, %rdx
jge 0x3894f
movq 0x28(%rcx), %rax
movq %rdx, 0x58(%rsp)
movq (%rax,%rdx,8), %rax
xorl %edx, %edx
movq %rax, 0x60(%rsp)
movq %rax, %rcx
movslq 0x34(%rax), %rax
cmpq %rax, %rdx
jge 0x3893d
movq %rdx, 0x28(%rsp)
movq 0x28(%rcx), %rax
movq 0x28(%rsp), %rcx
movq (%rax,%rcx,8), %rcx
movq 0x28(%rsp), %rax
cmpq $0x0, 0x28(%rcx)
je 0x3892d
movq %rcx, 0x68(%rsp)
leaq 0x40(%rcx), %rsi
leaq 0x150(%rsp), %rdi
callq 0xc0e0c
xorl %edx, %edx
movq 0x68(%rsp), %rcx
movslq 0x30(%rcx), %rax
cmpq %rax, %rdx
jge 0x38928
movq %rdx, 0x70(%rsp)
imulq $0x50, %rdx, %rsi
addq 0x28(%rcx), %rsi
movq %r15, %rdi
movq %rsi, 0x8(%rsp)
callq 0x310e4
movsd 0xf0(%rsp), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
leaq 0x1e0(%rsp), %rdi
jne 0x3891b
jp 0x3891b
movsd 0xf8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3891b
jp 0x3891b
movsd 0x100(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3891b
jp 0x3891b
movq %rdi, %r14
movq 0x8(%rsp), %rsi
callq 0x310c8
leaq 0x138(%rsp), %rdi
movq %r14, %rsi
leaq 0x150(%rsp), %rdx
callq 0xc0fd9
xorl %eax, %eax
cmpl $0x5, %eax
je 0x38894
movl %eax, 0x30(%rsp)
cvtsi2sd %eax, %xmm0
callq 0x27860
mulsd 0xf2bc8(%rip), %xmm0    # 0x12b090
movsd %xmm0, 0x10(%rsp)
movq %r12, %rdi
callq 0xe1cd2
xorpd %xmm0, %xmm0
movq %rbp, %rdi
movq 0x8(%rsp), %rsi
movsd 0x10(%rsp), %xmm1
movq %r13, %rdx
xorl %ecx, %ecx
movq %r12, %r8
callq 0x38c7e
testl %eax, %eax
je 0x385ba
cmpl $0x3, 0x44(%rsp)
jl 0x385ba
movb 0x18c252(%rip), %al     # 0x1c4760
testb %al, %al
leaq 0x18bf49(%rip), %r13    # 0x1c4460
je 0x38672
movsd 0x10(%rsp), %xmm0
movsd 0xf2b5d(%rip), %xmm1    # 0x12b088
mulsd %xmm1, %xmm0
minsd %xmm0, %xmm1
movsd 0xf2b05(%rip), %xmm0    # 0x12b040
maxsd %xmm1, %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x78(%rsp)
movl 0x44(%rsp), %ecx
movl %ecx, %eax
cltd
pushq $0x20
popq %rsi
idivl %esi
leal 0x1(%rax), %edx
movslq %edx, %rdx
movq %rdx, 0x18(%rsp)
cltq
leaq 0x8(,%rax,8), %rbp
xorl %r15d, %r15d
xorl %r12d, %r12d
xorl %r14d, %r14d
movslq %ecx, %rax
cmpq %rax, %r12
jge 0x385c7
cmpq $0x1f, %r14
ja 0x385c7
movq 0x38(%rsp), %rax
movq (%rax,%r15), %rsi
leaq 0xf0(%rsp), %rbx
movq %rbx, %rdi
callq 0x310c8
movq %r13, %rdi
movq %rbx, %rsi
callq 0xc10e6
incq %r14
addq 0x18(%rsp), %r12
movl 0x44(%rsp), %ecx
addq %rbp, %r15
addq $0x18, %r13
jmp 0x38573
movq %r12, %rdi
callq 0xe1d40
jmp 0x38667
leaq 0x1c8(%rsp), %r15
movq %r15, %rdi
movl %r14d, %esi
leaq 0x18be84(%rip), %rbx    # 0x1c4460
movq %rbx, %rdx
xorl %ecx, %ecx
callq 0x92678
movq %r15, %rsi
leaq 0xf0(%rsp), %r15
movq %r15, %rdi
movl %r14d, %edx
movq %rbx, %rcx
xorl %r8d, %r8d
leaq 0x90(%rsp), %r9
callq 0x92701
leaq 0xd0(%rsp), %rbx
movq %rbx, %rdi
leaq 0x120(%rsp), %rsi
callq 0xc15cc
movq %rbx, %rdi
callq 0xc18be
movsd 0x78(%rsp), %xmm0
ucomisd 0x90(%rsp), %xmm0
ja 0x38646
ucomisd 0x98(%rsp), %xmm0
jbe 0x386af
leaq 0x38(%rsp), %r12
movq %r12, %rdi
callq 0xe1d40
movq 0x20(%rsp), %rbx
leaq 0x39b(%rip), %r13       # 0x389fa
leaq 0x168(%rsp), %rbp
movl 0x30(%rsp), %eax
incl %eax
jmp 0x384aa
leaq 0x18c0e7(%rip), %rdi    # 0x1c4760
callq 0x281e0
testl %eax, %eax
je 0x3851d
xorl %ebx, %ebx
leaq (%rbx,%r13), %rdi
callq 0xc0dfa
addq $0x18, %rbx
cmpq $0x300, %rbx            # imm = 0x300
jne 0x38688
leaq 0x18c0bb(%rip), %rdi    # 0x1c4760
callq 0x275f0
jmp 0x3851d
movq %rbx, %rdi
leaq 0x138(%rsp), %rsi
callq 0xc1836
movq 0x20(%rsp), %rbx
leaq 0x38(%rsp), %r12
leaq 0x32a(%rip), %r13       # 0x389fa
leaq 0x168(%rsp), %rbp
ucomisd 0xf2a18(%rip), %xmm0    # 0x12b0f8
jbe 0x386ef
leaq 0xd0(%rsp), %rdi
callq 0xc1912
cvtpd2ps 0xd0(%rsp), %xmm0
movq 0x8(%rsp), %rsi
movlpd %xmm0, 0x20(%rsi)
cvtsd2ss 0xe0(%rsp), %xmm0
movss %xmm0, 0x28(%rsi)
leaq 0xb0(%rsp), %r14
movq %r14, %rdi
callq 0x38d32
movq %r14, %rdi
leaq 0xfd56e(%rip), %rsi     # 0x135c98
callq 0xc185c
testl %eax, %eax
je 0x3876f
leaq 0xb0(%rsp), %r14
movq %r14, %rdi
movq %r15, %rsi
callq 0xc15cc
movq %r14, %rdi
callq 0xc18be
cvtpd2ps 0xb0(%rsp), %xmm0
movq 0x8(%rsp), %rax
movlpd %xmm0, 0x2c(%rax)
cvtsd2ss 0xc0(%rsp), %xmm0
movss %xmm0, 0x34(%rax)
movq 0x8(%rsp), %rax
movzwl 0x40(%rax), %eax
cvtsi2sd %eax, %xmm0
mulsd 0xf3724(%rip), %xmm0    # 0x12bea8
ucomisd 0xf296c(%rip), %xmm0    # 0x12b0f8
jne 0x38790
jnp 0x387b9
movq 0x8(%rsp), %rax
movzwl 0x42(%rax), %eax
cvtsi2sd %eax, %xmm0
mulsd 0xf3703(%rip), %xmm0    # 0x12bea8
ucomisd 0xf294b(%rip), %xmm0    # 0x12b0f8
jne 0x3888c
jp 0x3888c
movsd 0x10(%rsp), %xmm1
mulsd 0xf28c9(%rip), %xmm1    # 0x12b090
xorpd %xmm0, %xmm0
ucomisd %xmm1, %xmm0
movsd 0xf2839(%rip), %xmm3    # 0x12b010
jae 0x387ef
ucomisd 0xf288f(%rip), %xmm1    # 0x12b070
movapd %xmm1, %xmm3
jbe 0x387ef
movsd 0xf2881(%rip), %xmm3    # 0x12b070
movsd 0x98(%rsp), %xmm0
divsd 0x90(%rsp), %xmm0
ucomisd 0xf28ef(%rip), %xmm0    # 0x12b0f8
jb 0x38811
sqrtsd %xmm0, %xmm0
jmp 0x38822
movsd %xmm3, 0x18(%rsp)
callq 0x27710
movsd 0x18(%rsp), %xmm3
mulsd %xmm3, %xmm0
movsd 0xf3682(%rip), %xmm2    # 0x12beb0
ucomisd %xmm2, %xmm3
mulsd 0xf367e(%rip), %xmm3    # 0x12beb8
addsd 0xf27f6(%rip), %xmm3    # 0x12b038
cvttsd2si %xmm3, %eax
movl $0xfa00, %ecx           # imm = 0xFA00
cmoval %ecx, %eax
movq 0x8(%rsp), %rcx
movw %ax, 0x40(%rcx)
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jae 0x38881
movw $0xfa00, %ax            # imm = 0xFA00
ucomisd %xmm2, %xmm0
ja 0x38883
mulsd 0xf3645(%rip), %xmm0    # 0x12beb8
addsd 0xf27bd(%rip), %xmm0    # 0x12b038
cvttsd2si %xmm0, %eax
jmp 0x38883
xorl %eax, %eax
movq 0x8(%rsp), %rcx
movw %ax, 0x42(%rcx)
movq %r12, %rdi
callq 0xe1d40
movq %r15, %rdi
movq 0x8(%rsp), %rsi
callq 0x310e4
movsd 0xf0(%rsp), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x3891b
jp 0x3891b
movsd 0xf8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3891b
jp 0x3891b
movsd 0x100(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3891b
jp 0x3891b
movsd 0x148(%rsp), %xmm0
ucomisd %xmm1, %xmm0
leaq 0xfd454(%rip), %rsi     # 0x135d40
jbe 0x388f5
leaq 0xfd403(%rip), %rsi     # 0x135cf8
movq 0x8(%rsp), %rdi
callq 0x38d1a
movq 0x8(%rsp), %r14
movq %r14, %rdi
leaq 0xfd3ba(%rip), %rsi     # 0x135cc8
callq 0x38d50
movl $0x640064, 0x40(%r14)   # imm = 0x640064
movq 0x70(%rsp), %rdx
incq %rdx
jmp 0x38400
movq 0x28(%rsp), %rax
incq %rax
movq %rax, %rdx
movq 0x60(%rsp), %rax
jmp 0x383b6
movq 0x58(%rsp), %rdx
incq %rdx
movq 0x50(%rsp), %rax
jmp 0x38392
movq 0x48(%rsp), %rcx
incq %rcx
jmp 0x38371
leaq 0x168(%rsp), %rdi
callq 0x38d68
leaq 0x80(%rsp), %rdi
callq 0xe1d40
addq $0x1f8, %rsp            # imm = 0x1F8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
leaq 0x18bdce(%rip), %rdi    # 0x1c4760
callq 0x275d0
jmp 0x389c0
jmp 0x389b8
jmp 0x389b1
jmp 0x389bd
jmp 0x389bd
jmp 0x389bd
jmp 0x389b1
jmp 0x389b1
jmp 0x389b1
jmp 0x389b1
jmp 0x389bd
jmp 0x389bd
jmp 0x389bd
movq %rax, %rbx
jmp 0x389ca
jmp 0x389bd
movq %rax, %rbx
jmp 0x389d7
movq %rax, %rbx
leaq 0x38(%rsp), %rdi
callq 0xe1d40
leaq 0x168(%rsp), %rdi
callq 0x38d68
leaq 0x80(%rsp), %rdi
callq 0xe1d40
movq %rbx, %rdi
callq 0x28170