// File: /workspace/github_cpp_makefile_star2_O2/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 $0x1b8, %rsp            # imm = 0x1B8
movq %rdi, %rbx
movq 0x8(%rdi), %rdi
movq 0x8(%rdi), %r15
testq %r15, %r15
je 0x2cec6
movq 0x10(%r15), %r12
testq %r12, %r12
je 0x2cec6
callq 0x32960
leaq 0xb0(%rsp), %r14
movq %r14, %rdi
movq %rax, %rsi
callq 0x9de5a
movsd 0x8(%r14), %xmm0
movsd 0x10(%r14), %xmm1
movsd 0x20(%r14), %xmm2
movsd 0x28(%r14), %xmm3
leaq 0x78(%rsp), %rdi
callq 0xc75ba
leaq 0x78(%rsp), %rdi
callq 0x329f2
mulsd 0xed76e(%rip), %xmm0    # 0x11a3a8
movsd 0xed70e(%rip), %xmm1    # 0x11a350
maxsd %xmm0, %xmm1
leaq 0xe8(%rsp), %rdi
leaq 0x78(%rsp), %rsi
pushq $0x5
popq %rdx
movapd %xmm1, %xmm0
movl $0x200, %ecx            # imm = 0x200
callq 0xba4a4
leaq 0x4f8(%rbx), %r14
leaq 0xe8(%rsp), %rsi
movq %r14, %rdi
callq 0xba6a6
movq %r15, 0x10(%rsp)
leaq 0xe8(%rsp), %rdi
callq 0xba93c
leaq 0xe8(%rsp), %rdi
movq %r14, 0x8(%rsp)
movq %r14, %rsi
callq 0xba784
xorl %edx, %edx
leaq 0xa0(%rsp), %r13
movq %r12, 0x20(%rsp)
movq 0x10(%rsp), %rcx
movslq 0x24(%rcx), %rax
cmpq %rax, %rdx
jge 0x2ce85
movq 0x18(%rcx), %rax
movq %rdx, 0x18(%rsp)
movq (%rax,%rdx,8), %rcx
cmpl $0x0, 0x3c(%rcx)
jle 0x2cce0
movq 0x18(%rsp), %rdx
incq %rdx
jmp 0x2ccb1
xorl %edx, %edx
movq %rcx, 0x28(%rsp)
movslq 0x4c(%rcx), %rax
cmpq %rax, %rdx
jge 0x2ccd6
movq 0x40(%rcx), %rax
movq %rdx, 0x30(%rsp)
movq (%rax,%rdx,8), %r14
movq %r12, %rdi
movq %r14, %rsi
callq 0x32a02
pushq $0x2a
popq %rbp
xorl %r12d, %r12d
movslq 0x8(%r14), %rax
cmpq %rax, %r12
jge 0x2ce63
movq (%r14), %rax
movzwl (%rax,%rbp), %r15d
leaq 0x48(%rsp), %rdi
movq %r14, %rsi
movl %r12d, %edx
callq 0x32a3e
movswl %r15w, %eax
cvtsi2sd %eax, %xmm0
divsd 0xed66d(%rip), %xmm0    # 0x11a3b0
cvtsd2ss %xmm0, %xmm0
cvtss2sd %xmm0, %xmm0
addsd 0x58(%rsp), %xmm0
movapd %xmm0, 0x60(%rsp)
movsd 0x48(%rsp), %xmm0
movsd 0x50(%rsp), %xmm1
movq %r13, %rdi
callq 0xc9fbc
leaq 0x38(%rsp), %rdi
movq 0x8(%rsp), %rsi
movq %r13, %rdx
callq 0x32a76
movsd 0x38(%rsp), %xmm0
addsd 0xed285(%rip), %xmm0    # 0x11a010
cvttsd2si %xmm0, %eax
testl %eax, %eax
js 0x2ce57
cmpl %eax, 0x5b8(%rbx)
jle 0x2ce57
movsd 0x40(%rsp), %xmm0
addsd 0xed25f(%rip), %xmm0    # 0x11a010
cvttsd2si %xmm0, %ecx
testl %ecx, %ecx
js 0x2ce57
cmpl %ecx, 0x5bc(%rbx)
jle 0x2ce57
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 0xed5bf(%rip), %xmm3    # 0x11a3b8
cmpeqsd %xmm3, %xmm0
movapd %xmm0, %xmm2
andnpd %xmm1, %xmm2
andpd %xmm4, %xmm0
orpd %xmm2, %xmm0
movlpd %xmm0, (%rdx,%rsi,8)
imull 0x1b0(%rsp), %ecx
movq 0x1a0(%rsp), %rdx
addl %eax, %ecx
movslq %ecx, %rax
movsd (%rdx,%rax,8), %xmm0
movapd %xmm0, %xmm1
movsd 0xed1cf(%rip), %xmm4    # 0x11a008
addsd %xmm4, %xmm1
cmpeqsd %xmm3, %xmm0
movapd %xmm0, %xmm2
andnpd %xmm1, %xmm2
andpd %xmm4, %xmm0
orpd %xmm2, %xmm0
movlpd %xmm0, (%rdx,%rax,8)
incq %r12
addq $0x30, %rbp
jmp 0x2cd0e
movq 0x20(%rsp), %r12
movq %r12, %rdi
movq %r14, %rsi
callq 0x32a8e
movq 0x30(%rsp), %rdx
incq %rdx
movq 0x28(%rsp), %rcx
jmp 0x2cce7
leaq 0xe8(%rsp), %rsi
movq 0x8(%rsp), %rbx
movq %rbx, %rdi
callq 0xbe2fa
movq %rbx, %rdi
callq 0xbb45c
leaq 0xe8(%rsp), %rdi
callq 0xba93c
leaq 0x78(%rsp), %rdi
callq 0xc7d1e
leaq 0xb0(%rsp), %rdi
callq 0x9eb12
addq $0x1b8, %rsp            # imm = 0x1B8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2cede
jmp 0x2cef0
jmp 0x2cede
movq %rax, %rbx
jmp 0x2cf00
movq %rax, %rbx
jmp 0x2cf0a
jmp 0x2cef0
jmp 0x2cef0
jmp 0x2cef0
jmp 0x2cef0
movq %rax, %rbx
leaq 0xe8(%rsp), %rdi
callq 0xba93c
leaq 0x78(%rsp), %rdi
callq 0xc7d1e
leaq 0xb0(%rsp), %rdi
callq 0x9eb12
movq %rbx, %rdi
callq 0x28140
nop