// File: tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelUtils.cpp
// Function: gaps::CreateGrid(gaps::R3SurfelScene*, gaps::R3Box const&, double, int)
R3Grid *
CreateGrid(R3SurfelScene *scene, const R3Box& bbox,
  RNLength grid_spacing, int max_resolution)
{
  // Compute grid resolution
  int xres = (int) (bbox.XLength() / grid_spacing + 0.5);
  int yres = (int) (bbox.YLength() / grid_spacing + 0.5);
  int zres = (int) (bbox.ZLength() / grid_spacing + 0.5);
  if (xres > max_resolution) xres = max_resolution;
  if (yres > max_resolution) yres = max_resolution;
  if (zres > max_resolution) zres = max_resolution;
  if (xres < 2) xres = 2;
  if (yres < 2) yres = 2;
  if (zres < 2) zres = 2;
  
  // Create grid
  R3Grid *grid = new R3Grid(xres, yres, zres, bbox);
  if (!grid) {
    RNFail("Unable to allocate grid\n");
    return NULL;
  }

  // Rasterize points into grid
  R3SurfelTree *tree = scene->Tree();
  R3SurfelDatabase *database = tree->Database();
  for (int i = 0; i < tree->NNodes(); i++) {
    R3SurfelNode *node = tree->Node(i);
    if (node->NParts() > 0) continue;
    if (!R3Intersects(bbox, node->BBox())) continue;
    for (int j = 0; j < node->NBlocks(); j++) {
      R3SurfelBlock *block = node->Block(j);
      if (!R3Intersects(bbox, block->BBox())) continue;
      const R3Point& origin = block->PositionOrigin();
      database->ReadBlock(block);
      for (int k = 0; k < block->NSurfels(); k++) {
        const R3Surfel *surfel = block->Surfel(k);
        double px = origin.X() + surfel->X();
        double py = origin.Y() + surfel->Y();
        double pz = origin.Z() + surfel->Z();
        R3Point position(px, py, pz);
        if (!R3Intersects(bbox, position)) continue;
        grid->RasterizeWorldPoint(position, 1.0);
      }
      database->ReleaseBlock(block);
    }
  }

  // Return grid
  return grid;
}

