// File: /workspace/github_cpp_makefile_star2_O1/tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelPointSet.cpp
// Function: gaps::R3SurfelPointSet::PrincipleAxes(gaps::R3Point const*, double*) const
R3Triad R3SurfelPointSet::
PrincipleAxes(const R3Point *center, RNScalar *variances) const
{
  // Check points
  if (npoints == 0) {
    if (variances) { 
      variances[0] = 0.0;
      variances[1] = 0.0;
      variances[2] = 0.0;
    }
    return R3xyz_triad;
  }

  // Get centroid
  R3Point centroid = (center) ? *center : Centroid();

  // Compute covariance matrix
  RNScalar m[9] = { 0 };
  for (int i = 0; i < npoints; i++) {
    R3SurfelPoint& point = points[i];
    RNScalar x = point.X() - centroid[0];
    RNScalar y = point.Y() - centroid[1];
    RNScalar z = point.Z() - centroid[2];
    m[0] += x*x;
    m[4] += y*y;
    m[8] += z*z;
    m[1] += x*y;
    m[3] += x*y;
    m[2] += x*z;
    m[6] += x*z;
    m[5] += y*z;
    m[7] += y*z;
  }

  // Normalize covariance matrix
  for (int i = 0; i < 9; i++) m[i] /= npoints;

  // Compute eigenvalues and eigenvectors
  RNScalar U[9];
  RNScalar W[3];
  RNScalar Vt[9];
  RNSvdDecompose(3, 3, m, U, W, Vt);  // m == U . DiagonalMatrix(W) . Vt

  // Copy principle axes into more convenient form
  // W has eigenvalues (greatest to smallest) and Vt has eigenvectors (normalized)
  R3Vector axes[3];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      axes[i][j] = Vt[3*i+j];
    }
  }

  // Flip axes so that "heavier" on positive side for first two dimensions
  int positive_count[3] = { 0, 0, 0 };
  int negative_count[3] = { 0, 0, 0 };
  for (int i = 0; i < npoints; i++) {
    R3SurfelPoint& point = points[i];
    RNScalar x = point.X() - centroid[0];
    RNScalar y = point.Y() - centroid[1];
    RNScalar z = point.Z() - centroid[2];
    R3Vector vector(x, y, z);
    for (int j = 0; j < 2; j++) {
      RNScalar dot = axes[j].Dot(vector);
      if (dot > 0.0) positive_count[j]++;
      else negative_count[j]++;
    }
  }
  for (int j =0; j < 2; j++) {
    if (positive_count[j] < negative_count[j]) {
      axes[j].Flip();
    }
  }

  // Set third axis to form orthonormal triad with other two
  axes[2] = axes[0] % axes[1];

  // Just checking
  assert(RNIsEqual(axes[0].Length(), 1.0, RN_BIG_EPSILON));
  assert(RNIsEqual(axes[1].Length(), 1.0, RN_BIG_EPSILON));
  assert(RNIsEqual(axes[2].Length(), 1.0, RN_BIG_EPSILON));
  assert(RNIsZero(axes[0].Dot(axes[1]), RN_BIG_EPSILON));
  assert(RNIsZero(axes[1].Dot(axes[2]), RN_BIG_EPSILON));
  assert(RNIsZero(axes[0].Dot(axes[2]), RN_BIG_EPSILON));

  // Return variances (eigenvalues)
  if (variances) {
    variances[0] = W[0];
    variances[1] = W[1];
    variances[2] = W[2];
  }

  // Return triad
  return R3Triad(axes[0], axes[1], axes[2]);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1b8, %rsp            # imm = 0x1B8
