// File: /workspace/github_cpp_makefile_star2_O2/mitchellsayer[P]FastPointCloudSegmentation/src/./point_cloud_segmenter.cc
// Function: PointCloudSegmenter::CalculatePlaneNormal(std::vector<Vec3, std::allocator<Vec3>>&)
Vec3 PointCloudSegmenter::CalculatePlaneNormal(std::vector<Vec3>& cur_p_gnd) {
  int n = cur_p_gnd.size();

  if (n > 3) {
    //Calculate centroid
    Vec3 centroid;

    std::vector<Vec3>::iterator it;

    for (it = cur_p_gnd.begin(); it != cur_p_gnd.end(); it++) {
      centroid.x += it->x;
      centroid.y += it->y;
      centroid.z += it->z;
    }

    centroid.x /= n;
    centroid.y /= n;
    centroid.z /= n;

    cloud_centroid = centroid;

    //Calc covarience matrix
    double xx = 0, xy = 0, xz = 0,
           yy = 0, yz = 0, zz = 0;

    Vec3 r;

    for (it = cur_p_gnd.begin(); it != cur_p_gnd.end(); it++) {
      r.x = it->x - centroid.x;
      r.y = it->y - centroid.y;
      r.z = it->z - centroid.z;

      xx += r.x * r.x;
      xy += r.x * r.y;
      xz += r.x * r.z;
      yy += r.y * r.y;
      yz += r.y * r.z;
      zz += r.z * r.z;
    }

    double det_x = yy*zz - yz*yz;
    double det_y = xx*zz - xz*xz;
    double det_z = xx*yy - xy*xy;

    double det_max = std::max(det_x, std::max(det_y, det_z));

    Vec3 norm;

    // Pick path with best conditioning
    if (det_max == det_x) {
      norm.x = det_x;
      norm.y = xz*yz - xy*zz;
      norm.z = xy*yz - xz*yy;
    } else if (det_max == det_y) {
      norm.x = xz*yz - xy*zz;
      norm.y = det_y;
      norm.z = xy*xz - yz*xx;
    } else {
      norm.x = xy*yz - xz*yy;
      norm.y = xy*xz - yz*xx;
      norm.z = det_z;
    }

    double dir_mag = sqrt(pow(norm.x, 2) + pow(norm.y, 2) + pow(norm.z, 2));

    norm.x /= dir_mag;
    norm.y /= dir_mag;
    norm.z /= dir_mag;

    return norm;
  }

  return Vec3();
}

