// File: /workspace/github_cpp_makefile_star2_O1/tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelPointSet.cpp
// Function: gaps::R3SurfelPointSet::InsertPoints(gaps::R3SurfelBlock*, gaps::R3Point const&, double, double, double)
void R3SurfelPointSet::
InsertPoints(R3SurfelBlock *block, const R3Point& center, RNLength radius, RNCoord zmin, RNCoord zmax)
{
  // Check block
  if (block->NSurfels() == 0) return;

  // Check and update bounding box (conservatively)
  R3Box constraint_box(center[0] - radius, center[1] - radius, zmin, center[0] + radius, center[1] + radius, zmax);
  R3Box intersection_box = constraint_box;
  intersection_box.Intersect(block->BBox());
  if (intersection_box.IsEmpty()) return;
  bbox.Union(intersection_box);

  // Update timestamp range (conservatively)
  timestamp_range.Union(block->TimestampRange());

  // Translate constraint to block coordinate system (and store in floats)
  const R3Point& origin = block->PositionOrigin();
  float xc = center[0] - origin[0];
  float yc = center[1] - origin[1];
  float zlo = zmin - origin[2];
  float zhi = zmax - origin[2];
  float rr = radius * radius;

  // Allocate space for points
  AllocatePoints(npoints + block->NSurfels());

  // Read block
  if (block->database) block->database->ReadBlock(block);

  // Copy points
  for (int i = 0; i < block->NSurfels(); i++) {
    const R3Surfel *surfel = block->Surfel(i);
    if (surfel->Z() < zlo) continue;
    if (surfel->Z() > zhi) continue;
    float dx = surfel->X() - xc;
    float dy = surfel->Y() - yc;
    float dd = dx*dx + dy*dy;
    if (dd > rr) continue;
    points[npoints].Reset(block, surfel);
    npoints++;
  }

  // Release block
  if (block->database) block->database->ReleaseBlock(block);
}

