// File: 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:
subq $0x328, %rsp            # imm = 0x328
movq %rdi, 0x320(%rsp)
movsd %xmm0, 0x318(%rsp)
movl %esi, 0x314(%rsp)
movq 0x320(%rsp), %rax
movq %rax, 0x108(%rsp)
movq $0x0, 0x308(%rsp)
leaq 0x2f8(%rsp), %rdi
callq 0x43e70
movq 0x108(%rsp), %rsi
leaq 0x2e0(%rsp), %rdi
callq 0x560c0
jmp 0x5a7cf
movq $0x0, 0x2c8(%rsp)
movl $0x0, 0x2c4(%rsp)
movq 0x108(%rsp), %rdi
movl 0x2c4(%rsp), %eax
movl %eax, 0x100(%rsp)
callq 0x18700
movl %eax, 0x104(%rsp)
jmp 0x5a80a
movl 0x100(%rsp), %eax
movl 0x104(%rsp), %ecx
cmpl %ecx, %eax
jge 0x5b121
movq 0x108(%rsp), %rdi
movl 0x2c4(%rsp), %esi
callq 0x18710
movq %rax, 0xf8(%rsp)
jmp 0x5a83e
movq 0xf8(%rsp), %rax
movq %rax, 0x2b8(%rsp)
movq 0x2b8(%rsp), %rdi
callq 0x5b2f0
movl %eax, 0xf4(%rsp)
jmp 0x5a864
movl 0xf4(%rsp), %eax
cmpl $0x0, %eax
je 0x5a8b7
movq 0x2b8(%rsp), %rdi
callq 0x5b310
movss %xmm0, 0xf0(%rsp)
jmp 0x5a888
movss 0xf0(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jbe 0x5a8b7
jmp 0x5b10b
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
jmp 0x5b199
cmpq $0x0, 0x308(%rsp)
jne 0x5aa32
leaq 0x2a8(%rsp), %rdi
callq 0x43e70
jmp 0x5a8d5
movl $0x0, 0x2a4(%rsp)
movq 0x108(%rsp), %rdi
movl 0x2a4(%rsp), %eax
movl %eax, 0xe8(%rsp)
callq 0x18700
movl %eax, 0xec(%rsp)
jmp 0x5a904
movl 0xe8(%rsp), %eax
movl 0xec(%rsp), %ecx
cmpl %ecx, %eax
jge 0x5a979
movq 0x108(%rsp), %rdi
movl 0x2a4(%rsp), %esi
callq 0x18710
movq %rax, 0xe0(%rsp)
jmp 0x5a934
movq 0xe0(%rsp), %rsi
leaq 0x2a8(%rsp), %rdi
callq 0x43e80
jmp 0x5a94b
jmp 0x5a94d
movl 0x2a4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x2a4(%rsp)
jmp 0x5a8e0
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
jmp 0x5aa20
cmpq $0x0, 0x308(%rsp)
jne 0x5a9f4
movl $0x60, %edi
callq 0x13570
movq %rax, 0xd8(%rsp)
jmp 0x5a998
movq 0xd8(%rsp), %rdi
leaq -0x4be7(%rip), %rdx     # 0x55dc0
xorl %eax, %eax
movl %eax, %ecx
leaq 0x2a8(%rsp), %rsi
callq 0x5b330
jmp 0x5a9ba
movq 0xd8(%rsp), %rax
movq %rax, 0x308(%rsp)
jmp 0x5a9f4
movq 0xd8(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
movl $0x60, %esi
callq 0x13580
jmp 0x5aa20
cmpq $0x0, 0x308(%rsp)
jne 0x5aa11
leaq 0x14ad9a(%rip), %rdi    # 0x1a57a0
xorl %eax, %eax
callq 0x1463a0
jmp 0x5aa0f
jmp 0x5aa11
leaq 0x2a8(%rsp), %rdi
callq 0x43fb0
jmp 0x5aa32
leaq 0x2a8(%rsp), %rdi
callq 0x43fb0
jmp 0x5b199
leaq 0x2f8(%rsp), %rdi
xorl %esi, %esi
callq 0x1425c0
jmp 0x5aa43
movq 0x308(%rsp), %rdi
movq 0x2b8(%rsp), %rsi
movsd 0x318(%rsp), %xmm1
movl 0x314(%rsp), %edx
xorl %eax, %eax
movl %eax, %r8d
xorps %xmm0, %xmm0
leaq 0x2f8(%rsp), %rcx
callq 0x5b5e0
jmp 0x5aa7a
leaq 0x2f8(%rsp), %rdi
callq 0x16d50
movl %eax, 0xd4(%rsp)
jmp 0x5aa90
movl 0xd4(%rsp), %eax
cmpl $0x3, %eax
jge 0x5aaa1
jmp 0x5b10b
cmpq $0x0, 0x2c8(%rsp)
jne 0x5abbd
movl 0x314(%rsp), %eax
incl %eax
cltq
movq %rax, 0xb8(%rsp)
movl $0x18, %ecx
mulq %rcx
movq %rax, %rdi
movq $-0x1, %rax
cmovoq %rax, %rdi
movq %rdi, 0xc0(%rsp)
callq 0x13030
movq %rax, 0xc8(%rsp)
jmp 0x5aaf0
movq 0xb8(%rsp), %rax
cmpq $0x0, %rax
je 0x5ab60
movq 0xc8(%rsp), %rax
movq 0xb8(%rsp), %rcx
imulq $0x18, %rcx, %rdx
movq %rax, %rcx
addq %rdx, %rcx
movq %rcx, 0xa8(%rsp)
movq %rax, 0xb0(%rsp)
movq 0xb0(%rsp), %rdi
movq %rdi, 0xa0(%rsp)
callq 0xf8340
jmp 0x5ab3f
movq 0xa8(%rsp), %rcx
movq 0xa0(%rsp), %rax
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0xb0(%rsp)
jne 0x5ab28
movq 0xc8(%rsp), %rax
movq %rax, 0x2c8(%rsp)
cmpq $0x0, 0x2c8(%rsp)
jne 0x5abbb
leaq 0x14ac4a(%rip), %rdi    # 0x1a57cc
xorl %eax, %eax
callq 0x1463a0
jmp 0x5ab8b
jmp 0x5abbb
movq 0xc0(%rsp), %rsi
movq 0xc8(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
callq 0x13a90
jmp 0x5b199
jmp 0x5abbd
movl $0x0, 0x2a0(%rsp)
movq 0x2b8(%rsp), %rsi
leaq 0x288(%rsp), %rdi
callq 0x18730
jmp 0x5abdf
movq 0x2c8(%rsp), %rax
movslq 0x2a0(%rsp), %rcx
movl %ecx, %edx
incl %edx
movl %edx, 0x2a0(%rsp)
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rdi
leaq 0x288(%rsp), %rsi
callq 0xf89e0
jmp 0x5ac11
movl $0x0, 0x284(%rsp)
movl 0x284(%rsp), %eax
movl %eax, 0x98(%rsp)
leaq 0x2f8(%rsp), %rdi
callq 0x16d50
movl %eax, 0x9c(%rsp)
jmp 0x5ac40
movl 0x98(%rsp), %eax
movl 0x9c(%rsp), %ecx
cmpl %ecx, %eax
jge 0x5ace5
movl 0x284(%rsp), %esi
leaq 0x2f8(%rsp), %rdi
callq 0x44520
movq %rax, 0x90(%rsp)
jmp 0x5ac74
movq 0x90(%rsp), %rax
movq %rax, 0x278(%rsp)
movq 0x278(%rsp), %rsi
leaq 0x260(%rsp), %rdi
callq 0x18730
jmp 0x5ac9b
movq 0x2c8(%rsp), %rax
movslq 0x2a0(%rsp), %rcx
movl %ecx, %edx
incl %edx
movl %edx, 0x2a0(%rsp)
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rdi
leaq 0x260(%rsp), %rsi
callq 0xf89e0
jmp 0x5accd
jmp 0x5accf
movl 0x284(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x284(%rsp)
jmp 0x5ac1c
cmpl $0x6, 0x2a0(%rsp)
jg 0x5ad02
movl 0x2a0(%rsp), %eax
subl $0x1, %eax
movl %eax, 0x8c(%rsp)
jmp 0x5ad10
movl $0x6, %eax
movl %eax, 0x8c(%rsp)
jmp 0x5ad10
movl 0x8c(%rsp), %eax
movl %eax, 0x25c(%rsp)
movq 0x2c8(%rsp), %rax
movslq 0x25c(%rsp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rax
movq %rax, 0x80(%rsp)
movq 0x2b8(%rsp), %rsi
leaq 0x238(%rsp), %rdi
callq 0x18730
jmp 0x5ad55
movq 0x80(%rsp), %rdi
leaq 0x238(%rsp), %rsi
callq 0x8e7e0
movsd %xmm0, 0x78(%rsp)
jmp 0x5ad72
movsd 0x78(%rsp), %xmm0
movsd %xmm0, 0x250(%rsp)
movsd 0x250(%rsp), %xmm1
leaq 0x1a17b7(%rip), %rax    # 0x1fc548
movsd (%rax), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x5adaf
leaq 0x1a17a6(%rip), %rax    # 0x1fc548
movsd (%rax), %xmm0
movsd %xmm0, 0x250(%rsp)
movl 0x2a0(%rsp), %esi
movq 0x2c8(%rsp), %rdx
xorl %eax, %eax
movl %eax, %ecx
leaq 0x208(%rsp), %rdi
callq 0x97b20
jmp 0x5add1
movl 0x2a0(%rsp), %edx
movq 0x2c8(%rsp), %rcx
xorl %eax, %eax
movl %eax, %r8d
leaq 0x1c0(%rsp), %rdi
leaq 0x208(%rsp), %rsi
leaq 0x220(%rsp), %r9
callq 0x97be0
jmp 0x5ae04
leaq 0x1c0(%rsp), %rdi
movl $0x2, %esi
callq 0x44220
movq %rax, 0x70(%rsp)
jmp 0x5ae1d
movq 0x70(%rsp), %rsi
leaq 0x1a8(%rsp), %rdi
callq 0xf94f0
jmp 0x5ae31
leaq 0x1c0(%rsp), %rdi
xorl %esi, %esi
callq 0x44220
movq %rax, 0x68(%rsp)
jmp 0x5ae47
movq 0x68(%rsp), %rsi
leaq 0x190(%rsp), %rdi
callq 0xf94f0
jmp 0x5ae5b
movsd 0x220(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x5ae8c
movsd 0x228(%rsp), %xmm0
divsd 0x220(%rsp), %xmm0
callq 0x13420
movsd %xmm0, 0x60(%rsp)
jmp 0x5ae9c
movsd 0x1492d4(%rip), %xmm0   # 0x1a4168
movsd %xmm0, 0x60(%rsp)
jmp 0x5ae9c
movsd 0x60(%rsp), %xmm0
movsd %xmm0, 0x188(%rsp)
movsd 0x188(%rsp), %xmm0
movsd 0x250(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x180(%rsp)
movq 0x2b8(%rsp), %rdi
callq 0x44cb0
movq %rax, 0x58(%rsp)
jmp 0x5aede
movq 0x58(%rsp), %rax
movq %rax, 0x178(%rsp)
movq 0x178(%rsp), %rdi
callq 0x43710
movq %rax, 0x50(%rsp)
jmp 0x5aeff
movq 0x50(%rsp), %rax
movq %rax, 0x170(%rsp)
cmpq $0x0, 0x170(%rsp)
je 0x5af3c
movq 0x170(%rsp), %rdi
movl $0x1, %esi
callq 0x5f430
movq %rax, 0x48(%rsp)
jmp 0x5af30
movq 0x48(%rsp), %rax
movq %rax, 0x40(%rsp)
jmp 0x5af45
xorl %eax, %eax
movq %rax, 0x40(%rsp)
jmp 0x5af45
movq 0x40(%rsp), %rax
movq %rax, 0x168(%rsp)
cmpq $0x0, 0x168(%rsp)
je 0x5afdc
leaq 0x148(%rsp), %rdi
leaq 0x208(%rsp), %rsi
leaq 0x1a8(%rsp), %rdx
callq 0xf5ab0
jmp 0x5af7c
movq 0x168(%rsp), %rdi
callq 0x31bd0
movq %rax, 0x38(%rsp)
jmp 0x5af90
movq 0x38(%rsp), %rax
movq %rax, 0x140(%rsp)
movq 0x140(%rsp), %rsi
leaq 0x148(%rsp), %rdi
callq 0x8eb30
movsd %xmm0, 0x30(%rsp)
jmp 0x5afba
movsd 0x30(%rsp), %xmm1
xorps %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x5afda
leaq 0x1a8(%rsp), %rdi
callq 0xf9c90
jmp 0x5afd8
jmp 0x5afda
jmp 0x5b051
movq 0x2b8(%rsp), %rsi
leaq 0x110(%rsp), %rdi
callq 0x18730
jmp 0x5aff3
leaq 0x128(%rsp), %rdi
leaq 0x110(%rsp), %rsi
leaq 0x2e0(%rsp), %rdx
callq 0xf86b0
jmp 0x5b012
leaq 0x128(%rsp), %rdi
leaq 0x1a8(%rsp), %rsi
callq 0xf9ac0
movsd %xmm0, 0x28(%rsp)
jmp 0x5b02f
movsd 0x28(%rsp), %xmm1
xorps %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x5b04f
leaq 0x1a8(%rsp), %rdi
callq 0xf9c90
jmp 0x5b04d
jmp 0x5b04f
jmp 0x5b051
movq 0x2b8(%rsp), %rdi
callq 0x5b2f0
movl %eax, 0x24(%rsp)
jmp 0x5b064
movl 0x24(%rsp), %eax
cmpl $0x0, %eax
jne 0x5b086
movq 0x2b8(%rsp), %rdi
leaq 0x1a8(%rsp), %rsi
callq 0x559f0
jmp 0x5b084
jmp 0x5b086
movq 0x2b8(%rsp), %rdi
callq 0x5b650
movl %eax, 0x20(%rsp)
jmp 0x5b099
movl 0x20(%rsp), %eax
cmpl $0x0, %eax
jne 0x5b0bb
movq 0x2b8(%rsp), %rdi
leaq 0x190(%rsp), %rsi
callq 0x55a30
jmp 0x5b0b9
jmp 0x5b0bb
movq 0x2b8(%rsp), %rdi
xorl %esi, %esi
callq 0x4f9b0
movss %xmm0, 0x1c(%rsp)
jmp 0x5b0d2
movss 0x1c(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jne 0x5b109
jp 0x5b109
movq 0x2b8(%rsp), %rdi
cvttsd2si 0x250(%rsp), %esi
movsd 0x180(%rsp), %xmm0
cvtsd2ss %xmm0, %xmm0
callq 0x55ac0
jmp 0x5b107
jmp 0x5b109
jmp 0x5b10b
movl 0x2c4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x2c4(%rsp)
jmp 0x5a7e6
cmpq $0x0, 0x2c8(%rsp)
je 0x5b14b
movq 0x2c8(%rsp), %rax
movq %rax, 0x10(%rsp)
cmpq $0x0, %rax
je 0x5b149
movq 0x10(%rsp), %rdi
callq 0x13750
jmp 0x5b14b
cmpq $0x0, 0x308(%rsp)
je 0x5b184
movq 0x308(%rsp), %rax
movq %rax, 0x8(%rsp)
cmpq $0x0, %rax
je 0x5b182
movq 0x8(%rsp), %rdi
callq 0x5b670
movq 0x8(%rsp), %rdi
movl $0x60, %esi
callq 0x13580
jmp 0x5b184
leaq 0x2f8(%rsp), %rdi
callq 0x43fb0
addq $0x328, %rsp            # imm = 0x328
retq
leaq 0x2f8(%rsp), %rdi
callq 0x43fb0
movq 0x2d8(%rsp), %rdi
callq 0x13970
nopw %cs:(%rax,%rax)
nopl (%rax)