// Assembly:
subq $0x108, %rsp            # imm = 0x108
movq %rdi, 0xf8(%rsp)
movq %rsi, 0xf0(%rsp)
movsd %xmm0, 0xe8(%rsp)
movl %edx, 0xe4(%rsp)
movq 0xf0(%rsp), %rdi
callq 0x44130
divsd 0xe8(%rsp), %xmm0
movsd 0x168c93(%rip), %xmm1   # 0x1a4158
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0xe0(%rsp)
movq 0xf0(%rsp), %rdi
callq 0x44150
divsd 0xe8(%rsp), %xmm0
movsd 0x168c66(%rip), %xmm1   # 0x1a4158
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0xdc(%rsp)
movq 0xf0(%rsp), %rdi
callq 0x44170
divsd 0xe8(%rsp), %xmm0
movsd 0x168c39(%rip), %xmm1   # 0x1a4158
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0xd8(%rsp)
movl 0xe0(%rsp), %eax
cmpl 0xe4(%rsp), %eax
jle 0x3b54c
movl 0xe4(%rsp), %eax
movl %eax, 0xe0(%rsp)
movl 0xdc(%rsp), %eax
cmpl 0xe4(%rsp), %eax
jle 0x3b56a
movl 0xe4(%rsp), %eax
movl %eax, 0xdc(%rsp)
movl 0xd8(%rsp), %eax
cmpl 0xe4(%rsp), %eax
jle 0x3b588
movl 0xe4(%rsp), %eax
movl %eax, 0xd8(%rsp)
cmpl $0x2, 0xe0(%rsp)
jge 0x3b59d
movl $0x2, 0xe0(%rsp)
cmpl $0x2, 0xdc(%rsp)
jge 0x3b5b2
movl $0x2, 0xdc(%rsp)
cmpl $0x2, 0xd8(%rsp)
jge 0x3b5c7
movl $0x2, 0xd8(%rsp)
movl $0x250, %edi            # imm = 0x250
callq 0x13570
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x40(%rsp)
movl 0xe0(%rsp), %esi
movl 0xdc(%rsp), %edx
movl 0xd8(%rsp), %ecx
movq 0xf0(%rsp), %r8
callq 0xdef40
jmp 0x3b600
movq 0x40(%rsp), %rax
movq %rax, 0xd0(%rsp)
cmpq $0x0, 0xd0(%rsp)
jne 0x3b65f
leaq 0x169666(%rip), %rdi    # 0x1a4c85
movb $0x0, %al
callq 0x1464b0
movq $0x0, 0x100(%rsp)
jmp 0x3b96c
movq 0x40(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xc8(%rsp)
movl %eax, 0xc4(%rsp)
movl $0x250, %esi            # imm = 0x250
callq 0x13580
jmp 0x3b97c
movq 0xf8(%rsp), %rdi
callq 0x16ce0
movq %rax, 0xb8(%rsp)
movq 0xb8(%rsp), %rdi
callq 0x16d10
movq %rax, 0xb0(%rsp)
movl $0x0, 0xac(%rsp)
movl 0xac(%rsp), %eax
movl %eax, 0x3c(%rsp)
movq 0xb8(%rsp), %rdi
callq 0x16cf0
movl %eax, %ecx
movl 0x3c(%rsp), %eax
cmpl %ecx, %eax
jge 0x3b95c
movq 0xb8(%rsp), %rdi
movl 0xac(%rsp), %esi
callq 0x30a90
movq %rax, 0xa0(%rsp)
movq 0xa0(%rsp), %rdi
callq 0x31660
cmpl $0x0, %eax
jle 0x3b6ed
jmp 0x3b946
movq 0xf0(%rsp), %rax
movq %rax, 0x30(%rsp)
movq 0xa0(%rsp), %rdi
callq 0x5ee30
movq 0x30(%rsp), %rdi
movq %rax, %rsi
xorl %eax, %eax
movl %eax, %edx
callq 0x887d0
cmpl $0x0, %eax
jne 0x3b722
jmp 0x3b946
movl $0x0, 0x9c(%rsp)
movl 0x9c(%rsp), %eax
movl %eax, 0x2c(%rsp)
movq 0xa0(%rsp), %rdi
callq 0x186b0
movl %eax, %ecx
movl 0x2c(%rsp), %eax
cmpl %ecx, %eax
jge 0x3b944
movq 0xa0(%rsp), %rdi
movl 0x9c(%rsp), %esi
callq 0x186d0
movq %rax, 0x90(%rsp)
movq 0xf0(%rsp), %rax
movq %rax, 0x20(%rsp)
movq 0x90(%rsp), %rdi
callq 0x4a500
movq 0x20(%rsp), %rdi
movq %rax, %rsi
xorl %eax, %eax
movl %eax, %edx
callq 0x887d0
cmpl $0x0, %eax
jne 0x3b7a4
jmp 0x3b92e
movq 0x90(%rsp), %rdi
callq 0x189c0
movq %rax, 0x88(%rsp)
movq 0xb0(%rsp), %rdi
movq 0x90(%rsp), %rsi
callq 0x31140
movl $0x0, 0x84(%rsp)
movl 0x84(%rsp), %eax
movl %eax, 0x1c(%rsp)
movq 0x90(%rsp), %rdi
callq 0x1c0e0
movl %eax, %ecx
movl 0x1c(%rsp), %eax
cmpl %ecx, %eax
jge 0x3b919
movq 0x90(%rsp), %rdi
movl 0x84(%rsp), %esi
callq 0x43660
movq %rax, 0x78(%rsp)
movq 0x88(%rsp), %rdi
callq 0x187b0
movsd %xmm0, (%rsp)
movq 0x78(%rsp), %rdi
callq 0x44190
movaps %xmm0, %xmm1
movsd (%rsp), %xmm0
cvtss2sd %xmm1, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, 0x70(%rsp)
movq 0x88(%rsp), %rdi
callq 0x187c0
movsd %xmm0, 0x8(%rsp)
movq 0x78(%rsp), %rdi
callq 0x441a0
movaps %xmm0, %xmm1
movsd 0x8(%rsp), %xmm0
cvtss2sd %xmm1, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, 0x68(%rsp)
movq 0x88(%rsp), %rdi
callq 0x187a0
movsd %xmm0, 0x10(%rsp)
movq 0x78(%rsp), %rdi
callq 0x441b0
movaps %xmm0, %xmm1
movsd 0x10(%rsp), %xmm0
cvtss2sd %xmm1, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, 0x60(%rsp)
movsd 0x70(%rsp), %xmm0
movsd 0x68(%rsp), %xmm1
movsd 0x60(%rsp), %xmm2
leaq 0x48(%rsp), %rdi
callq 0xf8350
movq 0xf0(%rsp), %rdi
leaq 0x48(%rsp), %rsi
callq 0x441c0
cmpl $0x0, %eax
jne 0x3b8e7
jmp 0x3b903
movq 0xd0(%rsp), %rdi
leaq 0x48(%rsp), %rsi
movsd 0x16886c(%rip), %xmm0   # 0x1a4168
xorl %edx, %edx
callq 0x43ff0
movl 0x84(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x84(%rsp)
jmp 0x3b7d9
movq 0xb0(%rsp), %rdi
movq 0x90(%rsp), %rsi
callq 0x311c0
movl 0x9c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x9c(%rsp)
jmp 0x3b72d
jmp 0x3b946
movl 0xac(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xac(%rsp)
jmp 0x3b694
movq 0xd0(%rsp), %rax
movq %rax, 0x100(%rsp)
movq 0x100(%rsp), %rax
addq $0x108, %rsp            # imm = 0x108
retq
movq 0xc8(%rsp), %rdi
callq 0x13970
nopl (%rax)