// File: /workspace/github_cpp_makefile_star2_O3/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 $0x1d8, %rsp            # imm = 0x1D8
movq %rdi, %rbx
leaq 0x40(%rsp), %rdi
callq 0xf8b80
movq %rbx, 0x8(%rsp)
movl 0x2c(%rbx), %eax
testl %eax, %eax
jle 0x3a512
xorl %edx, %edx
leaq 0x40(%rsp), %rbx
movq 0x8(%rsp), %rcx
movq 0x20(%rcx), %rcx
movq (%rcx,%rdx,8), %rsi
movl 0x34(%rsi), %ecx
testl %ecx, %ecx
jle 0x3a503
movq %rdx, 0x20(%rsp)
xorl %edx, %edx
movq %rsi, 0x10(%rsp)
movq 0x28(%rsi), %rax
movq (%rax,%rdx,8), %r13
movl 0x34(%r13), %eax
testl %eax, %eax
jle 0x3a4eb
movq %rdx, 0x18(%rsp)
xorl %ebp, %ebp
movq 0x28(%r13), %rcx
movq (%rcx,%rbp,8), %r14
cmpq $0x0, 0x28(%r14)
je 0x3a4d3
cmpl $0x0, 0x30(%r14)
jle 0x3a4d3
xorl %r15d, %r15d
xorl %r12d, %r12d
movq 0x28(%r14), %rsi
addq %r15, %rsi
movl 0x4c(%rsp), %edx
movq %rbx, %rdi
callq 0xf8c16
incq %r12
movslq 0x30(%r14), %rax
addq $0x50, %r15
cmpq %rax, %r12
jl 0x3a4ac
movl 0x34(%r13), %eax
incq %rbp
movslq %eax, %rcx
cmpq %rcx, %rbp
jl 0x3a490
movq 0x10(%rsp), %rsi
movl 0x34(%rsi), %ecx
movq 0x18(%rsp), %rdx
incq %rdx
movslq %ecx, %rax
cmpq %rax, %rdx
jl 0x3a479
movq 0x8(%rsp), %rax
movl 0x2c(%rax), %eax
movq 0x20(%rsp), %rdx
incq %rdx
movslq %eax, %rcx
cmpq %rcx, %rdx
jl 0x3a455
leaq 0x787(%rip), %rdx       # 0x3aca0
leaq 0x148(%rsp), %rdi
leaq 0x40(%rsp), %rsi
xorl %ecx, %ecx
callq 0x3ad90
movq 0x8(%rsp), %rax
movl 0x2c(%rax), %eax
testl %eax, %eax
jle 0x3ac16
xorl %ecx, %ecx
leaq 0x28(%rsp), %r15
leaq 0x771(%rip), %r12       # 0x3acbc
leaq 0x148(%rsp), %r13
leaq 0x1a0ee6(%rip), %rbp    # 0x1db440
movq %rcx, %rdx
movq 0x8(%rsp), %rcx
movq 0x20(%rcx), %rcx
movq %rdx, 0x50(%rsp)
movq (%rcx,%rdx,8), %rcx
movq %rcx, 0x38(%rsp)
movl 0x34(%rcx), %ecx
testl %ecx, %ecx
jle 0x3abff
xorl %eax, %eax
movq %rax, %rdx
movq 0x38(%rsp), %rax
movq 0x28(%rax), %rax
movq %rdx, 0x58(%rsp)
movq (%rax,%rdx,8), %rdx
movl 0x34(%rdx), %eax
testl %eax, %eax
jle 0x3abe0
xorl %esi, %esi
movq %rdx, 0x60(%rsp)
movq 0x28(%rdx), %rcx
movq (%rcx,%rsi,8), %rbx
cmpq $0x0, 0x28(%rbx)
je 0x3abc9
movq %rsi, 0x68(%rsp)
leaq 0x40(%rbx), %rsi
leaq 0x130(%rsp), %rdi
callq 0xd43ee
cmpl $0x0, 0x30(%rbx)
jle 0x3abbc
xorl %ecx, %ecx
movq %rbx, 0x70(%rsp)
movq 0x28(%rbx), %rax
movq %rcx, 0x78(%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 0xb8(%rsp), %rdi
callq 0xd4e72
movsd 0xb8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x3aba2
jp 0x3aba2
movsd 0xc0(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3aba2
jp 0x3aba2
movsd 0xc8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3aba2
jp 0x3aba2
addq %rbx, %r14
movsd (%r14), %xmm0
movsd 0x8(%r14), %xmm1
movq %r14, (%rsp)
movsd 0x10(%r14), %xmm2
leaq 0x1c0(%rsp), %rbx
movq %rbx, %rdi
callq 0xd43de
leaq 0x118(%rsp), %rdi
movq %rbx, %rsi
leaq 0x130(%rsp), %rdx
callq 0xd463a
xorl %eax, %eax
leaq 0xb8(%rsp), %r14
movl %eax, 0x20(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
callq 0x27850
mulsd 0x1119e2(%rip), %xmm0   # 0x14c0b0
movsd %xmm0, 0x10(%rsp)
movq %r15, %rdi
callq 0xf8b80
xorpd %xmm0, %xmm0
movq %r13, %rdi
movq (%rsp), %rsi
movsd 0x10(%rsp), %xmm1
movq %r12, %rdx
xorl %ecx, %ecx
movq %r15, %r8
callq 0x3aec8
testl %eax, %eax
je 0x3a7ad
cmpl $0x3, 0x34(%rsp)
jl 0x3a7ad
movb 0x1a102d(%rip), %al     # 0x1db740
testb %al, %al
je 0x3a87d
movl 0x34(%rsp), %ebx
testl %ebx, %ebx
jle 0x3a7ba
shrl $0x5, %ebx
leaq 0x8(,%rbx,8), %rax
movq %rax, 0x18(%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 0xb8(%rsp), %rbp
movq %rbp, %rdi
callq 0xd43de
movq %r13, %rdi
movq %rbp, %rsi
callq 0xd4746
leaq 0x1(%r15), %rbp
cmpl %r12d, 0x34(%rsp)
jle 0x3a797
addq $0x18, %r13
addq 0x18(%rsp), %r14
addl %ebx, %r12d
cmpq $0x1e, %r15
movq %rbp, %r15
jbe 0x3a745
leaq 0x28(%rsp), %r15
leaq 0x519(%rip), %r12       # 0x3acbc
leaq 0x148(%rsp), %r13
jmp 0x3a7bc
movq %r15, %rdi
callq 0xf8bea
jmp 0x3a869
xorl %ebp, %ebp
leaq 0x1a8(%rsp), %r14
movq %r14, %rdi
movl %ebp, %esi
leaq 0x1a0c70(%rip), %rbx    # 0x1db440
movq %rbx, %rdx
xorl %ecx, %ecx
callq 0xa0821
leaq 0xb8(%rsp), %rdi
movq %r14, %rsi
movl %ebp, %edx
movq %rbx, %rcx
xorl %r8d, %r8d
leaq 0xa0(%rsp), %r9
callq 0xa08ab
movq %rbx, %rbp
leaq 0x80(%rsp), %rbx
movq %rbx, %rdi
leaq 0xe8(%rsp), %rsi
callq 0xd4e82
movq %rbx, %rdi
callq 0xd5188
movsd 0x10(%rsp), %xmm0
movsd 0x11187d(%rip), %xmm1   # 0x14c0a8
mulsd %xmm1, %xmm0
minsd %xmm0, %xmm1
movsd 0x11180d(%rip), %xmm0   # 0x14c048
maxsd %xmm1, %xmm0
mulsd %xmm0, %xmm0
ucomisd 0xa0(%rsp), %xmm0
ja 0x3a859
ucomisd 0xa8(%rsp), %xmm0
jbe 0x3a8ba
movq %r15, %rdi
callq 0xf8bea
leaq 0xb8(%rsp), %r14
movl 0x20(%rsp), %eax
incl %eax
cmpl $0x5, %eax
jne 0x3a6b6
jmp 0x3aaae
leaq 0x1a0ebc(%rip), %rdi    # 0x1db740
callq 0x281c0
testl %eax, %eax
je 0x3a71b
xorl %ebx, %ebx
leaq (%rbx,%rbp), %rdi
callq 0xd43dc
addq $0x18, %rbx
cmpq $0x300, %rbx            # imm = 0x300
jne 0x3a893
leaq 0x1a0e90(%rip), %rdi    # 0x1db740
callq 0x275f0
jmp 0x3a71b
leaq 0x80(%rsp), %rdi
leaq 0x118(%rsp), %rsi
callq 0xd50fe
leaq 0xb8(%rsp), %r14
ucomisd 0x111761(%rip), %xmm0   # 0x14c040
jbe 0x3a8ee
leaq 0x80(%rsp), %rdi
callq 0xd51e0
cvtpd2ps 0x80(%rsp), %xmm0
movq (%rsp), %rax
movlpd %xmm0, 0x20(%rax)
xorps %xmm0, %xmm0
cvtsd2ss 0x90(%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 0x100(%rsp), %rbx
movq %rbx, %rdi
callq 0xd4e72
movq %rbx, %rdi
leaq 0x11c428(%rip), %rsi    # 0x156d68
callq 0xd5124
testl %eax, %eax
je 0x3a987
leaq 0x100(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
callq 0xd4e82
movq %rbx, %rdi
callq 0xd5188
cvtpd2ps 0x100(%rsp), %xmm0
movq (%rsp), %rax
movlpd %xmm0, 0x2c(%rax)
xorps %xmm0, %xmm0
cvtsd2ss 0x110(%rsp), %xmm0
movss %xmm0, 0x34(%rax)
movq (%rsp), %rax
movzwl 0x40(%rax), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd 0x11250a(%rip), %xmm0   # 0x14cea8
ucomisd 0x11169a(%rip), %xmm0   # 0x14c040
jne 0x3a9aa
jnp 0x3a9d5
movq (%rsp), %rax
movzwl 0x42(%rax), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd 0x1124e7(%rip), %xmm0   # 0x14cea8
ucomisd 0x111677(%rip), %xmm0   # 0x14c040
jne 0x3aaa6
jp 0x3aaa6
movsd 0x10(%rsp), %xmm1
mulsd 0x1116cd(%rip), %xmm1   # 0x14c0b0
xorpd %xmm0, %xmm0
ucomisd %xmm1, %xmm0
movsd 0x11162d(%rip), %xmm3   # 0x14c020
jae 0x3aa0b
ucomisd 0x11167b(%rip), %xmm1   # 0x14c078
movapd %xmm1, %xmm3
jbe 0x3aa0b
movsd 0x11166d(%rip), %xmm3   # 0x14c078
movsd 0xa8(%rsp), %xmm0
divsd 0xa0(%rsp), %xmm0
ucomisd 0x11161b(%rip), %xmm0   # 0x14c040
jb 0x3aa2d
sqrtsd %xmm0, %xmm0
jmp 0x3aa3e
movsd %xmm3, 0x18(%rsp)
callq 0x27710
movsd 0x18(%rsp), %xmm3
mulsd %xmm3, %xmm0
movsd 0x112466(%rip), %xmm2   # 0x14ceb0
ucomisd %xmm2, %xmm3
mulsd 0x112462(%rip), %xmm3   # 0x14ceb8
addsd 0x1115b2(%rip), %xmm3   # 0x14c010
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 0x3aa9c
movw $0xfa00, %ax            # imm = 0xFA00
ucomisd %xmm2, %xmm0
ja 0x3aa9e
mulsd 0x11242a(%rip), %xmm0   # 0x14ceb8
addsd 0x11157a(%rip), %xmm0   # 0x14c010
cvttsd2si %xmm0, %eax
jmp 0x3aa9e
xorl %eax, %eax
movq (%rsp), %rcx
movw %ax, 0x42(%rcx)
movq %r15, %rdi
callq 0xf8bea
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 0xd4e72
movsd 0xb8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x3aba2
jp 0x3aba2
movsd 0xc0(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3aba2
jp 0x3aba2
movsd 0xc8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x3aba2
jp 0x3aba2
movsd 0x128(%rsp), %xmm0
leaq 0x11c298(%rip), %rsi    # 0x156dc8
leaq 0x8(%rsi), %rax
leaq 0x11c2d5(%rip), %rdi    # 0x156e10
leaq 0x8(%rdi), %rcx
ucomisd %xmm1, %xmm0
movq %rdi, %rdx
cmovaq %rsi, %rdx
leaq 0x10(%rsi), %rsi
xorps %xmm0, %xmm0
cvtsd2ss (%rdx), %xmm0
leaq 0x10(%rdi), %rdx
cmovaq %rax, %rcx
movq (%rsp), %rdi
movss %xmm0, 0x20(%rdi)
xorps %xmm0, %xmm0
cvtsd2ss (%rcx), %xmm0
movss %xmm0, 0x24(%rdi)
cmovaq %rsi, %rdx
leaq 0x11c21b(%rip), %rax    # 0x156d98
movupd 0x8(%rax), %xmm0
movsd (%rdx), %xmm1
movhpd (%rax), %xmm1           # xmm1 = xmm1[0],mem[0]
cvtpd2ps %xmm1, %xmm1
cvtpd2ps %xmm0, %xmm0
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
movupd %xmm1, 0x28(%rdi)
movl $0x640064, 0x40(%rdi)   # imm = 0x640064
movq 0x78(%rsp), %rcx
incq %rcx
movq 0x70(%rsp), %rbx
movslq 0x30(%rbx), %rax
cmpq %rax, %rcx
jl 0x3a5e2
movq 0x60(%rsp), %rdx
movl 0x34(%rdx), %eax
movq 0x68(%rsp), %rsi
incq %rsi
movslq %eax, %rcx
cmpq %rcx, %rsi
jl 0x3a5a8
movq 0x38(%rsp), %rax
movl 0x34(%rax), %ecx
movq 0x58(%rsp), %rdx
incq %rdx
movslq %ecx, %rax
cmpq %rax, %rdx
movq %rdx, %rax
jl 0x3a581
movq 0x8(%rsp), %rax
movl 0x2c(%rax), %eax
movq 0x50(%rsp), %rdx
incq %rdx
movslq %eax, %rcx
cmpq %rcx, %rdx
movq %rdx, %rcx
jl 0x3a55a
leaq 0x148(%rsp), %rdi
callq 0x3af64
leaq 0x40(%rsp), %rdi
callq 0xf8bea
addq $0x1d8, %rsp            # imm = 0x1D8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
leaq 0x1a0af7(%rip), %rdi    # 0x1db740
callq 0x275d0
jmp 0x3ac77
jmp 0x3ac6f
jmp 0x3ac68
jmp 0x3ac74
jmp 0x3ac74
jmp 0x3ac74
jmp 0x3ac68
jmp 0x3ac68
jmp 0x3ac68
jmp 0x3ac68
jmp 0x3ac74
jmp 0x3ac74
jmp 0x3ac74
movq %rax, %rbx
jmp 0x3ac81
jmp 0x3ac74
movq %rax, %rbx
jmp 0x3ac8e
movq %rax, %rbx
leaq 0x28(%rsp), %rdi
callq 0xf8bea
leaq 0x148(%rsp), %rdi
callq 0x3af64
leaq 0x40(%rsp), %rdi
callq 0xf8bea
movq %rbx, %rdi
callq 0x28150