// File: 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:
subq $0x388, %rsp            # imm = 0x388
movq %rdi, 0x380(%rsp)
movq 0x380(%rsp), %rax
movq %rax, 0xe8(%rsp)
movl $0x4, 0x37c(%rsp)
movabsq $0x3fd0000000000000, %rax # imm = 0x3FD0000000000000
movq %rax, 0x370(%rsp)
movl $0x20, 0x36c(%rsp)
leaq 0x358(%rsp), %rdi
movq %rdi, 0xf0(%rsp)
callq 0x4a8d0
movq 0xe8(%rsp), %rdi
movq 0xf0(%rsp), %rsi
callq 0x4a260
jmp 0x497a3
leaq 0xc26(%rip), %rdx       # 0x4a3d0
xorl %eax, %eax
movl %eax, %ecx
leaq 0x2e8(%rsp), %rdi
leaq 0x358(%rsp), %rsi
callq 0x4a8e0
jmp 0x497c5
movl $0x0, 0x2e4(%rsp)
movq 0xe8(%rsp), %rdi
movl 0x2e4(%rsp), %eax
movl %eax, 0xe0(%rsp)
callq 0x3a030
movl %eax, 0xe4(%rsp)
jmp 0x497f4
movl 0xe0(%rsp), %eax
movl 0xe4(%rsp), %ecx
cmpl %ecx, %eax
jge 0x4a216
movq 0xe8(%rsp), %rdi
movl 0x2e4(%rsp), %esi
callq 0x3a050
movq %rax, 0xd8(%rsp)
jmp 0x49828
movq 0xd8(%rsp), %rax
movq %rax, 0x2d8(%rsp)
movl $0x0, 0x2d4(%rsp)
movl 0x2d4(%rsp), %eax
movl %eax, 0xd0(%rsp)
movq 0x2d8(%rsp), %rdi
callq 0x3a080
movl %eax, 0xd4(%rsp)
jmp 0x49867
movl 0xd0(%rsp), %eax
movl 0xd4(%rsp), %ecx
cmpl %ecx, %eax
jge 0x4a1fe
movq 0x2d8(%rsp), %rdi
movl 0x2d4(%rsp), %esi
callq 0x3a0a0
movq %rax, 0xc8(%rsp)
jmp 0x4989b
movq 0xc8(%rsp), %rax
movq %rax, 0x2c8(%rsp)
movl $0x0, 0x2c4(%rsp)
movl 0x2c4(%rsp), %eax
movl %eax, 0xc0(%rsp)
movq 0x2c8(%rsp), %rdi
callq 0x3a0d0
movl %eax, 0xc4(%rsp)
jmp 0x498da
movl 0xc0(%rsp), %eax
movl 0xc4(%rsp), %ecx
cmpl %ecx, %eax
jge 0x4a1e6
movq 0x2c8(%rsp), %rdi
movl 0x2c4(%rsp), %esi
callq 0x3a0f0
movq %rax, 0xb8(%rsp)
jmp 0x4990e
movq 0xb8(%rsp), %rax
movq %rax, 0x2b8(%rsp)
movq 0x2b8(%rsp), %rdi
callq 0x4ab90
movl %eax, 0xb4(%rsp)
jmp 0x49934
movl 0xb4(%rsp), %eax
cmpl $0x0, %eax
jne 0x49977
jmp 0x4a1d0
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x350(%rsp)
movl %eax, 0x34c(%rsp)
jmp 0x4a245
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x350(%rsp)
movl %eax, 0x34c(%rsp)
jmp 0x4a238
movq 0x2b8(%rsp), %rdi
callq 0x44c60
movq %rax, 0xa8(%rsp)
jmp 0x4998e
movq 0xa8(%rsp), %rdi
callq 0x3a330
movq %rax, 0xa0(%rsp)
jmp 0x499a5
movq 0xa0(%rsp), %rsi
leaq 0x2a0(%rsp), %rdi
callq 0x17fdd0
jmp 0x499bc
movl $0x0, 0x29c(%rsp)
movl 0x29c(%rsp), %eax
movl %eax, 0x98(%rsp)
movq 0x2b8(%rsp), %rdi
callq 0x3a120
movl %eax, 0x9c(%rsp)
jmp 0x499eb
movl 0x98(%rsp), %eax
movl 0x9c(%rsp), %ecx
cmpl %ecx, %eax
jge 0x4a1ce
movq 0x2b8(%rsp), %rdi
movl 0x29c(%rsp), %esi
callq 0x3a130
movq %rax, 0x90(%rsp)
jmp 0x49a1f
movq 0x90(%rsp), %rax
movq %rax, 0x290(%rsp)
movq 0x290(%rsp), %rsi
leaq 0x278(%rsp), %rdi
callq 0x3a240
jmp 0x49a46
leaq 0x278(%rsp), %rdi
callq 0x39e10
movl %eax, 0x8c(%rsp)
jmp 0x49a5c
movl 0x8c(%rsp), %eax
cmpl $0x0, %eax
setne %al
xorb $-0x1, %al
testb $0x1, %al
jne 0x49a71
jmp 0x49a76
jmp 0x4a1b8
movq 0x290(%rsp), %rsi
leaq 0x260(%rsp), %rdi
callq 0x3a200
jmp 0x49a8d
leaq 0x248(%rsp), %rdi
leaq 0x260(%rsp), %rsi
leaq 0x2a0(%rsp), %rdx
callq 0x1800f0
jmp 0x49aac
movl $0x0, 0x244(%rsp)
cmpl $0x4, 0x244(%rsp)
jg 0x4a0f5
movl 0x244(%rsp), %esi
movl $0x2, %edi
callq 0x4abb0
movsd %xmm0, 0x80(%rsp)
jmp 0x49ae1
movsd 0x80(%rsp), %xmm1
movsd 0x21c5b6(%rip), %xmm0   # 0x2660a8
mulsd %xmm1, %xmm0
movsd %xmm0, 0x238(%rsp)
movsd 0x21c599(%rip), %xmm0   # 0x2660a0
mulsd 0x238(%rsp), %xmm0
movsd %xmm0, 0x230(%rsp)
movsd 0x230(%rsp), %xmm0
movsd 0x21c576(%rip), %xmm1   # 0x2660a0
ucomisd %xmm1, %xmm0
jbe 0x49b41
movsd 0x21c568(%rip), %xmm0   # 0x2660a0
movsd %xmm0, 0x230(%rsp)
movsd 0x21c537(%rip), %xmm0   # 0x266080
ucomisd 0x230(%rsp), %xmm0
jbe 0x49b65
movsd 0x21c524(%rip), %xmm0   # 0x266080
movsd %xmm0, 0x230(%rsp)
movsd 0x230(%rsp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x228(%rsp)
leaq 0x218(%rsp), %rdi
callq 0x4a8d0
jmp 0x49b8a
movq 0x290(%rsp), %rsi
movsd 0x238(%rsp), %xmm1
leaq 0x85e(%rip), %rdx       # 0x4a400
xorl %eax, %eax
movl %eax, %ecx
leaq 0x2e8(%rsp), %rdi
xorps %xmm0, %xmm0
leaq 0x218(%rsp), %r8
callq 0x4abd0
movl %eax, 0x7c(%rsp)
jmp 0x49bc4
movl 0x7c(%rsp), %eax
cmpl $0x0, %eax
jne 0x49bf6
movl $0x10, 0x214(%rsp)
jmp 0x4a0b0
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x350(%rsp)
movl %eax, 0x34c(%rsp)
jmp 0x4a0cd
leaq 0x218(%rsp), %rdi
callq 0x38730
movl %eax, 0x78(%rsp)
jmp 0x49c09
movl 0x78(%rsp), %eax
cmpl $0x3, %eax
jge 0x49c22
movl $0x10, 0x214(%rsp)
jmp 0x4a0b0
movl $0x0, 0x210(%rsp)
cmpb $0x0, 0x2b564c(%rip)    # 0x2ff280
jne 0x49c93
leaq 0x2b5643(%rip), %rdi    # 0x2ff280
callq 0x296a0
cmpl $0x0, %eax
je 0x49c93
leaq 0x2b5332(%rip), %rax    # 0x2fef80
movq %rax, 0x70(%rsp)
jmp 0x49c55
movq 0x70(%rsp), %rdi
movq %rdi, 0x68(%rsp)
callq 0x17fd80
jmp 0x49c66
movq 0x68(%rsp), %rax
addq $0x18, %rax
leaq 0x2b530a(%rip), %rcx    # 0x2fef80
addq $0x300, %rcx            # imm = 0x300
cmpq %rcx, %rax
movq %rax, 0x70(%rsp)
jne 0x49c55
leaq 0x2b55f2(%rip), %rdi    # 0x2ff280
callq 0x29a40
leaq 0x218(%rsp), %rdi
callq 0x38730
movl %eax, 0x64(%rsp)
jmp 0x49ca6
movl 0x64(%rsp), %eax
movl $0x20, %ecx
cltd
idivl %ecx
addl $0x1, %eax
movl %eax, 0x20c(%rsp)
movl $0x0, 0x208(%rsp)
movl 0x208(%rsp), %eax
movl %eax, 0x5c(%rsp)
leaq 0x218(%rsp), %rdi
callq 0x38730
movl %eax, 0x60(%rsp)
jmp 0x49ce5
movl 0x5c(%rsp), %eax
movl 0x60(%rsp), %ecx
cmpl %ecx, %eax
jge 0x49da8
cmpl $0x20, 0x210(%rsp)
jl 0x49d29
jmp 0x49da8
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x350(%rsp)
movl %eax, 0x34c(%rsp)
leaq 0x2b5561(%rip), %rdi    # 0x2ff280
callq 0x297c0
jmp 0x4a0cd
movl 0x208(%rsp), %esi
leaq 0x218(%rsp), %rdi
callq 0x4ad10
movq %rax, 0x50(%rsp)
jmp 0x49d44
movq 0x50(%rsp), %rax
movq (%rax), %rsi
leaq 0x1f0(%rsp), %rdi
callq 0x3a200
jmp 0x49d5b
movslq 0x210(%rsp), %rax
movl %eax, %ecx
incl %ecx
movl %ecx, 0x210(%rsp)
leaq (%rax,%rax,2), %rcx
leaq 0x2b5207(%rip), %rax    # 0x2fef80
leaq (%rax,%rcx,8), %rdi
leaq 0x1f0(%rsp), %rsi
callq 0x180420
jmp 0x49d8c
jmp 0x49d8e
movl 0x20c(%rsp), %eax
addl 0x208(%rsp), %eax
movl %eax, 0x208(%rsp)
jmp 0x49cc7
movl 0x210(%rsp), %esi
leaq 0x2b51ca(%rip), %rdx    # 0x2fef80
xorl %eax, %eax
movl %eax, %ecx
leaq 0x1b8(%rsp), %rdi
callq 0x1172c0
jmp 0x49dc9
movl 0x210(%rsp), %edx
leaq 0x2b51a9(%rip), %rcx    # 0x2fef80
xorl %eax, %eax
movl %eax, %r8d
leaq 0x170(%rsp), %rdi
leaq 0x1b8(%rsp), %rsi
leaq 0x1d0(%rsp), %r9
callq 0x117380
jmp 0x49dfb
leaq 0x170(%rsp), %rdi
movl $0x2, %esi
callq 0x4ad30
movq %rax, 0x48(%rsp)
jmp 0x49e14
movq 0x48(%rsp), %rsi
leaq 0x158(%rsp), %rdi
callq 0x180f30
jmp 0x49e28
leaq 0x158(%rsp), %rdi
callq 0x181650
jmp 0x49e37
movsd 0x1d0(%rsp), %xmm1
movsd 0x228(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x49e5f
movl $0x10, 0x214(%rsp)
jmp 0x4a0b0
movsd 0x1d8(%rsp), %xmm1
movsd 0x228(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x49e87
movl $0x10, 0x214(%rsp)
jmp 0x4a0b0
leaq 0x158(%rsp), %rdi
leaq 0x248(%rsp), %rsi
callq 0x181500
movsd %xmm0, 0x40(%rsp)
jmp 0x49ea4
movsd 0x40(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x49ec4
leaq 0x158(%rsp), %rdi
callq 0x1816d0
jmp 0x49ec2
jmp 0x49ec4
movq 0x290(%rsp), %rdi
leaq 0x158(%rsp), %rsi
callq 0x4ad50
jmp 0x49edb
movq 0x290(%rsp), %rsi
leaq 0x140(%rsp), %rdi
callq 0x4adc0
jmp 0x49ef2
leaq 0x6b9497(%rip), %rsi    # 0x703390
leaq 0x140(%rsp), %rdi
callq 0x181550
movl %eax, 0x3c(%rsp)
jmp 0x49f0c
movl 0x3c(%rsp), %eax
cmpl $0x0, %eax
je 0x49f67
leaq 0x170(%rsp), %rdi
xorl %esi, %esi
callq 0x4ad30
movq %rax, 0x30(%rsp)
jmp 0x49f2b
movq 0x30(%rsp), %rsi
leaq 0x128(%rsp), %rdi
callq 0x180f30
jmp 0x49f3f
leaq 0x128(%rsp), %rdi
callq 0x181650
jmp 0x49f4e
movq 0x290(%rsp), %rdi
leaq 0x128(%rsp), %rsi
callq 0x4ae10
jmp 0x49f65
jmp 0x49f67
movq 0x290(%rsp), %rdi
xorl %esi, %esi
callq 0x4ae80
movsd %xmm0, 0x28(%rsp)
jmp 0x49f7e
movsd 0x28(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x49f91
jp 0x49f91
jmp 0x49fc4
movq 0x290(%rsp), %rdi
movl $0x1, %esi
callq 0x4ae80
movsd %xmm0, 0x20(%rsp)
jmp 0x49fab
movsd 0x20(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x4a0a5
jp 0x4a0a5
movsd 0x21c0dc(%rip), %xmm0   # 0x2660a8
mulsd 0x238(%rsp), %xmm0
movsd %xmm0, 0x120(%rsp)
xorps %xmm0, %xmm0
ucomisd 0x120(%rsp), %xmm0
jb 0x49fff
movsd 0x21c03c(%rip), %xmm0   # 0x266030
movsd %xmm0, 0x120(%rsp)
jmp 0x4a029
movsd 0x120(%rsp), %xmm0
movsd 0x21c000(%rip), %xmm1   # 0x266010
ucomisd %xmm1, %xmm0
jbe 0x4a027
movsd 0x21bff2(%rip), %xmm0   # 0x266010
movsd %xmm0, 0x120(%rsp)
jmp 0x4a029
movsd 0x1d0(%rsp), %xmm1
movsd 0x1d8(%rsp), %xmm0
divsd %xmm1, %xmm0
callq 0x298d0
movsd %xmm0, 0x118(%rsp)
movsd 0x118(%rsp), %xmm0
movsd 0x120(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x110(%rsp)
movq 0x290(%rsp), %rdi
movsd 0x120(%rsp), %xmm0
xorl %esi, %esi
callq 0x4aeb0
jmp 0x4a086
movq 0x290(%rsp), %rdi
movsd 0x110(%rsp), %xmm0
movl $0x1, %esi
callq 0x4aeb0
jmp 0x4a0a3
jmp 0x4a0a5
movl $0xe, 0x214(%rsp)
leaq 0x218(%rsp), %rdi
callq 0x4af50
movl 0x214(%rsp), %eax
subl $0xe, %eax
je 0x4a0f5
jmp 0x4a0cb
jmp 0x4a0df
leaq 0x218(%rsp), %rdi
callq 0x4af50
jmp 0x4a238
movl 0x244(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x244(%rsp)
jmp 0x49ab7
movq 0x290(%rsp), %rsi
leaq 0xf8(%rsp), %rdi
callq 0x3a240
jmp 0x4a10c
leaq 0xf8(%rsp), %rdi
callq 0x39e10
movl %eax, 0x1c(%rsp)
jmp 0x4a11f
movl 0x1c(%rsp), %eax
cmpl $0x0, %eax
je 0x4a1b6
movq 0x290(%rsp), %rax
movq %rax, 0x8(%rsp)
leaq 0x248(%rsp), %rdi
callq 0x38ca0
movsd %xmm0, 0x10(%rsp)
jmp 0x4a14e
movsd 0x10(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x4a16a
leaq 0x6b928c(%rip), %rax    # 0x7033f0
movq %rax, (%rsp)
jmp 0x4a177
leaq 0x6b92c7(%rip), %rax    # 0x703438
movq %rax, (%rsp)
jmp 0x4a177
movq 0x8(%rsp), %rdi
movq (%rsp), %rsi
callq 0x4ad50
jmp 0x4a187
movq 0x290(%rsp), %rdi
leaq 0x6b922a(%rip), %rsi    # 0x7033c0
callq 0x4ae10
jmp 0x4a19d
movq 0x290(%rsp), %rdi
movsd 0x21bef3(%rip), %xmm0   # 0x2660a0
callq 0x4af60
jmp 0x4a1b4
jmp 0x4a1b6
jmp 0x4a1b8
movl 0x29c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x29c(%rsp)
jmp 0x499c7
jmp 0x4a1d0
movl 0x2c4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x2c4(%rsp)
jmp 0x498b6
jmp 0x4a1e8
movl 0x2d4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x2d4(%rsp)
jmp 0x49843
jmp 0x4a200
movl 0x2e4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x2e4(%rsp)
jmp 0x497d0
leaq 0x2e8(%rsp), %rdi
callq 0x4afa0
leaq 0x358(%rsp), %rdi
callq 0x4af50
addq $0x388, %rsp            # imm = 0x388
retq
leaq 0x2e8(%rsp), %rdi
callq 0x4afa0
leaq 0x358(%rsp), %rdi
callq 0x4af50
movq 0x350(%rsp), %rdi
callq 0x29c10
nop