// File: tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelPointSet.cpp
// Function: gaps::R3SurfelPointSet::InsertPoints(gaps::R3SurfelPointSet const*, gaps::R3Point const&, double, double, double)
void R3SurfelPointSet::
InsertPoints(const R3SurfelPointSet *set, const R3Point& center, RNLength radius, RNCoord zmin, RNCoord zmax)
{
  // Check set
  if (set->NPoints() == 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(set->BBox());
  if (intersection_box.IsEmpty()) return;
  bbox.Union(intersection_box);

  // Check and update timestamp range (conservatively)
  timestamp_range.Union(set->TimestampRange());

  // Allocate space for points
  AllocatePoints(npoints + set->NPoints());

  // Copy points
  RNScalar rr = radius * radius;
  for (int i = 0; i < set->NPoints(); i++) {
    const R3SurfelPoint *point = set->Point(i);
    const R3Point& position = point->Position();
    if (position.Z() < zmin) continue;
    if (position.Z() > zmax) continue;
    RNScalar dx = point->X() - center.X();
    RNScalar dy = point->Y() - center.Y();
    RNScalar dd = dx*dx + dy*dy;
    if (dd > rr) continue;
    points[npoints] = *point;
    npoints++;
  }
}

// Assembly:
subq $0x1a8, %rsp            # imm = 0x1A8
movq %rdi, 0x1a0(%rsp)
movq %rsi, 0x198(%rsp)
movq %rdx, 0x190(%rsp)
movsd %xmm0, 0x188(%rsp)
movsd %xmm1, 0x180(%rsp)
movsd %xmm2, 0x178(%rsp)
movq 0x1a0(%rsp), %rax
movq %rax, 0xa0(%rsp)
movq 0x198(%rsp), %rdi
callq 0x18700
cmpl $0x0, %eax
jne 0x58a71
jmp 0x58ed9
movq 0x190(%rsp), %rdi
xorl %esi, %esi
movl %esi, 0x70(%rsp)
callq 0x1bba0
movsd 0x188(%rsp), %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x90(%rsp)
movq 0x190(%rsp), %rdi
movl $0x1, %esi
movl %esi, 0x74(%rsp)
callq 0x1bba0
movl 0x70(%rsp), %esi
movsd 0x188(%rsp), %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x78(%rsp)
movsd 0x180(%rsp), %xmm0
movsd %xmm0, 0x80(%rsp)
movq 0x190(%rsp), %rdi
callq 0x1bba0
movl 0x74(%rsp), %esi
movsd 0x188(%rsp), %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, 0x88(%rsp)
movq 0x190(%rsp), %rdi
callq 0x1bba0
movsd 0x78(%rsp), %xmm1
movsd 0x80(%rsp), %xmm2
movsd 0x88(%rsp), %xmm3
movaps %xmm0, %xmm4
movsd 0x90(%rsp), %xmm0
movsd 0x188(%rsp), %xmm5
addsd %xmm5, %xmm4
movsd 0x178(%rsp), %xmm5
leaq 0x140(%rsp), %rdi
movq %rdi, 0x98(%rsp)
callq 0xc9ee0
movq 0x98(%rsp), %rsi
leaq 0x108(%rsp), %rdi
callq 0xc9de0
jmp 0x58b73
movq 0x198(%rsp), %rdi
callq 0x435d0
movq %rax, 0x68(%rsp)
jmp 0x58b87
movq 0x68(%rsp), %rsi
leaq 0x108(%rsp), %rdi
callq 0xcbf10
jmp 0x58b9b
leaq 0x108(%rsp), %rdi
callq 0x44290
movl %eax, 0x64(%rsp)
jmp 0x58bae
movl 0x64(%rsp), %eax
cmpl $0x0, %eax
je 0x58c06
movl $0x1, 0xf8(%rsp)
jmp 0x58ebf
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x100(%rsp)
movl %eax, 0xfc(%rsp)
jmp 0x58ee1
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x100(%rsp)
movl %eax, 0xfc(%rsp)
leaq 0x108(%rsp), %rdi
callq 0x1b990
jmp 0x58ee1
movq 0xa0(%rsp), %rdi
addq $0x18, %rdi
leaq 0x108(%rsp), %rsi
callq 0xcbc30
jmp 0x58c21
movq 0xa0(%rsp), %rax
addq $0x50, %rax
movq %rax, 0x50(%rsp)
movq 0x198(%rsp), %rdi
callq 0x4f840
movq %rax, 0x58(%rsp)
jmp 0x58c46
movq 0x58(%rsp), %rsi
movq 0x50(%rsp), %rdi
callq 0x1bbc0
jmp 0x58c57
movq 0xa0(%rsp), %rax
movl 0x10(%rax), %eax
movl %eax, 0x48(%rsp)
movq 0x198(%rsp), %rdi
callq 0x18700
movl %eax, 0x4c(%rsp)
jmp 0x58c79
movq 0xa0(%rsp), %rdi
movl 0x4c(%rsp), %eax
movl 0x48(%rsp), %esi
addl %eax, %esi
movq (%rdi), %rax
movq 0x80(%rax), %rax
callq *%rax
jmp 0x58c99
movsd 0x188(%rsp), %xmm0
mulsd 0x188(%rsp), %xmm0
movsd %xmm0, 0xf0(%rsp)
movl $0x0, 0xec(%rsp)
movl 0xec(%rsp), %eax
movl %eax, 0x40(%rsp)
movq 0x198(%rsp), %rdi
callq 0x18700
movl %eax, 0x44(%rsp)
jmp 0x58cdd
movl 0x40(%rsp), %eax
movl 0x44(%rsp), %ecx
cmpl %ecx, %eax
jge 0x58eb4
movq 0x198(%rsp), %rdi
movl 0xec(%rsp), %esi
callq 0x18710
movq %rax, 0x38(%rsp)
jmp 0x58d08
movq 0x38(%rsp), %rax
movq %rax, 0xe0(%rsp)
movq 0xe0(%rsp), %rsi
leaq 0xc0(%rsp), %rdi
callq 0x18730
jmp 0x58d2c
leaq 0xc0(%rsp), %rax
movq %rax, 0xd8(%rsp)
movq 0xd8(%rsp), %rdi
callq 0x187a0
movsd %xmm0, 0x30(%rsp)
jmp 0x58d51
movsd 0x30(%rsp), %xmm1
movsd 0x180(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x58d6b
jmp 0x58e9e
movq 0xd8(%rsp), %rdi
callq 0x187a0
movsd %xmm0, 0x28(%rsp)
jmp 0x58d80
movsd 0x28(%rsp), %xmm0
ucomisd 0x178(%rsp), %xmm0
jbe 0x58d96
jmp 0x58e9e
movq 0xe0(%rsp), %rdi
callq 0x5b1c0
movsd %xmm0, 0x20(%rsp)
jmp 0x58dab
movq 0x190(%rsp), %rdi
callq 0x187b0
movsd %xmm0, 0x18(%rsp)
jmp 0x58dc0
movsd 0x18(%rsp), %xmm1
movsd 0x20(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0xb8(%rsp)
movq 0xe0(%rsp), %rdi
callq 0x5b1d0
movsd %xmm0, 0x10(%rsp)
jmp 0x58dee
movq 0x190(%rsp), %rdi
callq 0x187c0
movsd %xmm0, 0x8(%rsp)
jmp 0x58e03
movsd 0x8(%rsp), %xmm1
movsd 0x10(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0xb0(%rsp)
movsd 0xb8(%rsp), %xmm0
movsd 0xb8(%rsp), %xmm2
movsd 0xb0(%rsp), %xmm1
mulsd 0xb0(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xa8(%rsp)
movsd 0xa8(%rsp), %xmm0
ucomisd 0xf0(%rsp), %xmm0
jbe 0x58e67
jmp 0x58e9e
movq 0xa0(%rsp), %rax
movq 0xe0(%rsp), %rsi
movq 0x8(%rax), %rdi
movslq 0x10(%rax), %rax
shlq $0x4, %rax
addq %rax, %rdi
callq 0x55980
jmp 0x58e8d
movq 0xa0(%rsp), %rax
movl 0x10(%rax), %ecx
addl $0x1, %ecx
movl %ecx, 0x10(%rax)
movl 0xec(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xec(%rsp)
jmp 0x58cbf
movl $0x0, 0xf8(%rsp)
leaq 0x108(%rsp), %rdi
callq 0x1b990
leaq 0x140(%rsp), %rdi
callq 0x1b990
addq $0x1a8, %rsp            # imm = 0x1A8
retq
leaq 0x140(%rsp), %rdi
callq 0x1b990
movq 0x100(%rsp), %rdi
callq 0x13970
nopl (%rax,%rax)