// File: 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:
subq $0x358, %rsp            # imm = 0x358
movq %rdi, 0x350(%rsp)
movq 0x350(%rsp), %rax
movq %rax, 0xa8(%rsp)
movq 0x8(%rax), %rdi
callq 0x2f020
movq %rax, 0x348(%rsp)
cmpq $0x0, 0x348(%rsp)
jne 0x31c80
jmp 0x322ad
movq 0x348(%rsp), %rdi
callq 0x3d9a0
movq %rax, 0x340(%rsp)
cmpq $0x0, 0x340(%rsp)
jne 0x31ca5
jmp 0x322ad
movq 0xa8(%rsp), %rax
movq 0x8(%rax), %rdi
callq 0x3d7d0
movq %rax, %rsi
leaq 0x308(%rsp), %rdi
movq %rdi, 0x98(%rsp)
callq 0x133e40
movq 0x98(%rsp), %rdi
callq 0x3d9b0
movsd %xmm0, 0xa0(%rsp)
jmp 0x31ce6
leaq 0x308(%rsp), %rdi
callq 0x3d9d0
movsd %xmm0, 0x90(%rsp)
jmp 0x31cfe
leaq 0x308(%rsp), %rdi
callq 0x3d9f0
movsd %xmm0, 0x88(%rsp)
jmp 0x31d16
leaq 0x308(%rsp), %rdi
callq 0x3da10
movsd %xmm0, 0x80(%rsp)
jmp 0x31d2e
movsd 0x80(%rsp), %xmm3
movsd 0x88(%rsp), %xmm2
movsd 0x90(%rsp), %xmm1
movsd 0xa0(%rsp), %xmm0
leaq 0x2e0(%rsp), %rdi
callq 0x18ef80
jmp 0x31d61
leaq 0x2e0(%rsp), %rdi
callq 0x3da30
movsd %xmm0, 0x78(%rsp)
jmp 0x31d76
movsd 0x78(%rsp), %xmm0
movsd 0x20f7b4(%rip), %xmm1   # 0x241538
divsd %xmm1, %xmm0
movsd %xmm0, 0x2c8(%rsp)
movsd 0x20f74f(%rip), %xmm0   # 0x2414e8
ucomisd 0x2c8(%rsp), %xmm0
jbe 0x31de9
movsd 0x20f73c(%rip), %xmm0   # 0x2414e8
movsd %xmm0, 0x2c8(%rsp)
jmp 0x31de9
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
jmp 0x322c2
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
jmp 0x322b5
movsd 0x2c8(%rsp), %xmm0
leaq 0x1f8(%rsp), %rdi
leaq 0x2e0(%rsp), %rsi
movl $0x5, %edx
movl $0x200, %ecx            # imm = 0x200
callq 0x172c10
jmp 0x31e13
movq 0xa8(%rsp), %rdi
addq $0x4f8, %rdi            # imm = 0x4F8
leaq 0x1f8(%rsp), %rsi
callq 0x172ff0
jmp 0x31e31
leaq 0x1f8(%rsp), %rdi
callq 0x173460
movq 0xa8(%rsp), %rsi
addq $0x4f8, %rsi            # imm = 0x4F8
leaq 0x128(%rsp), %rdi
callq 0x173170
jmp 0x31e5c
movl $0x0, 0x124(%rsp)
movl 0x124(%rsp), %eax
movl %eax, 0x70(%rsp)
movq 0x348(%rsp), %rdi
callq 0x2f030
movl %eax, 0x74(%rsp)
jmp 0x31e85
movl 0x70(%rsp), %eax
movl 0x74(%rsp), %ecx
cmpl %ecx, %eax
jge 0x32252
movq 0x348(%rsp), %rdi
movl 0x124(%rsp), %esi
callq 0x2f1d0
movq %rax, 0x68(%rsp)
jmp 0x31eb0
movq 0x68(%rsp), %rax
movq %rax, 0x118(%rsp)
movq 0x118(%rsp), %rdi
callq 0x2f260
movl %eax, 0x64(%rsp)
jmp 0x31ed0
movl 0x64(%rsp), %eax
cmpl $0x0, %eax
jle 0x31f2a
jmp 0x3223c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
leaq 0x1f8(%rsp), %rdi
callq 0x173460
jmp 0x322b5
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
leaq 0x128(%rsp), %rdi
callq 0x173460
jmp 0x322b5
movl $0x0, 0x114(%rsp)
movl 0x114(%rsp), %eax
movl %eax, 0x5c(%rsp)
movq 0x118(%rsp), %rdi
callq 0x2f200
movl %eax, 0x60(%rsp)
jmp 0x31f53
movl 0x5c(%rsp), %eax
movl 0x60(%rsp), %ecx
cmpl %ecx, %eax
jge 0x3223a
movq 0x118(%rsp), %rdi
movl 0x114(%rsp), %esi
callq 0x2f280
movq %rax, 0x50(%rsp)
jmp 0x31f7e
movq 0x50(%rsp), %rax
movq %rax, 0x108(%rsp)
movq 0x340(%rsp), %rdi
movq 0x108(%rsp), %rsi
callq 0x3da50
jmp 0x31fa2
movl $0x0, 0x104(%rsp)
movl 0x104(%rsp), %eax
movl %eax, 0x48(%rsp)
movq 0x108(%rsp), %rdi
callq 0x2f2b0
movl %eax, 0x4c(%rsp)
jmp 0x31fcb
movl 0x48(%rsp), %eax
movl 0x4c(%rsp), %ecx
cmpl %ecx, %eax
jge 0x3220b
movq 0x108(%rsp), %rdi
movl 0x104(%rsp), %esi
callq 0x3dad0
movsd %xmm0, 0x40(%rsp)
jmp 0x31ff7
movsd 0x40(%rsp), %xmm0
movsd %xmm0, 0xf8(%rsp)
movq 0x108(%rsp), %rsi
movl 0x104(%rsp), %edx
leaq 0xe0(%rsp), %rdi
callq 0x3db10
jmp 0x32024
leaq 0xe0(%rsp), %rdi
callq 0x2ee90
movsd %xmm0, 0x38(%rsp)
jmp 0x32039
movsd 0x38(%rsp), %xmm0
movsd 0xf8(%rsp), %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0xd8(%rsp)
leaq 0xe0(%rsp), %rdi
callq 0x2ee70
movsd %xmm0, 0x30(%rsp)
jmp 0x3206a
leaq 0xe0(%rsp), %rdi
callq 0x2ee80
movsd %xmm0, 0x28(%rsp)
jmp 0x3207f
movsd 0x28(%rsp), %xmm1
movsd 0x30(%rsp), %xmm0
leaq 0xc8(%rsp), %rdi
callq 0x194a20
jmp 0x3209a
movq 0xa8(%rsp), %rsi
addq $0x4f8, %rsi            # imm = 0x4F8
leaq 0xb8(%rsp), %rdi
leaq 0xc8(%rsp), %rdx
callq 0x3dc10
jmp 0x320c0
leaq 0xb8(%rsp), %rdi
callq 0x3dc80
movsd %xmm0, 0x20(%rsp)
jmp 0x320d5
movsd 0x20(%rsp), %xmm0
movsd 0x20ef55(%rip), %xmm1   # 0x241038
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0xb4(%rsp)
cmpl $0x0, 0xb4(%rsp)
jl 0x3212d
movq 0xa8(%rsp), %rdi
movl 0xb4(%rsp), %eax
movl %eax, 0x18(%rsp)
addq $0x4f8, %rdi            # imm = 0x4F8
callq 0x3dc90
movl %eax, 0x1c(%rsp)
jmp 0x32121
movl 0x18(%rsp), %eax
movl 0x1c(%rsp), %ecx
cmpl %ecx, %eax
jl 0x32132
jmp 0x321f5
leaq 0xb8(%rsp), %rdi
callq 0x3dcb0
movsd %xmm0, 0x10(%rsp)
jmp 0x32147
movsd 0x10(%rsp), %xmm0
movsd 0x20eee3(%rip), %xmm1   # 0x241038
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0xb0(%rsp)
cmpl $0x0, 0xb0(%rsp)
jl 0x3219f
movq 0xa8(%rsp), %rdi
movl 0xb0(%rsp), %eax
movl %eax, 0x8(%rsp)
addq $0x4f8, %rdi            # imm = 0x4F8
callq 0x3dcc0
movl %eax, 0xc(%rsp)
jmp 0x32193
movl 0x8(%rsp), %eax
movl 0xc(%rsp), %ecx
cmpl %ecx, %eax
jl 0x321a1
jmp 0x321f5
movq 0xa8(%rsp), %rdi
addq $0x4f8, %rdi            # imm = 0x4F8
movl 0xb4(%rsp), %esi
movl 0xb0(%rsp), %edx
movsd 0xd8(%rsp), %xmm0
callq 0x3dce0
jmp 0x321ce
movl 0xb4(%rsp), %esi
movl 0xb0(%rsp), %edx
leaq 0x128(%rsp), %rdi
movsd 0x20ee24(%rip), %xmm0   # 0x241010
callq 0x3dce0
jmp 0x321f3
jmp 0x321f5
movl 0x104(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x104(%rsp)
jmp 0x31fad
movq 0x340(%rsp), %rdi
movq 0x108(%rsp), %rsi
callq 0x3dd80
jmp 0x32222
jmp 0x32224
movl 0x114(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x114(%rsp)
jmp 0x31f35
jmp 0x3223c
movl 0x124(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x124(%rsp)
jmp 0x31e67
movq 0xa8(%rsp), %rdi
addq $0x4f8, %rdi            # imm = 0x4F8
leaq 0x128(%rsp), %rsi
callq 0x17a6d0
jmp 0x32270
movq 0xa8(%rsp), %rdi
addq $0x4f8, %rdi            # imm = 0x4F8
callq 0x174a30
jmp 0x32286
leaq 0x128(%rsp), %rdi
callq 0x173460
leaq 0x2e0(%rsp), %rdi
callq 0x3de80
leaq 0x308(%rsp), %rdi
callq 0x3d220
addq $0x358, %rsp            # imm = 0x358
retq
leaq 0x2e0(%rsp), %rdi
callq 0x3de80
leaq 0x308(%rsp), %rdi
callq 0x3d220
movq 0x2d8(%rsp), %rdi
callq 0x292e0
nopl (%rax)