// File: /workspace/github_cpp_makefile_star2_O1/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, %r14
leaq 0x38(%rsp), %rdi
callq 0xfa7cc
cmpl $0x0, 0x2c(%r14)
movq %r14, 0x18(%rsp)
jle 0x3a820
xorl %ecx, %ecx
leaq 0x38(%rsp), %rbx
movq 0x20(%r14), %rax
movq %rcx, 0x20(%rsp)
movq (%rax,%rcx,8), %rcx
cmpl $0x0, 0x34(%rcx)
jle 0x3a806
xorl %edx, %edx
movq %rcx, 0x8(%rsp)
movq 0x28(%rcx), %rax
movq %rdx, 0x10(%rsp)
movq (%rax,%rdx,8), %r13
cmpl $0x0, 0x34(%r13)
jle 0x3a7f0
xorl %ebp, %ebp
movq 0x28(%r13), %rax
movq (%rax,%rbp,8), %r14
cmpq $0x0, 0x28(%r14)
je 0x3a7e4
cmpl $0x0, 0x30(%r14)
jle 0x3a7e4
xorl %r15d, %r15d
xorl %r12d, %r12d
movq 0x28(%r14), %rsi
addq %r15, %rsi
movl 0x44(%rsp), %edx
movq %rbx, %rdi
callq 0xfa862
incq %r12
movslq 0x30(%r14), %rax
addq $0x50, %r15
cmpq %rax, %r12
jl 0x3a7c1
incq %rbp
movslq 0x34(%r13), %rax
cmpq %rax, %rbp
jl 0x3a7a5
movq 0x10(%rsp), %rdx
incq %rdx
movq 0x8(%rsp), %rcx
movslq 0x34(%rcx), %rax
cmpq %rax, %rdx
jl 0x3a78f
movq 0x20(%rsp), %rcx
incq %rcx
movq 0x18(%rsp), %r14
movslq 0x2c(%r14), %rax
cmpq %rax, %rcx
jl 0x3a775
leaq 0x7b6(%rip), %rdx       # 0x3afdd
leaq 0x168(%rsp), %rdi
leaq 0x38(%rsp), %rsi
xorl %ecx, %ecx
callq 0x3b0ce
movq 0x18(%rsp), %rcx
cmpl $0x0, 0x2c(%rcx)
jle 0x3af53
xorl %eax, %eax
leaq 0x28(%rsp), %r15
leaq 0x7a1(%rip), %r12       # 0x3aff9
leaq 0x168(%rsp), %r13
leaq 0x11e539(%rip), %rdx    # 0x158da0
leaq 0x11e57a(%rip), %rsi    # 0x158de8
leaq 0x8(%rdx), %rdi
movq %rdi, 0x88(%rsp)
leaq 0x8(%rsi), %rdi
movq %rdi, 0x80(%rsp)
addq $0x10, %rdx
movq %rdx, 0x78(%rsp)
leaq 0x10(%rsi), %rdx
movq %rdx, 0x70(%rsp)
leaq 0x1a2bb1(%rip), %rbp    # 0x1dd450
movq %rax, %rdx
movq 0x20(%rcx), %rax
movq %rdx, 0x48(%rsp)
movq (%rax,%rdx,8), %rax
cmpl $0x0, 0x34(%rax)
jle 0x3af3b
xorl %esi, %esi
movq %rax, 0x50(%rsp)
movq 0x28(%rax), %rax
movq (%rax,%rsi,8), %rdx
cmpl $0x0, 0x34(%rdx)
jle 0x3af23
xorl %edi, %edi
movq %rsi, 0x60(%rsp)
movq %rdx, 0x58(%rsp)
movq 0x28(%rdx), %rax
movq (%rax,%rdi,8), %rbx
cmpq $0x0, 0x28(%rbx)
je 0x3af13
movq %rdi, 0x68(%rsp)
leaq 0x40(%rbx), %rsi
leaq 0x150(%rsp), %rdi
callq 0xd5e3e
cmpl $0x0, 0x30(%rbx)
jle 0x3aeff
xorl %ecx, %ecx
movq %rbx, 0x90(%rsp)
movq 0x28(%rbx), %rax
movq %rcx, 0x98(%rsp)
leaq (%rcx,%rcx,4), %rbx
shlq $0x4, %rbx
xorps %xmm0, %xmm0
cvtss2sd 0x20(%rax,%rbx), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x24(%rax,%rbx), %xmm1
movq %rax, %r14
xorps %xmm2, %xmm2
cvtss2sd 0x28(%rax,%rbx), %xmm2
leaq 0xd8(%rsp), %rdi
callq 0xd68c6
movsd 0xd8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
leaq 0x1e0(%rsp), %rax
jne 0x3aedf
jp 0x3aedf
movsd 0xe0(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3aedf
jp 0x3aedf
movsd 0xe8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3aedf
jp 0x3aedf
addq %rbx, %r14
movsd (%r14), %xmm0
movsd 0x8(%r14), %xmm1
movq %r14, (%rsp)
movsd 0x10(%r14), %xmm2
movq %rax, %rbx
movq %rax, %rdi
callq 0xd5e2e
leaq 0x138(%rsp), %rdi
movq %rbx, %rsi
leaq 0x150(%rsp), %rdx
callq 0xd6090
xorl %eax, %eax
leaq 0xd8(%rsp), %r14
movl %eax, 0x20(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
callq 0x27850
mulsd 0x1136b3(%rip), %xmm0   # 0x14e0c0
movsd %xmm0, 0x8(%rsp)
movq %r15, %rdi
callq 0xfa7cc
xorpd %xmm0, %xmm0
movq %r13, %rdi
movq (%rsp), %rsi
movsd 0x8(%rsp), %xmm1
movq %r12, %rdx
xorl %ecx, %ecx
movq %r15, %r8
callq 0x3b20a
testl %eax, %eax
je 0x3aaec
cmpl $0x3, 0x34(%rsp)
jl 0x3aaec
movb 0x1a2cfe(%rip), %al     # 0x1dd750
testb %al, %al
je 0x3abbc
movl 0x34(%rsp), %ebx
testl %ebx, %ebx
jle 0x3aaf9
shrl $0x5, %ebx
leaq 0x8(,%rbx,8), %rax
movq %rax, 0x10(%rsp)
incl %ebx
movl %ebx, %r12d
xorl %r14d, %r14d
movq %rbp, %r13
xorl %r15d, %r15d
movq 0x28(%rsp), %rax
movq (%rax,%r14), %rax
movsd (%rax), %xmm0
movsd 0x8(%rax), %xmm1
movsd 0x10(%rax), %xmm2
leaq 0xd8(%rsp), %rbp
movq %rbp, %rdi
callq 0xd5e2e
movq %r13, %rdi
movq %rbp, %rsi
callq 0xd619c
leaq 0x1(%r15), %rbp
cmpl %r12d, 0x34(%rsp)
jle 0x3aad6
addq $0x18, %r13
addq 0x10(%rsp), %r14
addl %ebx, %r12d
cmpq $0x1e, %r15
movq %rbp, %r15
jbe 0x3aa84
leaq 0x28(%rsp), %r15
leaq 0x517(%rip), %r12       # 0x3aff9
leaq 0x168(%rsp), %r13
jmp 0x3aafb
movq %r15, %rdi
callq 0xfa836
jmp 0x3aba8
xorl %ebp, %ebp
leaq 0x1c8(%rsp), %r14
movq %r14, %rdi
movl %ebp, %esi
leaq 0x1a2941(%rip), %rbx    # 0x1dd450
movq %rbx, %rdx
xorl %ecx, %ecx
callq 0xa1c89
leaq 0xd8(%rsp), %rdi
movq %r14, %rsi
movl %ebp, %edx
movq %rbx, %rcx
xorl %r8d, %r8d
leaq 0xc0(%rsp), %r9
callq 0xa1d13
movq %rbx, %rbp
leaq 0xa0(%rsp), %rbx
movq %rbx, %rdi
leaq 0x108(%rsp), %rsi
callq 0xd68d6
movq %rbx, %rdi
callq 0xd6be8
movsd 0x8(%rsp), %xmm0
movsd 0x11354e(%rip), %xmm1   # 0x14e0b8
mulsd %xmm1, %xmm0
minsd %xmm0, %xmm1
movsd 0x1134ce(%rip), %xmm0   # 0x14e048
maxsd %xmm1, %xmm0
mulsd %xmm0, %xmm0
ucomisd 0xc0(%rsp), %xmm0
ja 0x3ab98
ucomisd 0xc8(%rsp), %xmm0
jbe 0x3abf9
movq %r15, %rdi
callq 0xfa836
leaq 0xd8(%rsp), %r14
movl 0x20(%rsp), %eax
incl %eax
cmpl $0x5, %eax
jne 0x3a9f5
jmp 0x3aded
leaq 0x1a2b8d(%rip), %rdi    # 0x1dd750
callq 0x281d0
testl %eax, %eax
je 0x3aa5a
xorl %ebx, %ebx
leaq (%rbx,%rbp), %rdi
callq 0xd5e2c
addq $0x18, %rbx
cmpq $0x300, %rbx            # imm = 0x300
jne 0x3abd2
leaq 0x1a2b61(%rip), %rdi    # 0x1dd750
callq 0x275f0
jmp 0x3aa5a
leaq 0xa0(%rsp), %rdi
leaq 0x138(%rsp), %rsi
callq 0xd6b5e
leaq 0xd8(%rsp), %r14
ucomisd 0x113422(%rip), %xmm0   # 0x14e040
jbe 0x3ac2d
leaq 0xa0(%rsp), %rdi
callq 0xd6c54
cvtpd2ps 0xa0(%rsp), %xmm0
movq (%rsp), %rax
movlpd %xmm0, 0x20(%rax)
xorps %xmm0, %xmm0
cvtsd2ss 0xb0(%rsp), %xmm0
movss %xmm0, 0x28(%rax)
xorps %xmm0, %xmm0
cvtss2sd 0x2c(%rax), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x30(%rax), %xmm1
cvtss2sd 0x34(%rax), %xmm2
leaq 0x120(%rsp), %rbx
movq %rbx, %rdi
callq 0xd68c6
movq %rbx, %rdi
leaq 0x11e0c1(%rip), %rsi    # 0x158d40
callq 0xd6b84
testl %eax, %eax
je 0x3acc6
leaq 0x120(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
callq 0xd68d6
movq %rbx, %rdi
callq 0xd6be8
cvtpd2ps 0x120(%rsp), %xmm0
movq (%rsp), %rax
movlpd %xmm0, 0x2c(%rax)
xorps %xmm0, %xmm0
cvtsd2ss 0x130(%rsp), %xmm0
movss %xmm0, 0x34(%rax)
movq (%rsp), %rax
movzwl 0x40(%rax), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd 0x1141cb(%rip), %xmm0   # 0x14eea8
ucomisd 0x11335b(%rip), %xmm0   # 0x14e040
jne 0x3ace9
jnp 0x3ad14
movq (%rsp), %rax
movzwl 0x42(%rax), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd 0x1141a8(%rip), %xmm0   # 0x14eea8
ucomisd 0x113338(%rip), %xmm0   # 0x14e040
jne 0x3ade5
jp 0x3ade5
movsd 0x8(%rsp), %xmm1
mulsd 0x11339e(%rip), %xmm1   # 0x14e0c0
xorpd %xmm0, %xmm0
ucomisd %xmm1, %xmm0
movsd 0x1132ee(%rip), %xmm3   # 0x14e020
jae 0x3ad4a
ucomisd 0x11334c(%rip), %xmm1   # 0x14e088
movapd %xmm1, %xmm3
jbe 0x3ad4a
movsd 0x11333e(%rip), %xmm3   # 0x14e088
movsd 0xc8(%rsp), %xmm0
divsd 0xc0(%rsp), %xmm0
ucomisd 0x1132dc(%rip), %xmm0   # 0x14e040
jb 0x3ad6c
sqrtsd %xmm0, %xmm0
jmp 0x3ad7d
movsd %xmm3, 0x10(%rsp)
callq 0x27710
movsd 0x10(%rsp), %xmm3
mulsd %xmm3, %xmm0
movsd 0x114127(%rip), %xmm2   # 0x14eeb0
ucomisd %xmm2, %xmm3
mulsd 0x114123(%rip), %xmm3   # 0x14eeb8
addsd 0x113273(%rip), %xmm3   # 0x14e010
cvttsd2si %xmm3, %eax
movl $0xfa00, %ecx           # imm = 0xFA00
cmoval %ecx, %eax
movq (%rsp), %rcx
movw %ax, 0x40(%rcx)
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jae 0x3addb
movw $0xfa00, %ax            # imm = 0xFA00
ucomisd %xmm2, %xmm0
ja 0x3addd
mulsd 0x1140eb(%rip), %xmm0   # 0x14eeb8
addsd 0x11323b(%rip), %xmm0   # 0x14e010
cvttsd2si %xmm0, %eax
jmp 0x3addd
xorl %eax, %eax
movq (%rsp), %rcx
movw %ax, 0x42(%rcx)
movq %r15, %rdi
callq 0xfa836
movq (%rsp), %rax
xorps %xmm0, %xmm0
cvtss2sd 0x20(%rax), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x24(%rax), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x28(%rax), %xmm2
movq %r14, %rdi
callq 0xd68c6
movsd 0xd8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x3aedf
jp 0x3aedf
movsd 0xe0(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3aedf
jp 0x3aedf
movsd 0xe8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3aedf
jp 0x3aedf
movsd 0x148(%rsp), %xmm0
ucomisd %xmm1, %xmm0
leaq 0x11df75(%rip), %rax    # 0x158de8
cmovaq 0x19c125(%rip), %rax    # 0x1d6fa0
xorps %xmm0, %xmm0
cvtsd2ss (%rax), %xmm0
movq (%rsp), %rdx
movss %xmm0, 0x20(%rdx)
movq 0x80(%rsp), %rax
cmovaq 0x88(%rsp), %rax
xorps %xmm0, %xmm0
cvtsd2ss (%rax), %xmm0
movss %xmm0, 0x24(%rdx)
movq 0x70(%rsp), %rax
cmovaq 0x78(%rsp), %rax
leaq 0x11deb6(%rip), %rcx    # 0x158d70
movupd 0x8(%rcx), %xmm0
movsd (%rax), %xmm1
movhpd (%rcx), %xmm1           # xmm1 = xmm1[0],mem[0]
cvtpd2ps %xmm1, %xmm1
cvtpd2ps %xmm0, %xmm0
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
movupd %xmm1, 0x28(%rdx)
movl $0x640064, 0x40(%rdx)   # imm = 0x640064
movq 0x98(%rsp), %rcx
incq %rcx
movq 0x90(%rsp), %rbx
movslq 0x30(%rbx), %rax
cmpq %rax, %rcx
jl 0x3a91b
movq 0x18(%rsp), %rcx
movq 0x60(%rsp), %rsi
movq 0x58(%rsp), %rdx
movq 0x68(%rsp), %rdi
incq %rdi
movslq 0x34(%rdx), %rax
cmpq %rax, %rdi
jl 0x3a8de
incq %rsi
movq 0x50(%rsp), %rdx
movslq 0x34(%rdx), %rax
cmpq %rax, %rsi
movq %rdx, %rax
jl 0x3a8c0
movq 0x48(%rsp), %rdx
incq %rdx
movslq 0x2c(%rcx), %rax
cmpq %rax, %rdx
movq %rdx, %rax
jl 0x3a89f
leaq 0x168(%rsp), %rdi
callq 0x3b2a6
leaq 0x38(%rsp), %rdi
callq 0xfa836
addq $0x1f8, %rsp            # imm = 0x1F8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
leaq 0x1a27ca(%rip), %rdi    # 0x1dd750
callq 0x275d0
jmp 0x3afb4
jmp 0x3afac
jmp 0x3afa5
jmp 0x3afb1
jmp 0x3afb1
jmp 0x3afb1
jmp 0x3afa5
jmp 0x3afa5
jmp 0x3afa5
jmp 0x3afa5
jmp 0x3afb1
jmp 0x3afb1
jmp 0x3afb1
movq %rax, %rbx
jmp 0x3afbe
jmp 0x3afb1
movq %rax, %rbx
jmp 0x3afcb
movq %rax, %rbx
leaq 0x28(%rsp), %rdi
callq 0xfa836
leaq 0x168(%rsp), %rdi
callq 0x3b2a6
leaq 0x38(%rsp), %rdi
callq 0xfa836
movq %rbx, %rdi
callq 0x28160