// Assembly:
movq %rdx, %rcx
movq (%rdx), %rsi
movq 0x8(%rdx), %r8
movq %r8, %rax
subq %rsi, %rax
movabsq $0x1c, %r9
cqto
idivq %r9
cmpl $0x4, %eax
jl 0x2a56
pushq %rbp
movq %rsp, %rbp
xorps %xmm5, %xmm5
xorps %xmm7, %xmm7
xorps %xmm8, %xmm8
cmpq %r8, %rsi
je 0x2a6d
addss (%rsi), %xmm8
addss 0x4(%rsi), %xmm7
addss 0x8(%rsi), %xmm5
addq $0x1c, %rsi
jmp 0x2a3c
xorps %xmm0, %xmm0
movups %xmm0, (%rdi)
andq $0x0, 0x10(%rdi)
movl $0xbf800000, 0x18(%rdi) # imm = 0xBF800000
jmp 0x2cba
cvtsi2ss %eax, %xmm0
divss %xmm0, %xmm8
divss %xmm0, %xmm7
divss %xmm0, %xmm5
movss %xmm8, 0x8701(%rip)     # 0xb188
movss %xmm7, 0x86fd(%rip)     # 0xb18c
movss %xmm5, 0x86f9(%rip)     # 0xb190
movabsq $-0x4080000000000000, %rax # imm = 0xBF80000000000000
movq %rax, 0x86f4(%rip)      # 0xb19c
andq $0x0, 0x86e4(%rip)      # 0xb194
movq (%rcx), %rdx
movq 0x8(%rcx), %rcx
xorps %xmm0, %xmm0
xorpd %xmm4, %xmm4
xorpd %xmm3, %xmm3
xorpd %xmm1, %xmm1
xorpd %xmm2, %xmm2
xorpd %xmm6, %xmm6
cmpq %rcx, %rdx
je 0x2b66
movss (%rdx), %xmm9
movss 0x4(%rdx), %xmm10
subss %xmm8, %xmm9
subss %xmm7, %xmm10
movss 0x8(%rdx), %xmm11
subss %xmm5, %xmm11
movaps %xmm9, %xmm12
mulss %xmm9, %xmm12
cvtss2sd %xmm12, %xmm12
addsd %xmm12, %xmm6
movaps %xmm9, %xmm12
mulss %xmm10, %xmm12
cvtss2sd %xmm12, %xmm12
addsd %xmm12, %xmm2
mulss %xmm11, %xmm9
cvtss2sd %xmm9, %xmm9
addsd %xmm9, %xmm1
movaps %xmm10, %xmm9
mulss %xmm10, %xmm9
cvtss2sd %xmm9, %xmm9
addsd %xmm9, %xmm3
mulss %xmm11, %xmm10
cvtss2sd %xmm10, %xmm9
addsd %xmm9, %xmm0
mulss %xmm11, %xmm11
cvtss2sd %xmm11, %xmm9
addsd %xmm9, %xmm4
addq $0x1c, %rdx
jmp 0x2ace
movapd 0x44e1(%rip), %xmm10    # 0x7050
movapd %xmm0, %xmm5
mulsd %xmm0, %xmm5
movapd %xmm3, %xmm7
mulsd %xmm4, %xmm7
subsd %xmm5, %xmm7
movapd %xmm1, %xmm8
xorpd %xmm10, %xmm8
movapd %xmm1, %xmm9
mulsd %xmm1, %xmm9
movapd %xmm6, %xmm5
mulsd %xmm4, %xmm5
subsd %xmm9, %xmm5
movapd %xmm2, %xmm11
xorpd %xmm10, %xmm11
movapd %xmm2, %xmm12
mulsd %xmm2, %xmm12
movapd %xmm6, %xmm9
mulsd %xmm3, %xmm9
subsd %xmm12, %xmm9
movapd %xmm9, %xmm12
maxsd %xmm5, %xmm12
andq $0x0, 0xc(%rdi)
maxsd %xmm7, %xmm12
movq %rax, 0x14(%rdi)
ucomisd %xmm7, %xmm12
popq %rbp
jne 0x2c0e
jp 0x2c0e
mulsd %xmm11, %xmm4
mulsd %xmm0, %xmm1
addsd %xmm4, %xmm1
mulsd %xmm8, %xmm3
mulsd %xmm0, %xmm2
addsd %xmm3, %xmm2
movapd %xmm2, %xmm9
movapd %xmm1, %xmm5
jmp 0x2c5a
xorpd %xmm0, %xmm10
ucomisd %xmm5, %xmm12
mulsd %xmm10, %xmm6
movapd %xmm2, %xmm7
mulsd %xmm1, %xmm7
addsd %xmm6, %xmm7
jne 0x2c45
jp 0x2c45
mulsd %xmm11, %xmm4
mulsd %xmm0, %xmm1
addsd %xmm4, %xmm1
movapd %xmm7, %xmm9
movapd %xmm1, %xmm7
jmp 0x2c5a
mulsd %xmm8, %xmm3
mulsd %xmm0, %xmm2
addsd %xmm3, %xmm2
movapd %xmm7, %xmm5
movapd %xmm2, %xmm7
cvtsd2ss %xmm7, %xmm0
cvtsd2ss %xmm5, %xmm1
cvtsd2ss %xmm9, %xmm2
cvtss2sd %xmm0, %xmm0
movaps %xmm0, %xmm3
mulsd %xmm0, %xmm3
cvtss2sd %xmm1, %xmm1
movaps %xmm1, %xmm4
mulsd %xmm1, %xmm4
addsd %xmm3, %xmm4
cvtss2sd %xmm2, %xmm2
movaps %xmm2, %xmm3
mulsd %xmm2, %xmm3
addsd %xmm4, %xmm3
sqrtsd %xmm3, %xmm3
divsd %xmm3, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, (%rdi)
divsd %xmm3, %xmm1
cvtsd2ss %xmm1, %xmm0
movss %xmm0, 0x4(%rdi)
divsd %xmm3, %xmm2
cvtsd2ss %xmm2, %xmm0
movss %xmm0, 0x8(%rdi)
movq %rdi, %rax
retq