// Assembly:
pushq %r15
pushq %r14
pushq %r12
pushq %rbx
subq $0xa8, %rsp
cmpl $0x0, 0x8(%rsi)
je 0x33a4e
movaps %xmm2, %xmm5
movaps %xmm1, %xmm6
movapd %xmm0, %xmm2
movq %rdx, %r15
movq %rsi, %rbx
movq %rdi, %r14
movsd (%rdx), %xmm3
movsd 0x8(%rdx), %xmm4
movapd %xmm3, %xmm0
subsd %xmm2, %xmm0
movapd %xmm4, %xmm1
subsd %xmm2, %xmm1
addsd %xmm2, %xmm3
movsd %xmm2, 0x18(%rsp)
addsd %xmm2, %xmm4
leaq 0x70(%rsp), %r12
movq %r12, %rdi
movsd %xmm6, 0x8(%rsp)
movaps %xmm6, %xmm2
movsd %xmm5, 0x10(%rsp)
callq 0x6e028
leaq 0x38(%rsp), %rdi
movq %r12, %rsi
callq 0x6df88
movq %rbx, %rdi
callq 0x2cfc6
leaq 0x38(%rsp), %rdi
movq %rax, %rsi
callq 0x6ebb4
movsd 0x40(%rsp), %xmm0
ucomisd 0x58(%rsp), %xmm0
ja 0x33a3a
movsd 0x48(%rsp), %xmm0
ucomisd 0x60(%rsp), %xmm0
ja 0x33a3a
movsd 0x50(%rsp), %xmm0
ucomisd 0x68(%rsp), %xmm0
ja 0x33a3a
leaq 0x18(%r14), %rdi
leaq 0x38(%rsp), %rsi
callq 0x6ea70
movq %rbx, %rdi
callq 0x2d262
movsd (%rax), %xmm0
movsd 0x50(%r14), %xmm1
ucomisd %xmm0, %xmm1
jbe 0x3384e
movsd %xmm0, 0x50(%r14)
movsd 0x8(%rax), %xmm0
ucomisd 0x58(%r14), %xmm0
jbe 0x33861
movsd %xmm0, 0x58(%r14)
movsd (%r15), %xmm0
subsd 0x10(%rbx), %xmm0
movsd %xmm0, 0x28(%rsp)
movsd 0x8(%r15), %xmm0
subsd 0x18(%rbx), %xmm0
movsd %xmm0, 0x30(%rsp)
movsd 0x20(%rbx), %xmm0
movsd %xmm0, 0x20(%rsp)
movl 0x8(%rbx), %esi
addl 0x10(%r14), %esi
movq (%r14), %rax
movq %r14, %rdi
callq *0x80(%rax)
movq 0x98(%rbx), %rdi
testq %rdi, %rdi
movsd 0x18(%rsp), %xmm0
movsd 0x10(%rsp), %xmm1
movsd 0x8(%rsp), %xmm2
je 0x338f6
cmpl $0x0, 0xb4(%rbx)
jne 0x338f0
movq (%rdi), %rax
movq 0x8(%rdi), %rdx
movl 0x28(%rdi), %ecx
movq %rbx, %rsi
callq *0x88(%rax)
testl %eax, %eax
movsd 0x18(%rsp), %xmm0
movsd 0x10(%rsp), %xmm1
movsd 0x8(%rsp), %xmm2
je 0x338f6
incl 0xb4(%rbx)
cmpl $0x0, 0x8(%rbx)
jle 0x339d8
cvtsd2ss 0x28(%rsp), %xmm3
cvtsd2ss 0x30(%rsp), %xmm4
movsd 0x20(%rsp), %xmm5
subsd %xmm5, %xmm2
cvtsd2ss %xmm2, %xmm2
subsd %xmm5, %xmm1
xorps %xmm5, %xmm5
cvtsd2ss %xmm1, %xmm5
mulsd %xmm0, %xmm0
cvtsd2ss %xmm0, %xmm6
xorl %r15d, %r15d
xorl %r12d, %r12d
movss %xmm3, 0x18(%rsp)
movss %xmm4, 0x10(%rsp)
movss %xmm2, 0x8(%rsp)
movss %xmm5, 0x28(%rsp)
movss %xmm6, 0x20(%rsp)
movq (%rbx), %rdx
movss 0x8(%rdx,%r15), %xmm0
ucomiss %xmm0, %xmm2
ja 0x339c4
ucomiss %xmm5, %xmm0
ja 0x339c4
movss (%rdx,%r15), %xmm0
subss %xmm3, %xmm0
movss 0x4(%rdx,%r15), %xmm1
subss %xmm4, %xmm1
mulss %xmm1, %xmm1
mulss %xmm0, %xmm0
addss %xmm1, %xmm0
ucomiss %xmm6, %xmm0
ja 0x339c4
addq %r15, %rdx
movslq 0x10(%r14), %rdi
shlq $0x4, %rdi
addq 0x8(%r14), %rdi
movq %rbx, %rsi
callq 0x325ce
incl 0x10(%r14)
movss 0x18(%rsp), %xmm3
movss 0x10(%rsp), %xmm4
movss 0x8(%rsp), %xmm2
movss 0x28(%rsp), %xmm5
movss 0x20(%rsp), %xmm6
incq %r12
movslq 0x8(%rbx), %rax
addq $0x30, %r15
cmpq %rax, %r12
jl 0x33951
movq 0x98(%rbx), %r14
testq %r14, %r14
je 0x33a3a
cmpl $0x1, 0xb4(%rbx)
jne 0x33a08
movq (%r14), %rax
movq 0x8(%r14), %rdx
movl 0x28(%r14), %ecx
movq %r14, %rdi
movq %rbx, %rsi
callq *0x90(%rax)
testl %eax, %eax
je 0x33a3a
decl 0xb4(%rbx)
jne 0x33a3a
testb $0x2, 0x89(%rbx)
je 0x33a3a
movq (%r14), %rax
movq %r14, %rdi
movq %rbx, %rsi
callq *0x18(%rax)
movq %rbx, %rdi
callq 0x2ce2e
movl $0xc8, %esi
movq %rbx, %rdi
callq 0x134f0
leaq 0x38(%rsp), %rdi
callq 0x6ede0
leaq 0x70(%rsp), %rdi
callq 0x6ede0
addq $0xa8, %rsp
popq %rbx
popq %r12
popq %r14
popq %r15
retq
jmp 0x33a66
jmp 0x33a66
movq %rax, %rbx
jmp 0x33a73
movq %rax, %rbx
leaq 0x38(%rsp), %rdi
callq 0x6ede0
leaq 0x70(%rsp), %rdi
callq 0x6ede0
movq %rbx, %rdi
callq 0x138b0
nop