// File: 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:
pushq %rbp
movq %rsp, %rbp
subq $0x100, %rsp            # imm = 0x100
movq %rdi, -0xf0(%rbp)
movq %rdi, -0xe8(%rbp)
movq %rsi, -0x8(%rbp)
movq %rdx, -0x10(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5780
movl %eax, -0x14(%rbp)
cmpl $0x3, -0x14(%rbp)
jle 0x387d
leaq -0x30(%rbp), %rdi
callq 0x5730
leaq -0x38(%rbp), %rdi
callq 0x57d0
movq -0x10(%rbp), %rdi
callq 0x57f0
movq %rax, -0x40(%rbp)
movq -0x40(%rbp), %rax
movq %rax, -0x38(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5860
movq %rax, -0x48(%rbp)
leaq -0x38(%rbp), %rdi
leaq -0x48(%rbp), %rsi
callq 0x5820
testb $0x1, %al
jne 0x326e
jmp 0x32c6
leaq -0x38(%rbp), %rdi
callq 0x5c60
movss (%rax), %xmm0
addss -0x30(%rbp), %xmm0
movss %xmm0, -0x30(%rbp)
leaq -0x38(%rbp), %rdi
callq 0x5c60
movss 0x4(%rax), %xmm0
addss -0x2c(%rbp), %xmm0
movss %xmm0, -0x2c(%rbp)
leaq -0x38(%rbp), %rdi
callq 0x5c60
movss 0x8(%rax), %xmm0
addss -0x28(%rbp), %xmm0
movss %xmm0, -0x28(%rbp)
leaq -0x38(%rbp), %rdi
xorl %esi, %esi
callq 0x5920
movq %rax, -0x50(%rbp)
jmp 0x324e
cvtsi2ssl -0x14(%rbp), %xmm1
movss -0x30(%rbp), %xmm0
divss %xmm1, %xmm0
movss %xmm0, -0x30(%rbp)
cvtsi2ssl -0x14(%rbp), %xmm1
movss -0x2c(%rbp), %xmm0
divss %xmm1, %xmm0
movss %xmm0, -0x2c(%rbp)
cvtsi2ssl -0x14(%rbp), %xmm1
movss -0x28(%rbp), %xmm0
divss %xmm1, %xmm0
movss %xmm0, -0x28(%rbp)
movq -0x30(%rbp), %rax
movq %rax, 0x16f06(%rip)     # 0x1a210
movq -0x28(%rbp), %rax
movq %rax, 0x16f03(%rip)     # 0x1a218
movq -0x20(%rbp), %rax
movq %rax, 0x16f00(%rip)     # 0x1a220
movl -0x18(%rbp), %eax
movl %eax, 0x16eff(%rip)     # 0x1a228
xorps %xmm0, %xmm0
movsd %xmm0, -0x58(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x60(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x68(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x70(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x78(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x80(%rbp)
leaq -0x9c(%rbp), %rdi
callq 0x5730
movq -0x10(%rbp), %rdi
callq 0x57f0
movq %rax, -0xa8(%rbp)
movq -0xa8(%rbp), %rax
movq %rax, -0x38(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5860
movq %rax, -0xb0(%rbp)
leaq -0x38(%rbp), %rdi
leaq -0xb0(%rbp), %rsi
callq 0x5820
testb $0x1, %al
jne 0x33a9
jmp 0x34c4
leaq -0x38(%rbp), %rdi
callq 0x5c60
movss (%rax), %xmm0
subss -0x30(%rbp), %xmm0
movss %xmm0, -0x9c(%rbp)
leaq -0x38(%rbp), %rdi
callq 0x5c60
movss 0x4(%rax), %xmm0
subss -0x2c(%rbp), %xmm0
movss %xmm0, -0x98(%rbp)
leaq -0x38(%rbp), %rdi
callq 0x5c60
movss 0x8(%rax), %xmm0
subss -0x28(%rbp), %xmm0
movss %xmm0, -0x94(%rbp)
movss -0x9c(%rbp), %xmm0
mulss -0x9c(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x58(%rbp), %xmm0
movsd %xmm0, -0x58(%rbp)
movss -0x9c(%rbp), %xmm0
mulss -0x98(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x60(%rbp), %xmm0
movsd %xmm0, -0x60(%rbp)
movss -0x9c(%rbp), %xmm0
mulss -0x94(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x68(%rbp), %xmm0
movsd %xmm0, -0x68(%rbp)
movss -0x98(%rbp), %xmm0
mulss -0x98(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x70(%rbp), %xmm0
movsd %xmm0, -0x70(%rbp)
movss -0x98(%rbp), %xmm0
mulss -0x94(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x78(%rbp), %xmm0
movsd %xmm0, -0x78(%rbp)
movss -0x94(%rbp), %xmm0
mulss -0x94(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x80(%rbp), %xmm0
movsd %xmm0, -0x80(%rbp)
leaq -0x38(%rbp), %rdi
xorl %esi, %esi
callq 0x5920
movq %rax, -0xb8(%rbp)
jmp 0x3380
movsd -0x70(%rbp), %xmm0
movsd -0x80(%rbp), %xmm2
movsd -0x78(%rbp), %xmm1
mulsd -0x78(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xc0(%rbp)
movsd -0x58(%rbp), %xmm0
movsd -0x80(%rbp), %xmm2
movsd -0x68(%rbp), %xmm1
mulsd -0x68(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xc8(%rbp)
movsd -0x58(%rbp), %xmm0
movsd -0x70(%rbp), %xmm2
movsd -0x60(%rbp), %xmm1
mulsd -0x60(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xd0(%rbp)
leaq -0xc8(%rbp), %rdi
leaq -0xd0(%rbp), %rsi
callq 0x5e10
movq %rax, %rsi
leaq -0xc0(%rbp), %rdi
callq 0x5e10
movq -0xf0(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0xd8(%rbp)
callq 0x5730
movsd -0xd8(%rbp), %xmm0
ucomisd -0xc0(%rbp), %xmm0
jne 0x365f
jp 0x365f
movq -0xf0(%rbp), %rax
movsd -0xc0(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, (%rax)
movsd -0x68(%rbp), %xmm0
movsd -0x78(%rbp), %xmm2
movsd -0x60(%rbp), %xmm1
mulsd -0x80(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x4(%rax)
movsd -0x60(%rbp), %xmm0
movsd -0x78(%rbp), %xmm2
movsd -0x68(%rbp), %xmm1
mulsd -0x70(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x8(%rax)
jmp 0x37a0
movsd -0xd8(%rbp), %xmm0
ucomisd -0xc8(%rbp), %xmm0
jne 0x370f
jp 0x370f
movq -0xf0(%rbp), %rax
movsd -0x68(%rbp), %xmm0
movsd -0x78(%rbp), %xmm2
movsd -0x60(%rbp), %xmm1
mulsd -0x80(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, (%rax)
movsd -0xc8(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x4(%rax)
movsd -0x60(%rbp), %xmm0
movsd -0x68(%rbp), %xmm2
movsd -0x78(%rbp), %xmm1
mulsd -0x58(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x8(%rax)
jmp 0x379e
movq -0xf0(%rbp), %rax
movsd -0x60(%rbp), %xmm0
movsd -0x78(%rbp), %xmm2
movsd -0x68(%rbp), %xmm1
mulsd -0x70(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, (%rax)
movsd -0x60(%rbp), %xmm0
movsd -0x68(%rbp), %xmm2
movsd -0x78(%rbp), %xmm1
mulsd -0x58(%rbp), %xmm1
movq %xmm1, %rcx
movabsq $-0x8000000000000000, %rdx # imm = 0x8000000000000000
xorq %rdx, %rcx
movq %rcx, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x4(%rax)
movsd -0xd0(%rbp), %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x8(%rax)
jmp 0x37a0
movq -0xf0(%rbp), %rax
movss (%rax), %xmm0
movl $0x2, %edi
callq 0x5e50
movq -0xf0(%rbp), %rax
movsd %xmm0, -0x100(%rbp)
movss 0x4(%rax), %xmm0
movl $0x2, %edi
callq 0x5e50
movq -0xf0(%rbp), %rax
movaps %xmm0, %xmm1
movsd -0x100(%rbp), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xf8(%rbp)
movss 0x8(%rax), %xmm0
movl $0x2, %edi
callq 0x5e50
movaps %xmm0, %xmm1
movsd -0xf8(%rbp), %xmm0
addsd %xmm1, %xmm0
callq 0x2170
movq -0xf0(%rbp), %rax
movsd %xmm0, -0xe0(%rbp)
movsd -0xe0(%rbp), %xmm1
movss (%rax), %xmm0
cvtss2sd %xmm0, %xmm0
divsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, (%rax)
movsd -0xe0(%rbp), %xmm1
movss 0x4(%rax), %xmm0
cvtss2sd %xmm0, %xmm0
divsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x4(%rax)
movsd -0xe0(%rbp), %xmm1
movss 0x8(%rax), %xmm0
cvtss2sd %xmm0, %xmm0
divsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x8(%rax)
jmp 0x3889
movq -0xf0(%rbp), %rdi
callq 0x5730
movq -0xe8(%rbp), %rax
addq $0x100, %rsp            # imm = 0x100
popq %rbp
retq
nopl (%rax)