// File: /workspace/github_cpp_makefile_star2_O1/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 0x32b6f
movq 0x8(%rax), %r15
testq %r15, %r15
je 0x32b6f
movq %rsi, %rbx
movq %rdi, %r12
addq $0x8, %rsi
leaq 0xf0(%rsp), %rdi
callq 0xc532e
leaq 0x50(%rbx), %rsi
leaq 0xd8(%rsp), %rdi
callq 0xc6a4c
movl 0x98(%rbx), %eax
movl 0x9c(%rbx), %ecx
cvtsi2sd %eax, %xmm0
cvtsi2sd %ecx, %xmm1
addl 0xa0(%rbx), %eax
cvtsi2sd %eax, %xmm2
addl 0xa4(%rbx), %ecx
cvtsi2sd %ecx, %xmm3
leaq 0x98(%rsp), %r14
movq %r14, %rdi
callq 0xdccc0
movq %r14, %rdi
callq 0xdd0d4
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x32543
jnp 0x32b62
movsd 0xb0(%rsp), %xmm0
movsd 0xb8(%rsp), %xmm1
subsd 0xa0(%rsp), %xmm0
movapd %xmm0, 0x60(%rsp)
subsd 0xa8(%rsp), %xmm1
movsd %xmm1, 0x20(%rsp)
movslq 0x24(%r15), %rax
leaq (,%rax,4), %rdi
testq %rax, %rax
movq $-0x1, %r14
cmovsq %r14, %rdi
callq 0x27040
movq %rax, %r13
movq %r15, 0x48(%rsp)
movslq 0x24(%r15), %rax
leaq (,%rax,4), %rdi
testq %rax, %rax
cmovsq %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 %ecx, %ebp
movl %eax, 0x20(%rsp)
imull %eax, %ebp
movslq %ebp, %r14
leaq (,%r14,4), %rdi
testl %r14d, %r14d
movq $-0x1, %r15
cmovsq %r15, %rdi
callq 0x27040
movq %rax, 0x18(%rsp)
shlq $0x3, %r14
movl %ebp, 0x4(%rsp)
testl %ebp, %ebp
cmovsq %r15, %r14
movq %r14, %rdi
callq 0x27040
movq %rax, %rbp
movq %r14, %rdi
callq 0x27040
movq 0x48(%rsp), %r15
movq %rax, %r14
cmpl $0x0, 0x24(%r15)
movq 0x8(%rsp), %rdi
jle 0x32658
xorl %eax, %eax
xorl %ecx, %ecx
movl %eax, (%r13,%rcx,4)
movl %eax, (%rdi,%rcx,4)
incq %rcx
movslq 0x24(%r15), %rdx
cmpq %rdx, %rcx
jl 0x32644
movslq 0x24(%r12), %rax
testq %rax, %rax
jle 0x32680
movq 0x18(%r12), %rcx
xorl %edx, %edx
movq (%rcx,%rdx,8), %rsi
movslq 0x20(%rsi), %rsi
movl $0x1, (%rdi,%rsi,4)
incq %rdx
cmpq %rdx, %rax
jne 0x32669
leaq 0x120(%rsp), %rdi
movq %rbx, %rsi
callq 0x78d60
movq %r13, 0x10(%rsp)
leaq 0x120(%rsp), %rdi
movq $0x0, 0x78(%rsp)
xorl %esi, %esi
xorl %edx, %edx
movl 0x20(%rsp), %ecx
movl 0x2c(%rsp), %r8d
callq 0x78f18
movl 0x4(%rsp), %eax
movq %rax, 0x50(%rsp)
leaq (,%rax,4), %rax
movq %rax, 0x80(%rsp)
leaq 0x108(%rsp), %r13
leaq 0xc0(%rsp), %rbx
movq %r12, 0x58(%rsp)
movq %rbp, 0x38(%rsp)
movq %r14, 0x30(%rsp)
cmpl $0x0, 0x4(%rsp)
movq 0x50(%rsp), %r15
jle 0x32730
movq 0x18(%rsp), %rdi
movl $0xff, %esi
movq 0x80(%rsp), %rdx
callq 0x274b0
movabsq $0x47efffffe0000000, %rcx # imm = 0x47EFFFFFE0000000
xorl %eax, %eax
movq %rcx, (%rbp,%rax,8)
movq %rcx, (%r14,%rax,8)
incq %rax
cmpq %rax, %r15
jne 0x3271f
movq 0x48(%rsp), %rsi
cmpl $0x0, 0x24(%rsi)
movq 0x10(%rsp), %rdx
jle 0x32755
xorl %eax, %eax
movl $0x0, (%rdx,%rax,4)
incq %rax
movslq 0x24(%rsi), %rcx
cmpq %rcx, %rax
jl 0x32742
cmpl $0x0, 0x24(%r12)
jle 0x329ae
xorl %ecx, %ecx
movq 0x18(%r12), %rax
movq %rcx, 0x88(%rsp)
movq (%rax,%rcx,8), %rcx
cmpl $0x0, 0x4c(%rcx)
jle 0x32990
xorl %edx, %edx
movq %rcx, 0x40(%rsp)
movq 0x40(%rcx), %rax
movq %rdx, 0x90(%rsp)
movq (%rax,%rdx,8), %r14
cmpl $0x0, 0x8(%r14)
jle 0x32973
movl $0x8, %r15d
xorl %r12d, %r12d
movq (%r14), %rax
xorps %xmm1, %xmm1
cvtss2sd -0x4(%rax,%r15), %xmm1
addsd 0x18(%r14), %xmm1
xorps %xmm0, %xmm0
cvtss2sd -0x8(%rax,%r15), %xmm0
addsd 0x10(%r14), %xmm0
xorps %xmm2, %xmm2
cvtss2sd (%rax,%r15), %xmm2
addsd 0x20(%r14), %xmm2
movq %r13, %rdi
callq 0xc531e
movq %rbx, %rdi
movq %r13, %rsi
leaq 0xf0(%rsp), %rdx
callq 0xc5580
leaq 0xd8(%rsp), %rdi
movq %rbx, %rsi
callq 0xc604e
movapd %xmm0, %xmm1
xorpd %xmm0, %xmm0
movsd %xmm1, 0x60(%rsp)
ucomisd %xmm1, %xmm0
jae 0x32840
movq %rbx, %rdi
leaq 0x120(%rsp), %rsi
movq %r13, %rdx
callq 0x79d9c
movq %rbx, %rdi
leaq 0x5962f9(%rip), %rsi    # 0x5c8b30
callq 0xdfca2
testl %eax, %eax
je 0x32859
incq %r12
movslq 0x8(%r14), %rax
addq $0x30, %r15
cmpq %rax, %r12
jl 0x327a9
jmp 0x32973
movsd 0xc0(%rsp), %xmm0
addsd 0x1087a6(%rip), %xmm0   # 0x13b010
cvttsd2si %xmm0, %eax
testl %eax, %eax
js 0x32840
cmpl 0x20(%rsp), %eax
jge 0x32840
movsd 0xc8(%rsp), %xmm0
addsd 0x108787(%rip), %xmm0   # 0x13b010
cvttsd2si %xmm0, %ebp
testl %ebp, %ebp
js 0x32840
cmpl 0x2c(%rsp), %ebp
jge 0x32840
imull 0x20(%rsp), %ebp
addl %eax, %ebp
movsd 0x60(%rsp), %xmm0
movq 0x38(%rsp), %rax
subsd (%rax,%rbp,8), %xmm0
leaq 0x18d8d3(%rip), %rax    # 0x1c0188
ucomisd (%rax), %xmm0
ja 0x32840
movq 0x40(%rsp), %rdi
movl $0x1, %esi
callq 0x40210
testq %rax, %rax
je 0x328f4
movq 0x40(%rsp), %rdi
movl $0x1, %esi
callq 0x40210
addq $0x28, %rax
leaq 0x128(%rsp), %rdi
movq %rax, %rsi
callq 0x8ca78
jmp 0x328ff
leaq 0x18d89d(%rip), %rax    # 0x1c0198
movsd (%rax), %xmm0
movsd 0x60(%rsp), %xmm1
movq 0x38(%rsp), %rax
subsd (%rax,%rbp,8), %xmm1
andpd 0x108b99(%rip), %xmm1   # 0x13b4b0
movsd 0x108b19(%rip), %xmm2   # 0x13b438
ucomisd %xmm1, %xmm2
jb 0x32944
movapd %xmm0, %xmm1
movq 0x30(%rsp), %rax
subsd (%rax,%rbp,8), %xmm1
leaq 0x18d84e(%rip), %rax    # 0x1c0188
ucomisd (%rax), %xmm1
ja 0x32840
movq 0x40(%rsp), %rax
movl 0x20(%rax), %eax
movq 0x18(%rsp), %rcx
movl %eax, (%rcx,%rbp,4)
movq 0x38(%rsp), %rax
movsd 0x60(%rsp), %xmm1
movsd %xmm1, (%rax,%rbp,8)
movq 0x30(%rsp), %rax
movsd %xmm0, (%rax,%rbp,8)
jmp 0x32840
movq 0x90(%rsp), %rdx
incq %rdx
movq 0x40(%rsp), %rcx
movslq 0x4c(%rcx), %rax
cmpq %rax, %rdx
jl 0x32785
movq 0x88(%rsp), %rcx
incq %rcx
movq 0x58(%rsp), %r12
movslq 0x24(%r12), %rax
cmpq %rax, %rcx
jl 0x32763
cmpl $0x0, 0x4(%rsp)
movq 0x50(%rsp), %rsi
jle 0x329e9
xorl %eax, %eax
movq 0x18(%rsp), %rcx
movslq (%rcx,%rax,4), %rcx
testq %rcx, %rcx
js 0x329e1
movq 0x10(%rsp), %rdx
cmpl $0x0, (%rdx,%rcx,4)
jne 0x329e1
movq 0x10(%rsp), %rdx
movl $0x1, (%rdx,%rcx,4)
incq %rax
cmpq %rax, %rsi
jne 0x329bc
cmpl $0x0, 0x24(%r12)
jle 0x32a34
xorl %r14d, %r14d
movq 0x18(%r12), %rax
movq (%rax,%r14,8), %rsi
movslq 0x20(%rsi), %rax
movq 0x10(%rsp), %rcx
cmpl $0x1, (%rcx,%rax,4)
je 0x32a27
movq 0x8(%rsp), %rcx
movl $0x0, (%rcx,%rax,4)
movq (%r12), %rax
movq %r12, %rdi
xorl %edx, %edx
callq *0x330(%rax)
incq %r14
movslq 0x24(%r12), %rax
cmpq %rax, %r14
jl 0x329f4
cmpl $0x0, 0x4(%rsp)
jle 0x32af5
movl $0x1, %ebp
xorl %r15d, %r15d
movq 0x18(%rsp), %rax
movslq (%rax,%r15,4), %rax
testq %rax, %rax
js 0x32adb
movq 0x8(%rsp), %rcx
cmpl $0x0, (%rcx,%rax,4)
je 0x32adb
movq 0x48(%rsp), %rcx
movq 0x18(%rcx), %rcx
movq (%rcx,%rax,8), %r14
cmpl $0x0, 0x3c(%r14)
je 0x32adb
movq 0x8(%rsp), %rcx
movl $0x0, (%rcx,%rax,4)
movq (%r12), %rax
movq %r12, %rdi
movq %r14, %rsi
xorl %edx, %edx
callq *0x330(%rax)
cmpl $0x0, 0x3c(%r14)
jle 0x32adb
xorl %r12d, %r12d
movq 0x30(%r14), %rax
movq (%rax,%r12,8), %rsi
movslq 0x20(%rsi), %rax
movq 0x8(%rsp), %rcx
movl $0x1, (%rcx,%rax,4)
movq 0x58(%rsp), %rdi
movq (%rdi), %rax
xorl %edx, %edx
callq *0x328(%rax)
incq %r12
movslq 0x3c(%r14), %rax
cmpq %rax, %r12
jl 0x32aa0
xorl %ebp, %ebp
movq 0x58(%rsp), %r12
incq %r15
cmpq 0x50(%rsp), %r15
movq 0x30(%rsp), %r14
jne 0x32a47
testl %ebp, %ebp
setne %al
jmp 0x32afc
movb $0x1, %al
movq 0x30(%rsp), %r14
testb %al, %al
movq 0x38(%rsp), %rbp
jne 0x32b1a
movq 0x78(%rsp), %rax
incl %eax
movq %rax, 0x78(%rsp)
cmpl $0x8, %eax
jne 0x326f0
movq 0x10(%rsp), %rdi
callq 0x27c90
movq 0x8(%rsp), %rdi
callq 0x27c90
movq 0x18(%rsp), %rdi
callq 0x27c90
movq %rbp, %rdi
callq 0x27c90
movq %r14, %rdi
callq 0x27c90
movq (%r12), %rax
movq %r12, %rdi
callq *0x378(%rax)
leaq 0x120(%rsp), %rdi
callq 0x78e0c
leaq 0x98(%rsp), %rdi
callq 0xdd540
addq $0x1c8, %rsp            # imm = 0x1C8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x32ba2
jmp 0x32ba2
jmp 0x32b91
jmp 0x32b91
jmp 0x32b91
jmp 0x32b91
jmp 0x32b91
jmp 0x32b91
movq %rax, %rbx
jmp 0x32bb2
jmp 0x32ba2
jmp 0x32ba2
jmp 0x32ba2
jmp 0x32ba2
jmp 0x32ba2
jmp 0x32ba2
movq %rax, %rbx
leaq 0x120(%rsp), %rdi
callq 0x78e0c
leaq 0x98(%rsp), %rdi
callq 0xdd540
movq %rbx, %rdi
callq 0x28130
nop