// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/R3Utils/R3SurfelViewer.cpp
// Function: gaps::R3SurfelViewer::UpdateGroundZGrid()
void R3SurfelViewer::
UpdateGroundZGrid(void)
{
  // Get convenient variables
  R3SurfelTree *tree = scene->Tree();
  if (!tree) return;
  R3SurfelDatabase *database = tree->Database();
  if (!database) return;
 
  // Initialize grids
  R3Box b = scene->BBox();
  R2Box grid_box(b.XMin(), b.YMin(), b.XMax(), b.YMax());
  RNScalar spacing = grid_box.DiagonalRadius() / 128.0;
  if (spacing < 0.1) spacing = 0.1;
  ground_z_grid = R2Grid(grid_box, spacing, 5, 512);
  R2Grid weight_grid = ground_z_grid;

  // Fill grids
  for (int i = 0; i < tree->NNodes(); i++) {
    R3SurfelNode *node = tree->Node(i);
    if (node->NParts() > 0) continue;
    for (int j = 0; j < node->NBlocks(); j++) {
      R3SurfelBlock *block = node->Block(j);
      database->ReadBlock(block);
      for (int k = 0; k < block->NSurfels(); k++) {
        double elevation = block->SurfelElevation(k);
        R3Point sfl_position = block->SurfelPosition(k);
        double ground_z = sfl_position.Z() - elevation;
        R2Point world_position(sfl_position.X(), sfl_position.Y());
        R2Point grid_position = ground_z_grid.GridPosition(world_position);
        int ix = (int) (grid_position.X() + 0.5);
        if ((ix < 0) || (ix >= ground_z_grid.XResolution())) continue;
        int iy = (int) (grid_position.Y() + 0.5);
        if ((iy < 0) || (iy >= ground_z_grid.YResolution())) continue;
        ground_z_grid.AddGridValue(ix, iy, ground_z);
        weight_grid.AddGridValue(ix, iy, 1.0);
      }
      database->ReleaseBlock(block);
    }
  }

  // Compute average Z at ground
  ground_z_grid.Divide(weight_grid);

  // Fill holes
  ground_z_grid.FillHoles();
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1a8, %rsp            # imm = 0x1A8
movq 0x8(%rdi), %rax
movq 0x8(%rax), %rax
testq %rax, %rax
je 0x2deb2
movq 0x10(%rax), %rcx
movq %rcx, (%rsp)
testq %rcx, %rcx
je 0x2deb2
movq %rdi, %rbx
movq %rax, 0x8(%rsp)
movq 0x18(%rax), %rax
movq (%rax), %rdi
testq %rdi, %rdi
je 0x2db15
callq 0x4003c
movq %rax, %rsi
jmp 0x2db1c
leaq 0x196a3c(%rip), %rsi    # 0x1c4558
leaq 0xa0(%rsp), %r14
movq %r14, %rdi
callq 0xad2c4
movsd 0x8(%r14), %xmm0
movsd 0x10(%r14), %xmm1
movsd 0x20(%r14), %xmm2
movsd 0x28(%r14), %xmm3
leaq 0x78(%rsp), %rdi
callq 0xdb4dc
leaq 0x78(%rsp), %rdi
callq 0xdb872
mulsd 0x10a4b0(%rip), %xmm0   # 0x138010
mulsd 0x10a840(%rip), %xmm0   # 0x1383a8
movsd 0x10a7e0(%rip), %xmm1   # 0x138350
maxsd %xmm0, %xmm1
leaq 0xd8(%rsp), %rdi
leaq 0x78(%rsp), %rsi
movapd %xmm1, %xmm0
movl $0x5, %edx
movl $0x200, %ecx            # imm = 0x200
callq 0xcce28
leaq 0x4f8(%rbx), %r15
leaq 0xd8(%rsp), %rsi
movq %r15, %rdi
callq 0xcd016
leaq 0xd8(%rsp), %rdi
callq 0xcd2d8
leaq 0xd8(%rsp), %rdi
movq %r15, %rsi
callq 0xcd100
movq 0x8(%rsp), %rdx
movl 0x24(%rdx), %eax
testl %eax, %eax
jle 0x2de76
xorl %esi, %esi
movq 0x18(%rdx), %rcx
movq (%rcx,%rsi,8), %rcx
cmpl $0x0, 0x3c(%rcx)
jg 0x2de67
cmpl $0x0, 0x4c(%rcx)
jle 0x2de67
movq %rsi, 0x10(%rsp)
xorl %edx, %edx
movq %rcx, 0x18(%rsp)
movq 0x40(%rcx), %rax
movq %rdx, 0x20(%rsp)
movq (%rax,%rdx,8), %r14
movl 0xb4(%r14), %eax
testl %eax, %eax
jne 0x2dc3c
movq (%rsp), %rdi
movq (%rdi), %rax
movq 0x8(%rdi), %rdx
movl 0x28(%rdi), %ecx
movq %r14, %rsi
callq *0x88(%rax)
testl %eax, %eax
je 0x2dc45
movl 0xb4(%r14), %eax
incl %eax
movl %eax, 0xb4(%r14)
cmpl $0x0, 0x8(%r14)
jle 0x2dddb
xorl %r12d, %r12d
xorl %r13d, %r13d
movq (%r14), %rax
movzwl 0x2a(%rax,%r12), %ebp
xorps %xmm0, %xmm0
cvtss2sd (%rax,%r12), %xmm0
addsd 0x10(%r14), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rax,%r12), %xmm1
addsd 0x18(%r14), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rax,%r12), %xmm2
addsd 0x20(%r14), %xmm2
leaq 0x48(%rsp), %rdi
callq 0xc3d32
movswl %bp, %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
divsd 0x10a706(%rip), %xmm0   # 0x1383b0
cvtsd2ss %xmm0, %xmm0
cvtss2sd %xmm0, %xmm0
addsd 0x58(%rsp), %xmm0
movapd %xmm0, 0x60(%rsp)
movsd 0x48(%rsp), %xmm0
movsd 0x50(%rsp), %xmm1
leaq 0x38(%rsp), %rdi
callq 0xde1e6
movsd 0x38(%rsp), %xmm0
movsd 0x40(%rsp), %xmm1
leaq 0x28(%rsp), %rdi
movq %r15, %rsi
callq 0xd39e2
movsd 0x28(%rsp), %xmm0
addsd 0x10a315(%rip), %xmm0   # 0x138010
cvttsd2si %xmm0, %eax
testl %eax, %eax
js 0x2ddc7
cmpl %eax, 0x5b8(%rbx)
jle 0x2ddc7
movsd 0x30(%rsp), %xmm0
addsd 0x10a2ef(%rip), %xmm0   # 0x138010
cvttsd2si %xmm0, %ecx
testl %ecx, %ecx
js 0x2ddc7
cmpl %ecx, 0x5bc(%rbx)
jle 0x2ddc7
movq 0x5b0(%rbx), %rdx
movl 0x5c0(%rbx), %esi
imull %ecx, %esi
addl %eax, %esi
movslq %esi, %rsi
movsd (%rdx,%rsi,8), %xmm0
movapd 0x60(%rsp), %xmm4
movapd %xmm4, %xmm1
addsd %xmm0, %xmm1
movsd 0x10a64f(%rip), %xmm3   # 0x1383b8
cmpeqsd %xmm3, %xmm0
movapd %xmm0, %xmm2
andnpd %xmm1, %xmm2
andpd %xmm4, %xmm0
orpd %xmm2, %xmm0
movlpd %xmm0, (%rdx,%rsi,8)
imull 0x1a0(%rsp), %ecx
movq 0x190(%rsp), %rdx
addl %eax, %ecx
movslq %ecx, %rax
movsd (%rdx,%rax,8), %xmm0
movapd %xmm0, %xmm1
movsd 0x10a25f(%rip), %xmm4   # 0x138008
addsd %xmm4, %xmm1
cmpeqsd %xmm3, %xmm0
movapd %xmm0, %xmm2
andnpd %xmm1, %xmm2
andpd %xmm4, %xmm0
orpd %xmm2, %xmm0
movlpd %xmm0, (%rdx,%rax,8)
incq %r13
movslq 0x8(%r14), %rax
addq $0x30, %r12
cmpq %rax, %r13
jl 0x2dc56
movl 0xb4(%r14), %eax
cmpl $0x1, %eax
jne 0x2de09
movq (%rsp), %rdi
movq (%rdi), %rax
movq 0x8(%rdi), %rdx
movl 0x28(%rdi), %ecx
movq %r14, %rsi
callq *0x90(%rax)
testl %eax, %eax
je 0x2de40
movl 0xb4(%r14), %eax
decl %eax
movl %eax, 0xb4(%r14)
jne 0x2de40
testb $0x2, 0x89(%r14)
je 0x2de40
movq (%rsp), %rdi
movq (%rdi), %rax
movq %r14, %rsi
callq *0x18(%rax)
movq %r14, %rdi
callq 0x36372
movl $0xc8, %esi
movq %r14, %rdi
callq 0x27930
movq 0x20(%rsp), %rdx
incq %rdx
movq 0x18(%rsp), %rcx
movslq 0x4c(%rcx), %rax
cmpq %rax, %rdx
jl 0x2dc02
movq 0x8(%rsp), %rdx
movl 0x24(%rdx), %eax
movq 0x10(%rsp), %rsi
incq %rsi
movslq %eax, %rcx
cmpq %rcx, %rsi
jl 0x2dbda
leaq 0xd8(%rsp), %rsi
movq %r15, %rdi
callq 0xd1172
movq %r15, %rdi
callq 0xcdf04
leaq 0xd8(%rsp), %rdi
callq 0xcd2d8
leaq 0x78(%rsp), %rdi
callq 0xdbd54
leaq 0xa0(%rsp), %rdi
callq 0xae0e4
addq $0x1a8, %rsp            # imm = 0x1A8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2deca
jmp 0x2dedc
jmp 0x2deca
movq %rax, %rbx
jmp 0x2deec
movq %rax, %rbx
jmp 0x2def6
jmp 0x2dedc
jmp 0x2dedc
jmp 0x2dedc
jmp 0x2dedc
movq %rax, %rbx
leaq 0xd8(%rsp), %rdi
callq 0xcd2d8
leaq 0x78(%rsp), %rdi
callq 0xdbd54
leaq 0xa0(%rsp), %rdi
callq 0xae0e4
movq %rbx, %rdi
callq 0x28120
nop