movq %rdi, %rbx
cmpl $0x0, 0x10(%rsi)
je 0x32c7b
movq %rsi, %r15
movq %rcx, 0x88(%rsp)
leaq 0x28(%rsp), %rdi
testq %rdx, %rdx
je 0x32ca4
movq %rdx, %rsi
callq 0x84d8e
jmp 0x32cac
testq %rcx, %rcx
je 0x32c90
xorpd %xmm0, %xmm0
movupd %xmm0, (%rcx)
movq $0x0, 0x10(%rcx)
leaq 0x103051(%rip), %rsi    # 0x135ce8
movq %rbx, %rdi
callq 0x71826
jmp 0x33018
movq %r15, %rsi
callq 0x32b74
xorpd %xmm0, %xmm0
movapd %xmm0, 0x70(%rsp)
movapd %xmm0, 0x60(%rsp)
movapd %xmm0, 0x50(%rsp)
movapd %xmm0, 0x40(%rsp)
movq $0x0, 0x80(%rsp)
movslq 0x10(%r15), %rax
testq %rax, %rax
jle 0x32dee
movq 0x8(%r15), %rcx
movsd 0x28(%rsp), %xmm1
movupd 0x48(%rsp), %xmm0
movupd 0x30(%rsp), %xmm2
movapd 0x50(%rsp), %xmm4
shufpd $0x1, %xmm4, %xmm4      # xmm4 = xmm4[1,0]
movapd 0x60(%rsp), %xmm5
movapd 0x70(%rsp), %xmm6
movhpd 0x80(%rsp), %xmm0       # xmm0 = xmm0[0],mem[0]
movq %rax, %rdx
shlq $0x4, %rdx
xorpd %xmm3, %xmm3
xorl %esi, %esi
movq 0x8(%rcx,%rsi), %rdi
xorps %xmm7, %xmm7
cvtss2sd (%rdi), %xmm7
movq (%rcx,%rsi), %r8
addsd 0x10(%r8), %xmm7
subsd %xmm1, %xmm7
movapd %xmm7, %xmm8
mulsd %xmm7, %xmm8
cvtps2pd 0x4(%rdi), %xmm9
addsd %xmm8, %xmm3
movupd 0x18(%r8), %xmm8
addpd %xmm9, %xmm8
subpd %xmm2, %xmm8
movapd %xmm8, %xmm9
movsd %xmm7, %xmm9            # xmm9 = xmm7[0],xmm9[1]
mulpd %xmm8, %xmm9
addpd %xmm9, %xmm0
movhpd %xmm0, 0x80(%rsp)
movapd %xmm7, %xmm9
unpcklpd %xmm7, %xmm9            # xmm9 = xmm9[0],xmm7[0]
mulpd %xmm8, %xmm9
addpd %xmm9, %xmm4
movapd %xmm4, %xmm9
shufpd $0x1, %xmm4, %xmm9      # xmm9 = xmm9[1],xmm4[0]
movapd %xmm9, 0x50(%rsp)
movapd %xmm8, %xmm9
unpcklpd %xmm8, %xmm9            # xmm9 = xmm9[0],xmm8[0]
mulpd %xmm8, %xmm9
addpd %xmm9, %xmm5
movapd %xmm5, 0x60(%rsp)
unpcklpd %xmm8, %xmm7            # xmm7 = xmm7[0],xmm8[0]
unpckhpd %xmm8, %xmm8            # xmm8 = xmm8[1,1]
mulpd %xmm7, %xmm8
addpd %xmm8, %xmm6
movapd %xmm6, 0x70(%rsp)
addq $0x10, %rsi
cmpq %rsi, %rdx
jne 0x32d24
movlpd %xmm0, 0x48(%rsp)
jmp 0x32df2
xorpd %xmm3, %xmm3
movq %rbx, 0x90(%rsp)
movsd %xmm3, 0x40(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
xorl %eax, %eax
movsd 0x40(%rsp,%rax,8), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x40(%rsp,%rax,8)
incq %rax
cmpq $0x9, %rax
jne 0x32e09
leaq 0x40(%rsp), %rdx
leaq 0x170(%rsp), %rcx
leaq 0xf0(%rsp), %r8
leaq 0x120(%rsp), %r9
movl $0x3, %edi
movl $0x3, %esi
callq 0xaca05
xorl %ebx, %ebx
leaq (%rsp,%rbx), %rdi
addq $0xa0, %rdi
callq 0x85814
addq $0x18, %rbx
cmpq $0x48, %rbx
jne 0x32e50
xorl %eax, %eax
movq 0x130(%rsp,%rax), %rcx
movq %rcx, 0xb0(%rsp,%rax)
movupd 0x120(%rsp,%rax), %xmm0
movupd %xmm0, 0xa0(%rsp,%rax)
addq $0x18, %rax
cmpq $0x48, %rax
jne 0x32e6c
xorl %eax, %eax
movl %eax, 0x20(%rsp)
movq %rax, 0x18(%rsp)
movl %eax, 0x10(%rsp)
movq %rax, 0x8(%rsp)
cmpl %eax, 0x10(%r15)
jle 0x32f75
xorl %ebp, %ebp
leaq 0x108(%rsp), %r12
leaq 0x18(%rsp), %r14
leaq 0x8(%rsp), %rbx
movq %r15, 0x98(%rsp)
movq 0x8(%r15), %rax
movq %rbp, %rcx
shlq $0x4, %rcx
movq (%rax,%rcx), %rdx
movq 0x8(%rax,%rcx), %rax
xorps %xmm0, %xmm0
cvtss2sd (%rax), %xmm0
addsd 0x10(%rdx), %xmm0
subsd 0x28(%rsp), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rax), %xmm1
addsd 0x18(%rdx), %xmm1
subsd 0x30(%rsp), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rax), %xmm2
addsd 0x20(%rdx), %xmm2
subsd 0x38(%rsp), %xmm2
movq %r12, %rdi
callq 0x85816
leaq 0xa0(%rsp), %r13
xorl %r15d, %r15d
movq %r13, %rdi
movq %r12, %rsi
callq 0x85aae
ucomisd 0xab444(%rip), %xmm0    # 0xde388
movq %r14, %rax
ja 0x32f4c
movq %rbx, %rax
incl (%rax,%r15,4)
incq %r15
addq $0x18, %r13
cmpq $0x1, %r15
je 0x32f31
incq %rbp
movq 0x98(%rsp), %r15
movslq 0x10(%r15), %rax
cmpq %rax, %rbp
jl 0x32ed2
leaq 0xa0(%rsp), %r15
xorl %ebx, %ebx
movl 0x18(%rsp,%rbx,4), %eax
cmpl 0x8(%rsp,%rbx,4), %eax
jge 0x32f91
movq %r15, %rdi
callq 0x85ba4
incq %rbx
addq $0x18, %r15
cmpq $0x1, %rbx
je 0x32f7f
leaq 0xb8(%rsp), %r15
leaq 0x108(%rsp), %r13
leaq 0xa0(%rsp), %rsi
movq %r13, %rdi
movq %r15, %rdx
callq 0x8616a
leaq 0xd0(%rsp), %r12
movq %r12, %rdi
movq %r13, %rsi
callq 0x85fc2
movq 0x88(%rsp), %rax
testq %rax, %rax
je 0x32ffa
movaps 0xf0(%rsp), %xmm0
movups %xmm0, (%rax)
movsd 0x100(%rsp), %xmm0
movsd %xmm0, 0x10(%rax)
leaq 0xa0(%rsp), %rsi
movq 0x90(%rsp), %rbx
movq %rbx, %rdi
movq %r15, %rdx
movq %r12, %rcx
callq 0x71878
movq %rbx, %rax
addq $0x1b8, %rsp            # imm = 0x1B8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop