// File: /workspace/github_cpp_makefile_star2_O3/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 $0x1d8, %rsp            # imm = 0x1D8
movq 0x8(%rdi), %rax
testq %rax, %rax
je 0x32b3b
movq 0x8(%rax), %rax
movq %rax, 0x60(%rsp)
testq %rax, %rax
je 0x32b3b
movq %rsi, %rbx
movq %rdi, %r12
addq $0x8, %rsi
leaq 0x100(%rsp), %rdi
callq 0xc3d42
leaq 0x50(%rbx), %rsi
leaq 0xe8(%rsp), %rdi
callq 0xc5400
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 0xdb4dc
movq %r14, %rdi
callq 0xdb8e2
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x323ce
jnp 0x32b2e
movsd 0xb0(%rsp), %xmm0
movsd 0xb8(%rsp), %xmm1
subsd 0xa0(%rsp), %xmm0
movapd %xmm0, 0x70(%rsp)
subsd 0xa8(%rsp), %xmm1
movsd %xmm1, 0x28(%rsp)
movq 0x60(%rsp), %rax
movslq 0x24(%rax), %r15
leaq (,%r15,4), %rax
testq %r15, %r15
movq $-0x1, %r14
cmovnsq %rax, %r14
movq %r14, %rdi
callq 0x27040
movq %rax, %r13
movq %r12, 0x10(%rsp)
movq %r14, %rdi
callq 0x27040
movq %rax, 0x48(%rsp)
movapd 0x70(%rsp), %xmm1
cvttsd2si %xmm1, %eax
cvttpd2dq %xmm1, %xmm0
cvtdq2pd %xmm0, %xmm0
movsd 0x28(%rsp), %xmm2
mulsd %xmm0, %xmm2
divsd %xmm1, %xmm2
cvttsd2si %xmm2, %ecx
movl %ecx, 0x34(%rsp)
movl %ecx, %ebp
movl %eax, 0x28(%rsp)
imull %eax, %ebp
movslq %ebp, %r14
leaq (,%r14,4), %rdi
testl %r14d, %r14d
movq $-0x1, %r12
cmovsq %r12, %rdi
callq 0x27040
movq %rax, 0x18(%rsp)
shlq $0x3, %r14
movl %ebp, 0x4(%rsp)
testl %ebp, %ebp
cmovsq %r12, %r14
movq %r14, %rdi
callq 0x27040
movq %rax, 0x50(%rsp)
movq %r14, %rdi
callq 0x27040
movq 0x10(%rsp), %r12
movq %rax, %r14
testl %r15d, %r15d
movq 0x48(%rsp), %rdx
jle 0x324d7
xorl %eax, %eax
xorl %ecx, %ecx
movl %eax, (%r13,%rcx,4)
movl %eax, (%rdx,%rcx,4)
incq %rcx
cmpq %r15, %rcx
jl 0x324c7
movslq 0x24(%r12), %rax
testq %rax, %rax
movq %rdx, %r15
jle 0x32503
movq 0x18(%r12), %rcx
xorl %edx, %edx
movq (%rcx,%rdx,8), %rsi
movslq 0x20(%rsi), %rsi
movl $0x1, (%r15,%rsi,4)
incq %rdx
cmpq %rdx, %rax
jne 0x324eb
leaq 0x130(%rsp), %rdi
movq %rbx, %rsi
callq 0x7854c
movq %r14, 0x40(%rsp)
movq %r13, 0x8(%rsp)
leaq 0x130(%rsp), %rdi
movq $0x0, 0x38(%rsp)
xorl %esi, %esi
xorl %edx, %edx
movl 0x28(%rsp), %ecx
movl 0x34(%rsp), %r8d
callq 0x78704
movl 0x4(%rsp), %eax
movq %rax, 0x68(%rsp)
leaq (,%rax,4), %rcx
movq %rcx, 0x80(%rsp)
negq %rax
movq %rax, 0x58(%rsp)
leaq 0x118(%rsp), %rbx
leaq 0xd0(%rsp), %r13
cmpl $0x0, 0x4(%rsp)
movq 0x50(%rsp), %r14
movq 0x40(%rsp), %rbp
jle 0x325bb
movq 0x18(%rsp), %rdi
movl $0xff, %esi
movq 0x80(%rsp), %rdx
callq 0x274b0
movabsq $0x47efffffe0000000, %rdx # imm = 0x47EFFFFFE0000000
movq 0x68(%rsp), %rcx
xorl %eax, %eax
movq %rdx, (%r14,%rax,8)
movq %rdx, (%rbp,%rax,8)
incq %rax
cmpq %rax, %rcx
jne 0x325aa
movq 0x60(%rsp), %rax
movslq 0x24(%rax), %rcx
testq %rcx, %rcx
jle 0x326db
leal 0x3(%rcx), %eax
decq %rcx
movq %rcx, %xmm0
pshufd $0x44, %xmm0, %xmm0     # xmm0 = xmm0[0,1,0,1]
shrl $0x2, %eax
shlq $0x4, %rax
pxor 0x105ef4(%rip), %xmm0   # 0x1384e0
xorl %ecx, %ecx
movdqa 0x105eda(%rip), %xmm1   # 0x1384d0
movdqa 0x105ec2(%rip), %xmm2   # 0x1384c0
movdqa %xmm1, %xmm3
pxor 0x105ed6(%rip), %xmm3   # 0x1384e0
movdqa %xmm3, %xmm4
pcmpgtd %xmm0, %xmm4
pcmpeqd %xmm0, %xmm3
pshufd $0xf5, %xmm3, %xmm5     # xmm5 = xmm3[1,1,3,3]
pand %xmm4, %xmm5
pshufd $0xf5, %xmm4, %xmm3     # xmm3 = xmm4[1,1,3,3]
por %xmm5, %xmm3
movd %xmm3, %edx
notl %edx
testb $0x1, %dl
je 0x3263f
movq 0x8(%rsp), %rdx
movl $0x0, (%rdx,%rcx)
pxor 0x105ec9(%rip), %xmm3   # 0x138510
pextrw $0x4, %xmm3, %edx
testb $0x1, %dl
je 0x3265e
movq 0x8(%rsp), %rdx
movl $0x0, 0x4(%rdx,%rcx)
movdqa %xmm2, %xmm3
pxor 0x105e76(%rip), %xmm3   # 0x1384e0
movdqa %xmm3, %xmm4
pcmpgtd %xmm0, %xmm4
pcmpeqd %xmm0, %xmm3
pshufd $0xf5, %xmm3, %xmm5     # xmm5 = xmm3[1,1,3,3]
pand %xmm4, %xmm5
pshufd $0xf5, %xmm4, %xmm3     # xmm3 = xmm4[1,1,3,3]
por %xmm5, %xmm3
pxor 0x105e80(%rip), %xmm3   # 0x138510
pextrw $0x0, %xmm3, %edx
testb $0x1, %dl
je 0x326a7
movq 0x8(%rsp), %rdx
movl $0x0, 0x8(%rdx,%rcx)
pextrw $0x4, %xmm3, %edx
testb $0x1, %dl
je 0x326be
movq 0x8(%rsp), %rdx
movl $0x0, 0xc(%rdx,%rcx)
movdqa 0x105e2a(%rip), %xmm3   # 0x1384f0
paddq %xmm3, %xmm1
paddq %xmm3, %xmm2
addq $0x10, %rcx
cmpq %rcx, %rax
jne 0x325fe
movl 0x24(%r12), %eax
testl %eax, %eax
jle 0x32957
xorl %ecx, %ecx
movq %rcx, %rdx
movq 0x18(%r12), %rcx
movq %rdx, 0x88(%rsp)
movq (%rcx,%rdx,8), %rcx
movq %rcx, 0x20(%rsp)
movl 0x4c(%rcx), %ecx
testl %ecx, %ecx
jle 0x3293d
xorl %edx, %edx
movq 0x20(%rsp), %rax
movq 0x40(%rax), %rax
movq (%rax,%rdx,8), %r12
cmpl $0x0, 0x8(%r12)
jle 0x32924
movq %rdx, 0x90(%rsp)
movl $0x8, %r14d
xorl %r15d, %r15d
movq (%r12), %rax
xorps %xmm1, %xmm1
cvtss2sd -0x4(%rax,%r14), %xmm1
addsd 0x18(%r12), %xmm1
xorps %xmm0, %xmm0
cvtss2sd -0x8(%rax,%r14), %xmm0
addsd 0x10(%r12), %xmm0
xorps %xmm2, %xmm2
cvtss2sd (%rax,%r14), %xmm2
addsd 0x20(%r12), %xmm2
movq %rbx, %rdi
callq 0xc3d32
movq %r13, %rdi
movq %rbx, %rsi
leaq 0x100(%rsp), %rdx
callq 0xc3f8e
leaq 0xe8(%rsp), %rdi
movq %r13, %rsi
callq 0xc4a52
movapd %xmm0, %xmm1
xorpd %xmm0, %xmm0
movsd %xmm1, 0x70(%rsp)
ucomisd %xmm1, %xmm0
jae 0x327d5
movq %r13, %rdi
leaq 0x130(%rsp), %rsi
movq %rbx, %rdx
callq 0x79598
movq %r13, %rdi
leaq 0x593364(%rip), %rsi    # 0x5c5b30
callq 0xde4b6
testl %eax, %eax
je 0x327ef
incq %r15
movslq 0x8(%r12), %rax
addq $0x30, %r14
cmpq %rax, %r15
jl 0x3273a
jmp 0x3290f
movsd 0xd0(%rsp), %xmm0
addsd 0x105810(%rip), %xmm0   # 0x138010
cvttsd2si %xmm0, %eax
testl %eax, %eax
js 0x327d5
cmpl 0x28(%rsp), %eax
jge 0x327d5
movsd 0xd8(%rsp), %xmm0
addsd 0x1057f1(%rip), %xmm0   # 0x138010
cvttsd2si %xmm0, %ebp
testl %ebp, %ebp
js 0x327d5
cmpl 0x34(%rsp), %ebp
jge 0x327d5
imull 0x28(%rsp), %ebp
addl %eax, %ebp
movsd 0x70(%rsp), %xmm0
movq 0x50(%rsp), %rax
subsd (%rax,%rbp,8), %xmm0
leaq 0x18a93d(%rip), %rax    # 0x1bd188
movapd %xmm0, 0xc0(%rsp)
ucomisd (%rax), %xmm0
ja 0x327d5
movq 0x20(%rsp), %rdi
movl $0x1, %esi
callq 0x40278
testq %rax, %rax
je 0x32897
movq 0x20(%rsp), %rdi
movl $0x1, %esi
callq 0x40278
addq $0x28, %rax
leaq 0x138(%rsp), %rdi
movq %rax, %rsi
callq 0x8bb28
jmp 0x328a2
leaq 0x18a8fa(%rip), %rax    # 0x1bd198
movsd (%rax), %xmm0
movapd 0xc0(%rsp), %xmm2
andpd 0x105c4d(%rip), %xmm2   # 0x138500
movsd 0x105b7d(%rip), %xmm1   # 0x138438
ucomisd %xmm2, %xmm1
jb 0x328e0
movapd %xmm0, %xmm1
movq 0x40(%rsp), %rax
subsd (%rax,%rbp,8), %xmm1
leaq 0x18a8b2(%rip), %rax    # 0x1bd188
ucomisd (%rax), %xmm1
ja 0x327d5
movq 0x20(%rsp), %rax
movl 0x20(%rax), %eax
movq 0x18(%rsp), %rcx
movl %eax, (%rcx,%rbp,4)
movq 0x50(%rsp), %rax
movsd 0x70(%rsp), %xmm1
movsd %xmm1, (%rax,%rbp,8)
movq 0x40(%rsp), %rax
movsd %xmm0, (%rax,%rbp,8)
jmp 0x327d5
movq 0x20(%rsp), %rax
movl 0x4c(%rax), %ecx
movq 0x48(%rsp), %r15
movq 0x90(%rsp), %rdx
incq %rdx
movslq %ecx, %rax
cmpq %rax, %rdx
jl 0x32710
movq 0x10(%rsp), %r12
movl 0x24(%r12), %eax
movq 0x88(%rsp), %rdx
incq %rdx
movslq %eax, %rcx
cmpq %rcx, %rdx
movq %rdx, %rcx
jl 0x326ea
cmpl $0x0, 0x4(%rsp)
jle 0x3298f
xorl %ecx, %ecx
movq 0x18(%rsp), %rdx
movslq (%rdx,%rcx,4), %rdx
testq %rdx, %rdx
js 0x32985
movq 0x8(%rsp), %rsi
cmpl $0x0, (%rsi,%rdx,4)
jne 0x32985
movq 0x8(%rsp), %rsi
movl $0x1, (%rsi,%rdx,4)
incq %rcx
cmpq %rcx, 0x68(%rsp)
jne 0x32960
testl %eax, %eax
jle 0x329d5
xorl %r14d, %r14d
movq 0x18(%r12), %rcx
movq (%rcx,%r14,8), %rsi
movslq 0x20(%rsi), %rcx
movq 0x8(%rsp), %rdx
cmpl $0x1, (%rdx,%rcx,4)
je 0x329ca
movl $0x0, (%r15,%rcx,4)
movq (%r12), %rax
movq %r12, %rdi
xorl %edx, %edx
callq *0x330(%rax)
movl 0x24(%r12), %eax
incq %r14
movslq %eax, %rcx
cmpq %rcx, %r14
jl 0x32996
cmpl $0x0, 0x4(%rsp)
jle 0x32ae4
movb $0x1, %bpl
xorl %r14d, %r14d
movq 0x58(%rsp), %rdx
incq %r14
movq 0x18(%rsp), %rax
movslq -0x4(%rax,%r14,4), %rax
testq %rax, %rax
js 0x32a41
cmpl $0x0, (%r15,%rax,4)
je 0x32a41
movq 0x60(%rsp), %rcx
movq 0x18(%rcx), %rcx
movq (%rcx,%rax,8), %r12
cmpl $0x0, 0x3c(%r12)
je 0x32a41
movl $0x0, (%r15,%rax,4)
movq 0x10(%rsp), %rdi
movq (%rdi), %rax
movq %r12, %rsi
xorl %edx, %edx
callq *0x330(%rax)
cmpl $0x0, 0x3c(%r12)
movq 0x58(%rsp), %rdx
jg 0x32a53
leaq (%rdx,%r14), %rax
incq %rax
incq %r14
cmpq $0x1, %rax
jne 0x329ee
jmp 0x32ac0
xorl %r15d, %r15d
movq 0x30(%r12), %rax
movq (%rax,%r15,8), %rsi
movslq 0x20(%rsi), %rax
movq 0x48(%rsp), %rcx
movl $0x1, (%rcx,%rax,4)
movq 0x10(%rsp), %rdi
movq (%rdi), %rax
xorl %edx, %edx
callq *0x328(%rax)
incq %r15
movslq 0x3c(%r12), %rax
cmpq %rax, %r15
jl 0x32a56
xorl %ebp, %ebp
movq 0x58(%rsp), %rdx
movq %rdx, %rax
addq %r14, %rax
movq 0x48(%rsp), %r15
jne 0x329eb
movq 0x38(%rsp), %rax
incl %eax
movq %rax, 0x38(%rsp)
cmpl $0x8, %eax
movq 0x10(%rsp), %r12
jne 0x32571
jmp 0x32ae4
movq 0x38(%rsp), %rax
incl %eax
movq %rax, 0x38(%rsp)
cmpl $0x8, %eax
sete %al
orb %al, %bpl
testb $0x1, %bpl
movq 0x10(%rsp), %r12
je 0x32571
movq 0x8(%rsp), %rdi
callq 0x27c80
movq %r15, %rdi
callq 0x27c80
movq 0x18(%rsp), %rdi
callq 0x27c80
movq 0x50(%rsp), %rdi
callq 0x27c80
movq 0x40(%rsp), %rdi
callq 0x27c80
movq (%r12), %rax
movq %r12, %rdi
callq *0x378(%rax)
leaq 0x130(%rsp), %rdi
callq 0x785f8
leaq 0x98(%rsp), %rdi
callq 0xdbd54
addq $0x1d8, %rsp            # imm = 0x1D8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x32b6e
jmp 0x32b6e
jmp 0x32b5d
jmp 0x32b5d
jmp 0x32b5d
jmp 0x32b5d
jmp 0x32b5d
jmp 0x32b5d
movq %rax, %rbx
jmp 0x32b7e
jmp 0x32b6e
jmp 0x32b6e
jmp 0x32b6e
jmp 0x32b6e
jmp 0x32b6e
jmp 0x32b6e
movq %rax, %rbx
leaq 0x130(%rsp), %rdi
callq 0x785f8
leaq 0x98(%rsp), %rdi
callq 0xdbd54
movq %rbx, %rdi
callq 0x28120
nop