// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/R3Utils/R3SurfelViewer.cpp
// Function: gaps::R3SurfelViewer::UpdateWorkingSet(gaps::R3Viewer const&)
void R3SurfelViewer::
UpdateWorkingSet(const R3Viewer& view)
{
  // Just checking
  if (!scene) return;
  R3SurfelTree *tree = scene->Tree();
  if (!tree) return;
 
  // Get convenient variables
  R3Point eye = view.Camera().Origin();
  R3Vector towards = view.Camera().Towards();
  R2Box viewport_box = view.Viewport().BBox();
  if (viewport_box.Area() == 0) return;

  // Allocate temporary memory
  int xres = viewport_box.XLength();
  int yres = int (xres * (double) viewport_box.YLength() / (double) viewport_box.XLength());
  int *visible_marks = new int [ tree->NNodes() ];
  int *resident_marks = new int [ tree->NNodes() ];
  int *item_buffer = new int [ xres * yres ];
  RNScalar *depth_buffer = new RNScalar [ xres * yres ];
  RNScalar *viewpoint_distance_buffer = new RNScalar [ xres * yres ];
  for (int i = 0; i < tree->NNodes(); i++) { resident_marks[i] = visible_marks[i] = 0; }
  for (int i = 0; i < resident_nodes.NNodes(); i++) resident_marks[resident_nodes.Node(i)->TreeIndex()] = 1;

  // Create viewer
  R3Viewer tmp_viewer(view);
  tmp_viewer.ResizeViewport(0, 0, xres, yres);

  RNBoolean done = FALSE;
  const int max_iterations = 8;
  for (int iteration = 0; iteration < max_iterations; iteration++) {
    // Check if done
    if (done) break;
    done = TRUE;

    // Initialize buffers
    for (int i = 0; i < xres * yres; i++) {
      item_buffer[i] = -1;
      depth_buffer[i] = FLT_MAX;
      viewpoint_distance_buffer[i] = FLT_MAX;
    }
    for (int i = 0; i < tree->NNodes(); i++) {
      visible_marks[i] = 0;
    }

    // Render surfels into item buffer
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      for (int j = 0; j < node->NBlocks(); j++) {
        R3SurfelBlock *block = node->Block(j);
        const R3Point& block_origin = block->PositionOrigin();
        for (int k = 0; k < block->NSurfels(); k++) {
          const R3Surfel *surfel = block->Surfel(k);
          double wx = surfel->X() + block_origin.X();
          double wy = surfel->Y() + block_origin.Y();
          double wz = surfel->Z() + block_origin.Z();
          R3Point wp(wx, wy, wz);
          RNScalar depth = towards.Dot(wp - eye);
          if (depth <= 0) continue; 
          R2Point vp = tmp_viewer.ViewportPoint(wp);
          if (vp == R2infinite_point) continue; 
          int vx = (int) (vp.X() + 0.5);
          if ((vx < 0) || (vx >= xres)) continue; 
          int vy = (int) (vp.Y() + 0.5);
          if ((vy < 0) || (vy >= yres)) continue; 
          int index = xres*vy + vx;
          if (RNIsGreater(depth, depth_buffer[index])) continue;
          RNLength viewpoint_distance = (node->Scan()) ? R3Distance(tmp_viewer.Camera().Origin(), node->Scan()->Viewpoint()) : RN_INFINITY;
          if (RNIsEqual(depth, depth_buffer[index], 1E-3) && RNIsGreater(viewpoint_distance, viewpoint_distance_buffer[index])) continue;
          item_buffer[index] = node->TreeIndex();
          depth_buffer[index] = depth;
          viewpoint_distance_buffer[index] = viewpoint_distance;
        }
      }
    }

    // Mark visible nodes
    for (int i = 0; i < xres * yres; i++) {
      int node_index = item_buffer[i];
      if (node_index < 0) continue;
      if (visible_marks[node_index] == 0) {
        visible_marks[node_index] = 1;
      }
    }

    // Remove invisible nodes from working set
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      assert(resident_marks[node->TreeIndex()]);
      if (visible_marks[node->TreeIndex()] == 1) continue;
      resident_marks[node->TreeIndex()] = 0;
      RemoveFromWorkingSet(node);
    }
    
    // Add parts of visible nodes to working set
    for (int i = 0; i < xres * yres; i++) {
      int node_index = item_buffer[i];
      if (node_index < 0) continue;
      assert(depth_buffer[i] < FLT_MAX);
      assert(visible_marks[node_index] == 1);
      if (resident_marks[node_index] == 0) continue;
      R3SurfelNode *node = tree->Node(node_index);
      if (node->NParts() == 0) continue;
      resident_marks[node_index] = 0;
      RemoveFromWorkingSet(node);
      for (int j = 0; j < node->NParts(); j++) {
        R3SurfelNode *part = node->Part(j);
        resident_marks[part->TreeIndex()] = 1;
        InsertIntoWorkingSet(part);
        done = FALSE;
      }
    }

    if (done) break;
  }

  // Delete temporary memory
  delete [] visible_marks;
  delete [] resident_marks;
  delete [] item_buffer;
  delete [] depth_buffer;
  delete [] viewpoint_distance_buffer;

  // Invalidate VBO buffers
  InvalidateVBO();
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1c8, %rsp            # imm = 0x1C8
movq 0x8(%rdi), %rax
testq %rax, %rax
je 0x30bb8
movq 0x8(%rax), %rax
movq %rax, 0x50(%rsp)
testq %rax, %rax
je 0x30bb8
movq %rsi, %rbx
movq %rdi, %r13
addq $0x8, %rsi
leaq 0xf0(%rsp), %rdi
callq 0xb2330
leaq 0x50(%rbx), %rsi
leaq 0xd8(%rsp), %rdi
callq 0xb3446
leaq 0x98(%rbx), %rsi
leaq 0x88(%rsp), %r14
movq %r14, %rdi
callq 0x32cb6
movq %r14, %rdi
callq 0xc7970
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x30bca
jp 0x30bca
leaq 0x88(%rsp), %rdi
callq 0xc7d1e
addq $0x1c8, %rsp            # imm = 0x1C8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movsd 0xa0(%rsp), %xmm0
movsd 0xa8(%rsp), %xmm1
subsd 0x90(%rsp), %xmm0
movapd %xmm0, 0x60(%rsp)
subsd 0x98(%rsp), %xmm1
movsd %xmm1, 0x20(%rsp)
movq 0x50(%rsp), %rax
movslq 0x24(%rax), %r15
leaq (,%r15,4), %r14
testq %r15, %r15
pushq $-0x1
popq %r12
cmovsq %r12, %r14
movq %r14, %rdi
callq 0x27040
movq %rax, 0x30(%rsp)
movq %r13, 0x10(%rsp)
movq %r14, %rdi
callq 0x27040
movq %rax, 0x8(%rsp)
movapd 0x60(%rsp), %xmm1
cvttsd2si %xmm1, %eax
cvttpd2dq %xmm1, %xmm0
cvtdq2pd %xmm0, %xmm0
movsd 0x20(%rsp), %xmm2
mulsd %xmm0, %xmm2
divsd %xmm1, %xmm2
cvttsd2si %xmm2, %ecx
movl %ecx, 0x2c(%rsp)
movl %eax, 0x20(%rsp)
imull %eax, %ecx
movq %rcx, %rbp
movslq %ecx, %r13
leaq (,%r13,4), %rdi
testl %r13d, %r13d
cmovsq %r12, %rdi
callq 0x27040
movq %rax, 0x18(%rsp)
leaq (,%r13,8), %r14
testl %r13d, %r13d
cmovsq %r12, %r14
movq %r14, %rdi
callq 0x27040
movq %rax, 0x40(%rsp)
movq %r14, %rdi
callq 0x27040
movq %rax, 0x38(%rsp)
movq 0x10(%rsp), %rcx
xorl %eax, %eax
cmpq %r15, %rax
jge 0x30ccf
movq 0x30(%rsp), %rdx
andl $0x0, (%rdx,%rax,4)
movq 0x8(%rsp), %rdx
andl $0x0, (%rdx,%rax,4)
incq %rax
jmp 0x30cb3
movl 0x24(%rcx), %eax
movq 0x18(%rcx), %rcx
xorl %edx, %edx
testl %eax, %eax
cmovlel %edx, %eax
movq 0x8(%rsp), %rdi
cmpq %rdx, %rax
je 0x30cfb
movq (%rcx,%rdx,8), %rsi
movslq 0x20(%rsi), %rsi
movl $0x1, (%rdi,%rsi,4)
incq %rdx
jmp 0x30ce2
leaq 0x120(%rsp), %rdi
movq %rbx, %rsi
callq 0x6e83a
leaq 0x120(%rsp), %rdi
xorl %ebx, %ebx
xorl %esi, %esi
xorl %edx, %edx
movl 0x20(%rsp), %ecx
movl 0x2c(%rsp), %r8d
callq 0x6e9f2
testl %ebp, %ebp
cmovlel %ebx, %ebp
movq %rbp, 0x58(%rsp)
leaq 0x108(%rsp), %rbx
leaq 0xc0(%rsp), %r13
xorl %eax, %eax
cmpl $0x8, %eax
je 0x310fd
movq %rax, 0x70(%rsp)
xorl %eax, %eax
movq 0x30(%rsp), %r12
cmpq %rax, 0x58(%rsp)
je 0x30d89
movq 0x18(%rsp), %rcx
orl $-0x1, (%rcx,%rax,4)
movq 0x40(%rsp), %rcx
movabsq $0x47efffffe0000000, %rdx # imm = 0x47EFFFFFE0000000
movq %rdx, (%rcx,%rax,8)
movq 0x38(%rsp), %rcx
movq %rdx, (%rcx,%rax,8)
incq %rax
jmp 0x30d58
movq 0x50(%rsp), %rax
movslq 0x24(%rax), %rax
xorl %ecx, %ecx
cmpq %rax, %rcx
jge 0x30da3
andl $0x0, (%r12,%rcx,4)
incq %rcx
jmp 0x30d94
xorl %edx, %edx
movq 0x10(%rsp), %rcx
movslq 0x24(%rcx), %rax
cmpq %rax, %rdx
jge 0x30fdd
movq 0x18(%rcx), %rax
movq (%rax,%rdx,8), %rcx
xorl %esi, %esi
movq %rdx, 0x78(%rsp)
movq %rcx, 0x48(%rsp)
movslq 0x4c(%rcx), %rax
cmpq %rax, %rsi
jge 0x30fd0
movq 0x40(%rcx), %rax
movq %rsi, 0x80(%rsp)
movq (%rax,%rsi,8), %rbp
pushq $0x8
popq %r12
xorl %r15d, %r15d
movslq 0x8(%rbp), %rax
cmpq %rax, %r15
jge 0x30fb6
movq (%rbp), %rax
cvtss2sd -0x4(%rax,%r12), %xmm1
addsd 0x18(%rbp), %xmm1
cvtss2sd -0x8(%rax,%r12), %xmm0
addsd 0x10(%rbp), %xmm0
cvtss2sd (%rax,%r12), %xmm2
addsd 0x20(%rbp), %xmm2
movq %rbx, %rdi
callq 0xb2320
movq %r13, %rdi
movq %rbx, %rsi
leaq 0xf0(%rsp), %rdx
callq 0xb24fd
leaq 0xd8(%rsp), %rdi
movq %r13, %rsi
callq 0xb2d5a
movapd %xmm0, %xmm1
xorpd %xmm0, %xmm0
movsd %xmm1, 0x60(%rsp)
ucomisd %xmm1, %xmm0
jae 0x30e88
movq %r13, %rdi
leaq 0x120(%rsp), %rsi
movq %rbx, %rdx
callq 0x6f73e
movq %r13, %rdi
leaq 0x57fcd1(%rip), %rsi    # 0x5b0b50
callq 0xca28c
testl %eax, %eax
je 0x30e94
incq %r15
addq $0x30, %r12
jmp 0x30def
movsd 0xc0(%rsp), %xmm0
addsd 0xe916b(%rip), %xmm0    # 0x11a010
cvttsd2si %xmm0, %eax
testl %eax, %eax
js 0x30e88
cmpl 0x20(%rsp), %eax
jge 0x30e88
movsd 0xc8(%rsp), %xmm0
addsd 0xe914c(%rip), %xmm0    # 0x11a010
cvttsd2si %xmm0, %r14d
testl %r14d, %r14d
js 0x30e88
cmpl 0x2c(%rsp), %r14d
jge 0x30e88
imull 0x20(%rsp), %r14d
addl %eax, %r14d
movsd 0x60(%rsp), %xmm0
movq 0x40(%rsp), %rax
subsd (%rax,%r14,8), %xmm0
leaq 0x1772b2(%rip), %rax    # 0x1a81a8
movapd %xmm0, 0xb0(%rsp)
ucomisd (%rax), %xmm0
ja 0x30e88
movq 0x48(%rsp), %rdi
pushq $0x1
popq %rsi
callq 0x3d2fe
testq %rax, %rax
je 0x30f3a
movq 0x48(%rsp), %rdi
pushq $0x1
popq %rsi
callq 0x3d2fe
addq $0x28, %rax
leaq 0x128(%rsp), %rdi
movq %rax, %rsi
callq 0x7fb9a
jmp 0x30f45
leaq 0x177277(%rip), %rax    # 0x1a81b8
movsd (%rax), %xmm0
movapd 0xb0(%rsp), %xmm2
andpd 0xe956a(%rip), %xmm2    # 0x11a4c0
movsd 0xe94da(%rip), %xmm1    # 0x11a438
ucomisd %xmm2, %xmm1
jb 0x30f84
movapd %xmm0, %xmm1
movq 0x38(%rsp), %rax
subsd (%rax,%r14,8), %xmm1
leaq 0x17722e(%rip), %rax    # 0x1a81a8
ucomisd (%rax), %xmm1
ja 0x30e88
movq 0x48(%rsp), %rax
movl 0x20(%rax), %eax
movq 0x18(%rsp), %rcx
movl %eax, (%rcx,%r14,4)
movq 0x40(%rsp), %rax
movsd 0x60(%rsp), %xmm1
movsd %xmm1, (%rax,%r14,8)
movq 0x38(%rsp), %rax
movsd %xmm0, (%rax,%r14,8)
jmp 0x30e88
movq 0x80(%rsp), %rsi
incq %rsi
movq 0x78(%rsp), %rdx
movq 0x48(%rsp), %rcx
jmp 0x30dcb
incq %rdx
movq 0x30(%rsp), %r12
jmp 0x30da5
xorl %ecx, %ecx
cmpq %rcx, 0x58(%rsp)
je 0x31008
movq 0x18(%rsp), %rdx
movslq (%rdx,%rcx,4), %rdx
testq %rdx, %rdx
js 0x31003
cmpl $0x0, (%r12,%rdx,4)
jne 0x31003
movl $0x1, (%r12,%rdx,4)
incq %rcx
jmp 0x30fdf
xorl %r14d, %r14d
movslq %eax, %rcx
cmpq %rcx, %r14
jge 0x31050
movq 0x10(%rsp), %rcx
movq 0x18(%rcx), %rcx
movq (%rcx,%r14,8), %rsi
movslq 0x20(%rsi), %rcx
cmpl $0x1, (%r12,%rcx,4)
je 0x3104b
movq 0x8(%rsp), %rax
andl $0x0, (%rax,%rcx,4)
movq 0x10(%rsp), %r15
movq (%r15), %rax
movq %r15, %rdi
xorl %edx, %edx
callq *0x330(%rax)
movl 0x24(%r15), %eax
incq %r14
jmp 0x3100b
xorl %r15d, %r15d
pushq $0x1
popq %rbp
cmpq 0x58(%rsp), %r15
je 0x310ee
movq 0x18(%rsp), %rax
movslq (%rax,%r15,4), %rax
testq %rax, %rax
js 0x310e6
movq 0x8(%rsp), %rcx
cmpl $0x0, (%rcx,%rax,4)
je 0x310e6
movq 0x50(%rsp), %rcx
movq 0x18(%rcx), %rcx
movq (%rcx,%rax,8), %rsi
cmpl $0x0, 0x3c(%rsi)
je 0x310e6
movq 0x8(%rsp), %rcx
andl $0x0, (%rcx,%rax,4)
movq 0x10(%rsp), %rdi
movq (%rdi), %rax
movq %rsi, %r12
xorl %edx, %edx
callq *0x330(%rax)
xorl %r14d, %r14d
movslq 0x3c(%r12), %rax
cmpq %rax, %r14
jge 0x310e6
movq 0x30(%r12), %rax
movq (%rax,%r14,8), %rsi
movslq 0x20(%rsi), %rax
movq 0x8(%rsp), %rcx
movl $0x1, (%rcx,%rax,4)
movq 0x10(%rsp), %rdi
movq (%rdi), %rax
xorl %edx, %edx
callq *0x328(%rax)
incq %r14
xorl %ebp, %ebp
jmp 0x310ac
incq %r15
jmp 0x31056
movq 0x70(%rsp), %rax
incl %eax
testl %ebp, %ebp
je 0x30d43
movq 0x30(%rsp), %rdi
callq 0x27c80
movq 0x8(%rsp), %rdi
callq 0x27c80
movq 0x18(%rsp), %rdi
callq 0x27c80
movq 0x40(%rsp), %rdi
callq 0x27c80
movq 0x38(%rsp), %rdi
callq 0x27c80
movq 0x10(%rsp), %rdi
movq (%rdi), %rax
callq *0x378(%rax)
leaq 0x120(%rsp), %rdi
callq 0x6e8e6
jmp 0x30bab
jmp 0x31170
jmp 0x31170
jmp 0x3115f
jmp 0x3115f
jmp 0x3115f
jmp 0x3115f
jmp 0x3115f
jmp 0x3115f
movq %rax, %rbx
jmp 0x31180
jmp 0x31170
jmp 0x31170
jmp 0x31170
jmp 0x31170
jmp 0x31170
jmp 0x31170
movq %rax, %rbx
leaq 0x120(%rsp), %rdi
callq 0x6e8e6
leaq 0x88(%rsp), %rdi
callq 0xc7d1e
movq %rbx, %rdi
callq 0x28140
nop