// File: /workspace/github_cpp_makefile_star2_O1/tomfunkhouser[P]gaps/pkgs/R3Shapes/R3Align.cpp
// Function: gaps::R3AlignPoints(gaps::RNArray<gaps::R3Point*> const&, gaps::RNArray<gaps::R3Point*> const&, double const*, int, int, int)
R4Matrix
R3AlignPoints(const RNArray<R3Point *>& points1, const RNArray<R3Point *>& points2, 
  const RNScalar* weights, RNBoolean align_center, RNBoolean align_rotation, int align_scale)
{
  int i,j,k;

  // Get count of points
  int count = points1.NEntries();
  if (points2.NEntries() < count) count = points2.NEntries();

  // Check number of points
  if (count < 1) align_center = 0;
  if (count < 2) align_scale = 0;
  if (count < 3) align_rotation = 0;

  // Compute centers
  R3Point center1(0.0, 0.0, 0.0);
  R3Point center2(0.0, 0.0, 0.0);
  if (align_center){
    center1 = R3Centroid(points1, weights);
    center2 = R3Centroid(points2, weights);
  }

  // Compute scales
  RNScalar s1 = 1;
  RNScalar s2 = 1;
  if (align_scale){
    s1 = R3AverageDistance(center1, points1, weights);
    s2 = R3AverageDistance(center2, points2, weights);
  }

  // Compute cross-covariance of two point sets
  R4Matrix rotation = R4identity_matrix;
  if (align_rotation) {
    R4Matrix m = R4identity_matrix;
    m[0][0] = m[1][1] = m[2][2] = 0;
    RNScalar total_weight = 0;
    for (i=0; i< count; i++){
      RNScalar weight = (weights) ? weights[i] : 1;
      total_weight += weight;
      R3Point *point1 = points1[i];
      R3Point *point2 = points2[i];
      R3Vector p1 = (*point1 - center1) / s1;
      R3Vector p2 = (*point2 - center2) / s2;
      for(j=0;j<3;j++){
        for(k=0;k<3;k++){
          m[j][k] += weight * p1[j]*p2[k];
        }
      }
    }

    // Normalize cross-covariance matrix
    if (total_weight == 0) return R4identity_matrix;
    for(j=0;j<3;j++){for(k=0;k<3;k++){ m[j][k] /= total_weight; } }

    // Make cross-covariance matrix skew-symmetric
    R4Matrix a = R4identity_matrix;
    for(j=0;j<3;j++){for(k=0;k<3;k++){a[j][k]=m[j][k]-m[k][j];}}
    
    // Compute trace of cross-covariance matrix
    RNScalar trace=m[0][0]+m[1][1]+m[2][2];
    
    // Setup symmetric matrix whose eigenvectors give quaternion terms of optimal rotation
    RNScalar M[16];
    M[0]=trace;
    M[1]=M[4]=a[1][2];
    M[2]=M[8]=a[2][0];
    M[3]=M[12]=a[0][1];
    for(j=0;j<3;j++){
      for(k=0;k<3;k++){M[4*(j+1)+(k+1)]=m[j][k]+m[k][j];}
      M[4*(j+1)+(j+1)]-=trace;
    }

    // Perform SVD to get eigenvectors (quaternion terms of optimal rotation)
    RNScalar U[16];
    RNScalar W[4];
    RNScalar Vt[16];
    RNSvdDecompose(4, 4, M, U, W, Vt);  
    
    // Look at error using all eigenvectors and keep best
    int minI=0;
    R4Matrix temp[4];
    RNScalar e[4];
    for(i=0;i<4;i++){
      RNScalar p[4];
      for(j=0;j<4;j++){p[j]=U[4*j+i];}
      if(p[0]<0){for(j=0;j<4;j++){p[j]=-p[j];}}
      temp[i] = SetQuaternion(p);
      e[i]= R3AlignError(points1, points2, temp[i], center1, center2, s1, s2, weights);
      if (e[i]<e[minI]) minI=i;
    }
    rotation = temp[minI];
  }

  // Compute result
  R4Matrix result = R4identity_matrix;
  if (align_center) result.Translate(center1.Vector());
  if (align_scale) result.Scale(s1/s2);
  if (align_rotation) result.Transform(rotation);
  if (align_center) result.Translate(-(center2.Vector()));

  // Return resulting matrix that takes points2 to points1
  return result;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x6f8, %rsp            # imm = 0x6F8
movl %r9d, %ebx
movl %r8d, %ebp
movq %rcx, %r13
movq %rdi, 0x10(%rsp)
movl 0x730(%rsp), %r12d
movq %rsi, 0x28(%rsp)
movl 0xc(%rsi), %r15d
movq %rdx, 0x30(%rsp)
movl 0xc(%rdx), %eax
cmpl %r15d, %eax
cmovll %eax, %r15d
testl %r15d, %r15d
setle %r14b
leaq 0x78(%rsp), %rdi
xorpd %xmm0, %xmm0
xorpd %xmm1, %xmm1
xorpd %xmm2, %xmm2
callq 0x3ca26
leaq 0x60(%rsp), %rdi
xorpd %xmm0, %xmm0
xorpd %xmm1, %xmm1
xorpd %xmm2, %xmm2
callq 0x3ca26
testl %ebp, %ebp
sete %bpl
orb %r14b, %bpl
jne 0x4d8fd
leaq 0x370(%rsp), %r14
movq %r14, %rdi
movq 0x28(%rsp), %rsi
movq %r13, %rdx
callq 0x4ccf0
leaq 0x78(%rsp), %rdi
movq %r14, %rsi
callq 0x3cd94
leaq 0x370(%rsp), %r14
movq %r14, %rdi
movq 0x30(%rsp), %rsi
movq %r13, %rdx
callq 0x4ccf0
leaq 0x60(%rsp), %rdi
movq %r14, %rsi
callq 0x3cd94
cmpl $0x2, %r15d
setl %al
testl %r12d, %r12d
sete %cl
orb %al, %cl
movb %cl, 0xe(%rsp)
movb %bpl, 0xf(%rsp)
je 0x4d927
movsd 0x54719(%rip), %xmm0    # 0xa2038
movsd %xmm0, 0x20(%rsp)
jmp 0x4d951
leaq 0x78(%rsp), %rdi
movq 0x28(%rsp), %rsi
movq %r13, %rdx
callq 0x4d1a8
movsd %xmm0, 0x20(%rsp)
leaq 0x60(%rsp), %rdi
movq 0x30(%rsp), %rsi
movq %r13, %rdx
callq 0x4d1a8
movsd %xmm0, 0x18(%rsp)
movq 0x10(%rsp), %r12
cmpl $0x3, %r15d
setl %bpl
leaq 0x554d5(%rip), %rsi     # 0xa2e40
leaq 0x2f0(%rsp), %rdi
callq 0x2c5ba
testl %ebx, %ebx
sete %al
orb %bpl, %al
movb %al, 0xd(%rsp)
jne 0x4dfd5
leaq 0x554af(%rip), %rsi     # 0xa2e40
leaq 0x150(%rsp), %rbx
movq %rbx, %rdi
callq 0x2c5ba
xorl %ebp, %ebp
movq %rbp, 0x50(%rbx)
movq %rbp, 0x28(%rbx)
movq %rbp, (%rbx)
movl %r15d, %r15d
xorpd %xmm1, %xmm1
leaq 0x270(%rsp), %rbx
movsd %xmm1, 0x38(%rsp)
movsd 0x5466d(%rip), %xmm0    # 0xa2038
testq %r13, %r13
je 0x4d9d7
movsd (%r13,%rbp,8), %xmm0
movsd %xmm0, 0x90(%rsp)
movq %r13, %r12
movq 0x28(%rsp), %rax
movq (%rax), %rax
movq (%rax,%rbp,8), %rsi
movq 0x30(%rsp), %rax
movq (%rax), %rax
movq (%rax,%rbp,8), %r13
leaq 0xb0(%rsp), %r14
movq %r14, %rdi
leaq 0x78(%rsp), %rdx
callq 0x3cc88
leaq 0x370(%rsp), %rdi
movq %r14, %rsi
movsd 0x20(%rsp), %xmm0
callq 0x3e200
movq %rbx, %rdi
movq %r13, %rsi
leaq 0x60(%rsp), %rdx
callq 0x3cc88
movq %r14, %rdi
movq %rbx, %rsi
movsd 0x18(%rsp), %xmm0
callq 0x3e200
movsd 0x90(%rsp), %xmm2
leaq 0x150(%rsp), %rax
xorl %ecx, %ecx
movsd 0x370(%rsp,%rcx,8), %xmm0
mulsd %xmm2, %xmm0
xorl %edx, %edx
movsd 0xb0(%rsp,%rdx,8), %xmm1
mulsd %xmm0, %xmm1
addsd (%rax,%rdx,8), %xmm1
movsd %xmm1, (%rax,%rdx,8)
incq %rdx
cmpq $0x3, %rdx
jne 0x4da69
incq %rcx
addq $0x20, %rax
cmpq $0x3, %rcx
jne 0x4da5a
movsd 0x38(%rsp), %xmm1
addsd %xmm2, %xmm1
incq %rbp
cmpq %r15, %rbp
movq %r12, %r13
jne 0x4d9bd
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm1
movsd %xmm1, 0x38(%rsp)
jne 0x4dac5
jnp 0x4dfab
leaq 0x150(%rsp), %rax
xorl %ecx, %ecx
xorl %edx, %edx
movsd (%rax,%rdx,8), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, (%rax,%rdx,8)
incq %rdx
cmpq $0x3, %rdx
jne 0x4dad1
incq %rcx
addq $0x20, %rax
cmpq $0x3, %rcx
jne 0x4dacf
leaq 0x55344(%rip), %rsi     # 0xa2e40
movq %rbx, %rdi
callq 0x2c5ba
leaq 0x150(%rsp), %rax
xorl %ecx, %ecx
movq %rax, %rdx
xorl %esi, %esi
movsd (%rdx,%rsi), %xmm0
subsd (%rax,%rsi,4), %xmm0
movsd %xmm0, (%rbx,%rsi)
addq $0x8, %rsi
cmpq $0x18, %rsi
jne 0x4db13
incq %rcx
addq $0x20, %rbx
addq $0x20, %rdx
addq $0x8, %rax
cmpq $0x3, %rcx
jne 0x4db11
leaq 0x150(%rsp), %rax
movsd (%rax), %xmm0
addsd 0x28(%rax), %xmm0
addsd 0x50(%rax), %xmm0
leaq 0x218(%rsp), %rcx
movsd %xmm0, -0x28(%rcx)
movsd 0x278(%rsp), %xmm1
movsd 0x2a0(%rsp), %xmm2
movsd %xmm2, -0x8(%rcx)
movsd %xmm2, -0x20(%rcx)
movsd 0x2b0(%rsp), %xmm2
movsd %xmm2, 0x18(%rcx)
movsd %xmm2, -0x18(%rcx)
movsd %xmm1, 0x38(%rcx)
movsd %xmm1, -0x10(%rcx)
xorl %edx, %edx
movq %rax, %rsi
xorl %edi, %edi
movsd (%rsi,%rdi), %xmm1
addsd (%rax,%rdi,4), %xmm1
movsd %xmm1, (%rcx,%rdi)
addq $0x8, %rdi
cmpq $0x18, %rdi
jne 0x4dba4
incq %rdx
leaq (%rdx,%rdx,4), %rdi
movsd 0x1f0(%rsp,%rdi,8), %xmm1
subsd %xmm0, %xmm1
movsd %xmm1, 0x1f0(%rsp,%rdi,8)
addq $0x20, %rsi
addq $0x8, %rax
addq $0x20, %rcx
cmpq $0x3, %rdx
jne 0x4dba2
leaq 0x1f0(%rsp), %rdx
leaq 0x5f0(%rsp), %rbx
leaq 0x1d0(%rsp), %r8
leaq 0x570(%rsp), %r9
movl $0x4, %edi
movl $0x4, %esi
movq %rbx, %rcx
callq 0x720bd
xorl %r14d, %r14d
leaq (%rsp,%r14), %rdi
addq $0x370, %rdi            # imm = 0x370
callq 0x2c5b8
subq $-0x80, %r14
cmpq $0x200, %r14            # imm = 0x200
jne 0x4dc21
xorl %r15d, %r15d
movapd 0x543d7(%rip), %xmm1    # 0xa2020
leaq 0x678(%rsp), %rbp
xorl %r12d, %r12d
xorl %eax, %eax
movsd (%rbx,%rax,4), %xmm0
movsd %xmm0, 0x40(%rsp,%rax)
addq $0x8, %rax
cmpq $0x20, %rax
jne 0x4dc56
xorpd %xmm0, %xmm0
ucomisd 0x40(%rsp), %xmm0
jbe 0x4dc93
xorl %eax, %eax
movapd 0x40(%rsp,%rax,8), %xmm0
xorpd %xmm1, %xmm0
movapd %xmm0, 0x40(%rsp,%rax,8)
addq $0x2, %rax
cmpq $0x4, %rax
jne 0x4dc79
leaq 0xb0(%rsp), %rdi
leaq 0x5519e(%rip), %rsi     # 0xa2e40
callq 0x2c5ba
movsd 0x40(%rsp), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x4dcdf
movhpd 0x48(%rsp), %xmm0       # xmm0 = xmm0[0],mem[0]
movapd 0x5435b(%rip), %xmm1    # 0xa2020
xorpd %xmm1, %xmm0
movapd %xmm0, 0x40(%rsp)
movapd 0x50(%rsp), %xmm0
xorpd %xmm1, %xmm0
movapd %xmm0, 0x50(%rsp)
movapd 0x40(%rsp), %xmm11
movapd 0x50(%rsp), %xmm10
movapd %xmm11, %xmm0
mulpd %xmm11, %xmm0
unpckhpd %xmm0, %xmm0            # xmm0 = xmm0[1,1]
movapd %xmm11, %xmm1
mulsd %xmm11, %xmm1
addsd %xmm0, %xmm1
movapd %xmm10, %xmm2
mulsd %xmm10, %xmm2
addsd %xmm1, %xmm2
movapd %xmm10, %xmm0
unpckhpd %xmm10, %xmm0           # xmm0 = xmm0[1],xmm10[1]
mulsd %xmm0, %xmm0
addsd %xmm2, %xmm0
movsd 0x544af(%rip), %xmm1    # 0xa21e0
ucomisd %xmm0, %xmm1
leaq 0x55104(%rip), %rsi     # 0xa2e40
ja 0x4defd
ucomisd 0x5449e(%rip), %xmm0    # 0xa21e8
jb 0x4dd52
sqrtsd %xmm0, %xmm0
jmp 0x4dd7f
movapd %xmm10, 0x90(%rsp)
movapd %xmm11, 0xa0(%rsp)
callq 0xe340
movapd 0xa0(%rsp), %xmm11
movapd 0x90(%rsp), %xmm10
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
divpd %xmm0, %xmm11
movapd %xmm11, 0x40(%rsp)
divpd %xmm0, %xmm10
movapd %xmm10, 0x50(%rsp)
movapd %xmm11, %xmm4
unpckhpd %xmm11, %xmm4           # xmm4 = xmm4[1],xmm11[1]
movapd %xmm4, %xmm5
mulsd %xmm4, %xmm5
movapd %xmm11, %xmm0
mulsd %xmm11, %xmm0
addsd %xmm0, %xmm5
movapd %xmm10, %xmm3
shufpd $0x1, %xmm10, %xmm3     # xmm3 = xmm3[1],xmm10[0]
movapd %xmm10, %xmm1
movapd 0x5424c(%rip), %xmm9    # 0xa2020
xorpd %xmm9, %xmm1
mulsd %xmm10, %xmm1
addsd %xmm1, %xmm5
movapd %xmm10, %xmm2
unpckhpd %xmm10, %xmm2           # xmm2 = xmm2[1],xmm10[1]
movapd %xmm2, %xmm6
xorpd %xmm9, %xmm6
mulsd %xmm2, %xmm6
addsd %xmm6, %xmm5
movsd %xmm5, 0xb0(%rsp)
movapd %xmm11, %xmm5
xorpd %xmm9, %xmm5
movapd %xmm11, %xmm7
unpcklpd %xmm5, %xmm7            # xmm7 = xmm7[0],xmm5[0]
mulpd %xmm3, %xmm7
movapd %xmm4, %xmm8
mulpd %xmm10, %xmm8
addpd %xmm7, %xmm8
addpd %xmm8, %xmm8
movupd %xmm8, 0xb8(%rsp)
movapd %xmm2, %xmm7
mulsd %xmm11, %xmm7
movapd %xmm4, %xmm8
mulsd %xmm10, %xmm8
subsd %xmm7, %xmm8
addsd %xmm8, %xmm8
movsd %xmm8, 0xd0(%rsp)
movapd %xmm10, %xmm8
mulsd %xmm10, %xmm8
addsd %xmm0, %xmm8
movapd %xmm4, %xmm7
xorpd %xmm9, %xmm7
mulsd %xmm4, %xmm7
addsd %xmm7, %xmm8
addsd %xmm6, %xmm8
movsd %xmm8, 0xd8(%rsp)
mulsd %xmm11, %xmm4
movapd %xmm10, %xmm6
mulsd %xmm2, %xmm6
addsd %xmm4, %xmm6
addsd %xmm6, %xmm6
movsd %xmm6, 0xe0(%rsp)
movapd %xmm10, %xmm4
unpcklpd %xmm5, %xmm4            # xmm4 = xmm4[0],xmm5[0]
mulpd %xmm11, %xmm4
movhlps %xmm11, %xmm10          # xmm10 = xmm11[1],xmm10[1]
mulpd %xmm3, %xmm10
addpd %xmm4, %xmm10
addpd %xmm10, %xmm10
movupd %xmm10, 0xf0(%rsp)
mulsd %xmm2, %xmm2
addsd %xmm0, %xmm2
addsd %xmm7, %xmm2
addsd %xmm1, %xmm2
movsd %xmm2, 0x100(%rsp)
leaq 0xb0(%rsp), %rsi
movq %rbp, %rdi
callq 0x2c5ba
movq %r15, %rax
shlq $0x7, %rax
leaq (%rsp,%rax), %r14
addq $0x370, %r14            # imm = 0x370
movq %r14, %rdi
movq %rbp, %rsi
callq 0x2e19e
movq 0x28(%rsp), %rdi
movq 0x30(%rsp), %rsi
movq %r14, %rdx
leaq 0x78(%rsp), %rcx
leaq 0x60(%rsp), %r8
movsd 0x20(%rsp), %xmm0
movsd 0x18(%rsp), %xmm1
movq %r13, %r9
callq 0x4d55c
movsd %xmm0, 0x130(%rsp,%r15,8)
movslq %r12d, %r12
movsd 0x130(%rsp,%r12,8), %xmm1
ucomisd %xmm0, %xmm1
cmoval %r15d, %r12d
incq %r15
addq $0x8, %rbx
cmpq $0x4, %r15
movapd 0x540a1(%rip), %xmm1    # 0xa2020
jne 0x4dc54
movslq %r12d, %rax
shlq $0x7, %rax
leaq (%rsp,%rax), %rsi
addq $0x370, %rsi            # imm = 0x370
leaq 0x2f0(%rsp), %rdi
callq 0x2e19e
movq 0x10(%rsp), %r12
jmp 0x4dfbf
leaq 0x54e8e(%rip), %rsi     # 0xa2e40
movq 0x10(%rsp), %r12
movq %r12, %rdi
callq 0x2c5ba
xorpd %xmm0, %xmm0
movsd 0x38(%rsp), %xmm1
ucomisd %xmm0, %xmm1
jne 0x4dfd5
jnp 0x4e0ae
leaq 0x54e64(%rip), %rsi     # 0xa2e40
movq %r12, %rdi
callq 0x2c5ba
movb 0xf(%rsp), %bpl
testb %bpl, %bpl
jne 0x4e021
movsd 0x78(%rsp), %xmm0
movsd 0x80(%rsp), %xmm1
movsd 0x88(%rsp), %xmm2
leaq 0x370(%rsp), %rbx
movq %rbx, %rdi
callq 0x3d4be
movq %r12, %rdi
movq %rbx, %rsi
callq 0x2d6f4
cmpb $0x0, 0xe(%rsp)
jne 0x4e03c
movsd 0x20(%rsp), %xmm0
divsd 0x18(%rsp), %xmm0
movq %r12, %rdi
callq 0x2d8a2
cmpb $0x0, 0xd(%rsp)
jne 0x4e069
leaq 0x370(%rsp), %rbx
leaq 0x2f0(%rsp), %rdx
movq %rbx, %rdi
movq %r12, %rsi
callq 0x2e5ab
movq %r12, %rdi
movq %rbx, %rsi
callq 0x2e19e
testb %bpl, %bpl
jne 0x4e0ae
movsd 0x60(%rsp), %xmm0
movsd 0x68(%rsp), %xmm1
movsd 0x70(%rsp), %xmm2
leaq 0xb0(%rsp), %rbx
movq %rbx, %rdi
callq 0x3d4be
leaq 0x370(%rsp), %r14
movq %r14, %rdi
movq %rbx, %rsi
callq 0x3e154
movq %r12, %rdi
movq %r14, %rsi
callq 0x2d6f4
movq %r12, %rax
addq $0x6f8, %rsp            # imm = 0x6F8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq