// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/R3Shapes/R3Grid.cpp
// Function: gaps::R3Grid::GridPrincipleAxes(gaps::R3Point const*, double*) const
R3Triad R3Grid::
GridPrincipleAxes(const R3Point *grid_centroid, RNScalar *variances) const
{
  // Get centroid
  R3Point center = (grid_centroid) ? *grid_centroid : GridCentroid();

  // Compute covariance matrix
  RNScalar m[9] = { 0 };
  RNScalar total_value = 0;
  RNScalar *grid_valuesp = grid_values;
  for (int k = 0; k < grid_resolution[2]; k++) {
    for (int j = 0; j < grid_resolution[1]; j++) {
      for (int i = 0; i < grid_resolution[0]; i++) {
        R3Point position(i, j, k);
        RNScalar value = *(grid_valuesp++);
        RNScalar x = position[0] - center[0];
        RNScalar y = position[1] - center[1];
        RNScalar z = position[2] - center[2];
        m[0] += value * x*x;
        m[4] += value * y*y;
        m[8] += value * z*z;
        m[1] += value * x*y;
        m[3] += value * x*y;
        m[2] += value * x*z;
        m[6] += value * x*z;
        m[5] += value * y*z;
        m[7] += value * y*z;
        total_value += value;
      }
    }
  }

  // Normalize covariance matrix
  if (total_value == 0) return R3xyz_triad;
  for (int i = 0; i < 9; i++) m[i] /= total_value;

  // 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 first two axes so that "heavier" on positive side 
  grid_valuesp = grid_values;
  RNScalar positive_count[2] = { 0, 0 };
  RNScalar negative_count[2] = { 0, 0 };
  for (int k = 0; k < grid_resolution[2]; k++) {
    for (int j = 0; j < grid_resolution[1]; j++) {
      for (int i = 0; i < grid_resolution[0]; i++) {
        R3Point position(i, j, k);
        RNScalar value = *(grid_valuesp++);
        R3Vector vector = position - center;
        for (int j = 0; j < 2; j++) {
          RNScalar dot = axes[j].Dot(vector);
          if (dot > 0.0) positive_count[j] += value;
          else negative_count[j] += value;
        }
      }
    }
  }
  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 $0x238, %rsp            # imm = 0x238
movq %rsi, %r15
movq %rdi, %r12
testq %rdx, %rdx
movq %rcx, 0x60(%rsp)
leaq 0x78(%rsp), %rdi
je 0x2f86c
movq %rdx, %rsi
callq 0x3c542
jmp 0x2f874
movq %r15, %rsi
callq 0x2f700
xorpd %xmm9, %xmm9
movapd %xmm9, 0xc0(%rsp)
movapd %xmm9, 0xb0(%rsp)
movapd %xmm9, 0xa0(%rsp)
movapd %xmm9, 0x90(%rsp)
movq $0x0, 0xd0(%rsp)
movl 0x240(%r15), %ecx
testl %ecx, %ecx
jle 0x2fadf
movq %r12, 0x38(%rsp)
movq 0x230(%r15), %rbx
movl 0x23c(%r15), %eax
xorl %ebp, %ebp
xorpd %xmm8, %xmm8
leaq 0x180(%rsp), %r12
xorpd %xmm7, %xmm7
xorpd %xmm1, %xmm1
xorpd %xmm3, %xmm3
xorpd %xmm4, %xmm4
testl %eax, %eax
jle 0x2fac4
xorps %xmm2, %xmm2
cvtsi2sd %ebp, %xmm2
movl 0x238(%r15), %ecx
xorl %r14d, %r14d
movsd %xmm2, 0x68(%rsp)
testl %ecx, %ecx
jle 0x2fab1
xorps %xmm0, %xmm0
cvtsi2sd %r14d, %xmm0
movsd %xmm0, 0x70(%rsp)
xorl %r13d, %r13d
movapd %xmm4, 0x130(%rsp)
movapd %xmm3, 0x140(%rsp)
movapd %xmm1, 0x50(%rsp)
movsd %xmm8, (%rsp)
movapd %xmm9, 0x10(%rsp)
movsd %xmm7, 0x8(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %r13d, %xmm0
movq %r12, %rdi
movsd 0x70(%rsp), %xmm1
callq 0x3c532
movsd (%rsp), %xmm8
movsd 0x8(%rsp), %xmm7
movsd 0x180(%rsp), %xmm2
movsd 0x188(%rsp), %xmm0
subsd 0x78(%rsp), %xmm2
subsd 0x80(%rsp), %xmm0
movsd (%rbx), %xmm5
movsd 0x190(%rsp), %xmm1
subsd 0x88(%rsp), %xmm1
addq $0x8, %rbx
movapd %xmm5, %xmm3
movapd %xmm5, %xmm4
addsd %xmm5, %xmm7
mulsd %xmm2, %xmm5
mulsd %xmm5, %xmm2
addsd %xmm2, %xmm8
movsd %xmm8, 0x90(%rsp)
mulsd %xmm0, %xmm3
mulsd %xmm1, %xmm4
unpcklpd %xmm0, %xmm4            # xmm4 = xmm4[0],xmm0[0]
movapd %xmm1, %xmm2
unpcklpd %xmm5, %xmm2            # xmm2 = xmm2[0],xmm5[0]
movapd %xmm0, %xmm6
unpcklpd %xmm5, %xmm6            # xmm6 = xmm6[0],xmm5[0]
unpcklpd %xmm3, %xmm5            # xmm5 = xmm5[0],xmm3[0]
unpcklpd %xmm1, %xmm3            # xmm3 = xmm3[0],xmm1[0]
mulpd %xmm4, %xmm2
movapd 0x10(%rsp), %xmm4
addpd %xmm2, %xmm4
movapd %xmm4, 0x10(%rsp)
movapd 0x130(%rsp), %xmm4
movapd 0x10(%rsp), %xmm9
mulpd %xmm3, %xmm6
movapd 0x140(%rsp), %xmm3
addpd %xmm6, %xmm4
unpcklpd %xmm1, %xmm0            # xmm0 = xmm0[0],xmm1[0]
unpcklpd %xmm1, %xmm1            # xmm1 = xmm1[0,0]
mulpd %xmm5, %xmm1
addpd %xmm1, %xmm3
movapd 0x50(%rsp), %xmm1
mulpd %xmm5, %xmm0
movsd 0x68(%rsp), %xmm2
addpd %xmm0, %xmm1
movapd %xmm9, %xmm0
shufpd $0x1, %xmm4, %xmm0      # xmm0 = xmm0[1],xmm4[0]
movupd %xmm0, 0xa8(%rsp)
movapd %xmm4, %xmm0
movsd %xmm1, %xmm0            # xmm0 = xmm1[0],xmm0[1]
movupd %xmm0, 0x98(%rsp)
movapd %xmm1, %xmm0
shufpd $0x1, %xmm3, %xmm0      # xmm0 = xmm0[1],xmm3[0]
movupd %xmm0, 0xb8(%rsp)
movapd %xmm3, %xmm0
shufpd $0x1, %xmm9, %xmm0      # xmm0 = xmm0[1],xmm9[0]
movupd %xmm0, 0xc8(%rsp)
incl %r13d
movl 0x238(%r15), %ecx
cmpl %ecx, %r13d
jl 0x2f926
movl 0x23c(%r15), %eax
incl %r14d
cmpl %eax, %r14d
jl 0x2f90d
movl 0x240(%r15), %ecx
incl %ebp
cmpl %ecx, %ebp
jl 0x2f8ee
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm7
movq 0x38(%rsp), %r12
jne 0x2faf3
jp 0x2faf3
leaq 0xb66d2(%rip), %rsi     # 0xe61b8
movq %r12, %rdi
callq 0x2ab46
jmp 0x2fda1
xorl %eax, %eax
movsd 0x90(%rsp,%rax,8), %xmm0
divsd %xmm7, %xmm0
movsd %xmm0, 0x90(%rsp,%rax,8)
incq %rax
cmpq $0x9, %rax
jne 0x2faf5
leaq 0x90(%rsp), %rdx
leaq 0x180(%rsp), %rcx
leaq 0x150(%rsp), %r8
leaq 0x1d0(%rsp), %r9
movl $0x3, %edi
movl $0x3, %esi
callq 0x710b6
xorl %ebx, %ebx
leaq (%rsp,%rbx), %rdi
addq $0xe0, %rdi
callq 0x3cfc4
addq $0x18, %rbx
cmpq $0x48, %rbx
jne 0x2fb45
xorl %eax, %eax
movq 0x1e0(%rsp,%rax), %rcx
movq %rcx, 0xf0(%rsp,%rax)
movupd 0x1d0(%rsp,%rax), %xmm0
movupd %xmm0, 0xe0(%rsp,%rax)
addq $0x18, %rax
cmpq $0x48, %rax
jne 0x2fb61
movq 0x230(%r15), %r12
xorl %edx, %edx
movq %rdx, 0x30(%rsp)
movq %rdx, 0x48(%rsp)
movq %rdx, 0x20(%rsp)
movq %rdx, 0x40(%rsp)
movl 0x240(%r15), %ecx
testl %ecx, %ecx
jle 0x2fce3
movl 0x23c(%r15), %eax
leaq 0x168(%rsp), %r13
leaq 0x220(%rsp), %rbp
testl %eax, %eax
jle 0x2fcd9
movq %rdx, 0x50(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %edx, %xmm0
movsd %xmm0, (%rsp)
movl 0x238(%r15), %ecx
xorl %r14d, %r14d
testl %ecx, %ecx
jle 0x2fcc1
xorps %xmm0, %xmm0
cvtsi2sd %r14d, %xmm0
movsd %xmm0, 0x8(%rsp)
xorl %ebx, %ebx
xorps %xmm0, %xmm0
cvtsi2sd %ebx, %xmm0
movq %r13, %rdi
movsd 0x8(%rsp), %xmm1
movsd (%rsp), %xmm2
callq 0x3c532
movsd (%r12), %xmm0
movsd %xmm0, 0x10(%rsp)
movq %rbp, %rdi
movq %r13, %rsi
leaq 0x78(%rsp), %rdx
callq 0x3c78e
leaq 0xe0(%rsp), %rdi
movq %rbp, %rsi
callq 0x3d252
ucomisd 0x7258f(%rip), %xmm0    # 0xa21e8
leaq 0x30(%rsp), %rax
ja 0x2fc65
leaq 0x20(%rsp), %rax
movsd (%rax), %xmm0
addsd 0x10(%rsp), %xmm0
movsd %xmm0, (%rax)
leaq 0xf8(%rsp), %rdi
movq %rbp, %rsi
callq 0x3d252
ucomisd 0x7255d(%rip), %xmm0    # 0xa21e8
leaq 0x48(%rsp), %rax
ja 0x2fc97
leaq 0x40(%rsp), %rax
movsd 0x10(%rsp), %xmm0
addsd (%rax), %xmm0
movsd %xmm0, (%rax)
addq $0x8, %r12
incl %ebx
movl 0x238(%r15), %ecx
cmpl %ecx, %ebx
jl 0x2fc0b
movl 0x23c(%r15), %eax
incl %r14d
cmpl %eax, %r14d
jl 0x2fbf3
movl 0x240(%r15), %ecx
movq 0x50(%rsp), %rdx
incl %edx
cmpl %ecx, %edx
jl 0x2fbd0
leaq 0xe0(%rsp), %rdi
leaq 0x30(%rsp), %rax
leaq 0x20(%rsp), %rcx
movb $0x1, %bpl
leaq 0x48(%rsp), %rbx
leaq 0x40(%rsp), %r14
movq 0x38(%rsp), %r12
leaq 0xf8(%rsp), %r15
movsd (%rcx), %xmm0
ucomisd (%rax), %xmm0
jbe 0x2fd1e
callq 0x3d334
movq %r14, %rcx
movq %rbx, %rax
movq %r15, %rdi
testb $0x1, %bpl
movl $0x0, %ebp
jne 0x2fd0f
leaq 0xf8(%rsp), %rbx
leaq 0x168(%rsp), %r15
leaq 0xe0(%rsp), %rsi
movq %r15, %rdi
movq %rbx, %rdx
callq 0x3d8fa
leaq 0x110(%rsp), %r14
movq %r14, %rdi
movq %r15, %rsi
callq 0x3d752
movq 0x60(%rsp), %rax
testq %rax, %rax
je 0x2fd8b
movaps 0x150(%rsp), %xmm0
movups %xmm0, (%rax)
movsd 0x160(%rsp), %xmm0
movsd %xmm0, 0x10(%rax)
leaq 0xe0(%rsp), %rsi
movq %r12, %rdi
movq %rbx, %rdx
movq %r14, %rcx
callq 0x2ab98
movq %r12, %rax
addq $0x238, %rsp            # imm = 0x238
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq