// File: /workspace/github_cpp_makefile_star2_O1/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), %rcx
testq %rcx, %rcx
je 0x2dfee
movq 0x10(%rcx), %rdx
movq %rdx, (%rsp)
testq %rdx, %rdx
je 0x2dfee
movq %rdi, %rbx
movq %rcx, 0x8(%rsp)
movq 0x8(%rax), %rax
movq 0x18(%rax), %rax
movq (%rax), %rdi
testq %rdi, %rdi
je 0x2dc61
callq 0x3ff5c
movq %rax, %rsi
jmp 0x2dc68
leaq 0x1998f0(%rip), %rsi    # 0x1c7558
leaq 0xa0(%rsp), %r14
movq %r14, %rdi
callq 0xae528
movsd 0x8(%r14), %xmm0
movsd 0x10(%r14), %xmm1
movsd 0x20(%r14), %xmm2
movsd 0x28(%r14), %xmm3
leaq 0x78(%rsp), %rdi
callq 0xdccc0
leaq 0x78(%rsp), %rdi
callq 0xdd064
mulsd 0x10d364(%rip), %xmm0   # 0x13b010
mulsd 0x10d6f4(%rip), %xmm0   # 0x13b3a8
movsd 0x10d694(%rip), %xmm1   # 0x13b350
maxsd %xmm0, %xmm1
leaq 0xd8(%rsp), %rdi
leaq 0x78(%rsp), %rsi
movapd %xmm1, %xmm0
movl $0x5, %edx
movl $0x200, %ecx            # imm = 0x200
callq 0xce536
leaq 0x4f8(%rbx), %r15
leaq 0xd8(%rsp), %rsi
movq %r15, %rdi
callq 0xce734
leaq 0xd8(%rsp), %rdi
callq 0xce9e8
leaq 0xd8(%rsp), %rdi
movq %r15, %rsi
callq 0xce810
movq 0x8(%rsp), %rcx
cmpl $0x0, 0x24(%rcx)
jle 0x2dfb2
xorl %edx, %edx
movq 0x18(%rcx), %rax
movq %rdx, 0x10(%rsp)
movq (%rax,%rdx,8), %rcx
cmpl $0x0, 0x3c(%rcx)
jg 0x2df98
cmpl $0x0, 0x4c(%rcx)
jle 0x2df98
xorl %edx, %edx
movq %rcx, 0x18(%rsp)
movq 0x40(%rcx), %rax
movq %rdx, 0x20(%rsp)
movq (%rax,%rdx,8), %r14
cmpl $0x0, 0xb4(%r14)
jne 0x2dd7f
movq (%rsp), %rdi
movq (%rdi), %rax
movq 0x8(%rdi), %rdx
movl 0x28(%rdi), %ecx
movq %r14, %rsi
callq *0x88(%rax)
testl %eax, %eax
je 0x2dd86
incl 0xb4(%r14)
cmpl $0x0, 0x8(%r14)
jle 0x2df1f
xorl %r12d, %r12d
xorl %r13d, %r13d
movq (%r14), %rax
movzwl 0x2a(%rax,%r12), %ebp
movq (%r14), %rax
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 0xc531e
movswl %bp, %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
divsd 0x10d5c2(%rip), %xmm0   # 0x13b3b0
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 0xdf9d2
movsd 0x38(%rsp), %xmm0
movsd 0x40(%rsp), %xmm1
leaq 0x28(%rsp), %rdi
movq %r15, %rsi
callq 0xd51da
movsd 0x28(%rsp), %xmm0
addsd 0x10d1d1(%rip), %xmm0   # 0x13b010
cvttsd2si %xmm0, %eax
testl %eax, %eax
js 0x2df0b
cmpl %eax, 0x5b8(%rbx)
jle 0x2df0b
movsd 0x30(%rsp), %xmm0
addsd 0x10d1ab(%rip), %xmm0   # 0x13b010
cvttsd2si %xmm0, %ecx
testl %ecx, %ecx
js 0x2df0b
cmpl %ecx, 0x5bc(%rbx)
jle 0x2df0b
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 0x10d50b(%rip), %xmm3   # 0x13b3b8
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 0x10d11b(%rip), %xmm4   # 0x13b008
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 0x2dd97
cmpl $0x1, 0xb4(%r14)
jne 0x2df44
movq (%rsp), %rdi
movq (%rdi), %rax
movq 0x8(%rdi), %rdx
movl 0x28(%rdi), %ecx
movq %r14, %rsi
callq *0x90(%rax)
testl %eax, %eax
je 0x2df7e
decl 0xb4(%r14)
jne 0x2df7e
testb $0x2, 0x89(%r14)
je 0x2df7e
movq (%rsp), %rdi
movq (%rdi), %rax
movq %r14, %rsi
callq *0x18(%rax)
testq %r14, %r14
je 0x2df7e
movq %r14, %rdi
callq 0x363da
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 0x2dd4d
movq 0x10(%rsp), %rdx
incq %rdx
movq 0x8(%rsp), %rcx
movslq 0x24(%rcx), %rax
cmpq %rax, %rdx
jl 0x2dd25
leaq 0xd8(%rsp), %rsi
movq %r15, %rdi
callq 0xd2948
movq %r15, %rdi
callq 0xcf600
leaq 0xd8(%rsp), %rdi
callq 0xce9e8
leaq 0x78(%rsp), %rdi
callq 0xdd540
leaq 0xa0(%rsp), %rdi
callq 0xaf380
addq $0x1a8, %rsp            # imm = 0x1A8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2e006
jmp 0x2e018
jmp 0x2e006
movq %rax, %rbx
jmp 0x2e028
movq %rax, %rbx
jmp 0x2e032
jmp 0x2e018
jmp 0x2e018
jmp 0x2e018
jmp 0x2e018
movq %rax, %rbx
leaq 0xd8(%rsp), %rdi
callq 0xce9e8
leaq 0x78(%rsp), %rdi
callq 0xdd540
leaq 0xa0(%rsp), %rdi
callq 0xaf380
movq %rbx, %rdi
callq 0x28130
nop