// File: /workspace/github_cpp_makefile_star2_O3/mitchellsayer[P]FastPointCloudSegmentation/src/./point_cloud_segmenter.cc
// Function: PointCloudSegmenter::ExtractInitialSeeds(std::vector<Vec3, std::allocator<Vec3>>&, std::vector<Vec3, std::allocator<Vec3>>&)
void PointCloudSegmenter::ExtractInitialSeeds(std::vector<Vec3>& cloud_seg, std::vector<Vec3>& seeds) {
  if (!cloud_seg.empty()) {

    //Sort points in current segment by height
    std::sort(cloud_seg.begin(), cloud_seg.end(), CompareZ);

    Vec3 lpr;

    //Calc Lowest Point Representative (LPR)
    std::vector<Vec3>::iterator it;
    int i = 0;

    for (it = cloud_seg.begin(); it != cloud_seg.end(); it++) {
      if (it->z > -4) {
        if (i == n_lpr) break;

        lpr.x += it->x;
        lpr.y += it->y;
        lpr.z += it->z;

        i++;
      }
    }

    lpr.x /= n_lpr;
    lpr.y /= n_lpr;
    lpr.z /= n_lpr;

    //Add point as initial seed if its dist to lpr is < th_seeds
    for (it =  cloud_seg.begin(); it != cloud_seg.end(); it++) {
      if (it->z - lpr.z < this->th_seeds) {
        seeds.push_back(*it);
      }
    }
  }
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x48, %rsp
movq (%rsi), %r12
movq 0x8(%rsi), %r13
cmpq %r13, %r12
je 0x2f24
movq %rdx, %r14
movq %rsi, %r15
movq %rdi, -0x68(%rbp)
movq %r13, %rbx
subq %r12, %rbx
movq %rbx, %rax
sarq $0x2, %rax
movabsq $0x6db6db6db6db6db7, %rcx # imm = 0x6DB6DB6DB6DB6DB7
imulq %rax, %rcx
bsrq %rcx, %rdx
xorl $0x3f, %edx
addl %edx, %edx
xorq $0x7e, %rdx
leaq -0x782(%rip), %rcx      # 0x24dc
movq %r12, %rdi
movq %r13, %rsi
callq 0x5714
cmpq $0x1c1, %rbx            # imm = 0x1C1
jl 0x2db9
movq %r15, -0x60(%rbp)
movq %r14, -0x58(%rbp)
leaq 0x1c(%r12), %r14
movl $0x1c, %r15d
movq %r12, %rdi
leaq (%r12,%r15), %rbx
movss 0x8(%r12,%r15), %xmm0
movss 0x8(%r12), %xmm1
ucomiss %xmm0, %xmm1
jbe 0x2cd9
movups (%rbx), %xmm0
movups 0xc(%rbx), %xmm1
movups %xmm1, -0x44(%rbp)
movaps %xmm0, -0x50(%rbp)
subq %r15, %rdi
addq $0x38, %rdi
movq %r12, %rsi
movq %r15, %rdx
callq 0x2260
movaps -0x50(%rbp), %xmm0
movups %xmm0, (%r12)
movups -0x44(%rbp), %xmm0
movups %xmm0, 0xc(%r12)
jmp 0x2d24
movq (%rbx), %rax
movups 0xc(%rbx), %xmm1
movaps %xmm1, -0x50(%rbp)
movss 0x8(%rdi), %xmm1
ucomiss %xmm0, %xmm1
movq %rbx, %rcx
jbe 0x2d14
movq %r14, %rdx
leaq -0x1c(%rdx), %rcx
movups -0x10(%rdx), %xmm1
movups %xmm1, 0xc(%rdx)
movups -0x1c(%rdx), %xmm1
movups %xmm1, (%rdx)
movss -0x30(%rdx), %xmm1
ucomiss %xmm0, %xmm1
movq %rcx, %rdx
ja 0x2cf4
movq %rax, (%rcx)
movss %xmm0, 0x8(%rcx)
movaps -0x50(%rbp), %xmm0
movups %xmm0, 0xc(%rcx)
addq $0x1c, %r15
addq $0x1c, %r14
movq %rbx, %rdi
cmpq $0x1c0, %r15            # imm = 0x1C0
jne 0x2c8c
addq $0x1c0, %r12            # imm = 0x1C0
cmpq %r13, %r12
movq -0x58(%rbp), %r14
movq -0x60(%rbp), %r15
je 0x2e72
movq (%r12), %rax
movss 0x8(%r12), %xmm0
movups 0xc(%r12), %xmm1
movaps %xmm1, -0x50(%rbp)
movss -0x14(%r12), %xmm1
ucomiss %xmm0, %xmm1
movq %r12, %rcx
jbe 0x2d9b
movq %r12, %rdx
leaq -0x1c(%rdx), %rcx
movups -0x10(%rdx), %xmm1
movups %xmm1, 0xc(%rdx)
movups -0x1c(%rdx), %xmm1
movups %xmm1, (%rdx)
movss -0x30(%rdx), %xmm1
ucomiss %xmm0, %xmm1
movq %rcx, %rdx
ja 0x2d7b
movq %rax, (%rcx)
movss %xmm0, 0x8(%rcx)
movaps -0x50(%rbp), %xmm0
movups %xmm0, 0xc(%rcx)
addq $0x1c, %r12
cmpq %r13, %r12
jne 0x2d54
jmp 0x2e72
leaq 0x1c(%r12), %rax
cmpq %r13, %rax
je 0x2e72
movq %r12, %rdi
movq %rax, %rbx
movss 0x8(%rax), %xmm0
movss 0x8(%r12), %xmm1
ucomiss %xmm0, %xmm1
jbe 0x2e17
movups (%rbx), %xmm0
movups 0xc(%rbx), %xmm1
movups %xmm1, -0x44(%rbp)
movaps %xmm0, -0x50(%rbp)
movq %rbx, %rdx
subq %r12, %rdx
subq %rdx, %rdi
addq $0x38, %rdi
movq %r12, %rsi
callq 0x2260
movaps -0x50(%rbp), %xmm0
movups %xmm0, (%r12)
movups -0x44(%rbp), %xmm0
movups %xmm0, 0xc(%r12)
jmp 0x2e62
movq (%rbx), %rax
movups 0xc(%rbx), %xmm1
movaps %xmm1, -0x50(%rbp)
movss 0x8(%rdi), %xmm1
ucomiss %xmm0, %xmm1
movq %rbx, %rcx
jbe 0x2e52
movq %rbx, %rdx
leaq -0x1c(%rdx), %rcx
movups -0x10(%rdx), %xmm1
movups %xmm1, 0xc(%rdx)
movups -0x1c(%rdx), %xmm1
movups %xmm1, (%rdx)
movss -0x30(%rdx), %xmm1
ucomiss %xmm0, %xmm1
movq %rcx, %rdx
ja 0x2e32
movq %rax, (%rcx)
movss %xmm0, 0x8(%rcx)
movaps -0x50(%rbp), %xmm0
movups %xmm0, 0xc(%rcx)
leaq 0x1c(%rbx), %rax
movq %rbx, %rdi
cmpq %r13, %rax
jne 0x2dca
movq (%r15), %r12
movq 0x8(%r15), %rax
cmpq %rax, %r12
movq -0x68(%rbp), %rbx
je 0x2f24
movl 0x5c(%rbx), %ecx
xorps %xmm2, %xmm2
xorl %edx, %edx
movss 0x51e6(%rip), %xmm0     # 0x807c
movq %r12, %rsi
movss 0x8(%rsi), %xmm1
ucomiss %xmm0, %xmm1
jbe 0x2ead
cmpl %ecx, %edx
je 0x2eb6
addss %xmm1, %xmm2
incl %edx
addq $0x1c, %rsi
cmpq %rax, %rsi
jne 0x2e99
cmpq %rax, %r12
je 0x2f24
xorps %xmm0, %xmm0
cvtsi2ss %ecx, %xmm0
divss %xmm0, %xmm2
movss %xmm2, -0x58(%rbp)
movss 0x8(%r12), %xmm0
subss %xmm2, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x48(%rbx), %xmm1
ucomisd %xmm0, %xmm1
jbe 0x2f1a
movq 0x8(%r14), %rsi
cmpq 0x10(%r14), %rsi
je 0x2f0a
movups (%r12), %xmm0
movupd 0xc(%r12), %xmm1
movupd %xmm1, 0xc(%rsi)
movups %xmm0, (%rsi)
addq $0x1c, 0x8(%r14)
jmp 0x2f1a
movq %r14, %rdi
movq %r12, %rdx
callq 0x5594
movss -0x58(%rbp), %xmm2
addq $0x1c, %r12
cmpq 0x8(%r15), %r12
jne 0x2ecb
addq $0x48, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop