// File: 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:
subq $0x7e8, %rsp            # imm = 0x7E8
movq %rdi, 0x68(%rsp)
movq %rdi, %rax
movq %rax, 0x70(%rsp)
movl 0x7f0(%rsp), %eax
movq %rdi, 0x7e0(%rsp)
movq %rsi, 0x7d8(%rsp)
movq %rdx, 0x7d0(%rsp)
movq %rcx, 0x7c8(%rsp)
movl %r8d, 0x7c4(%rsp)
movl %r9d, 0x7c0(%rsp)
movq 0x7d8(%rsp), %rdi
callq 0x13600
movl %eax, 0x7b0(%rsp)
movq 0x7d0(%rsp), %rdi
callq 0x13600
cmpl 0x7b0(%rsp), %eax
jge 0x8e169
movq 0x7d0(%rsp), %rdi
callq 0x13600
movl %eax, 0x7b0(%rsp)
cmpl $0x1, 0x7b0(%rsp)
jge 0x8e17e
movl $0x0, 0x7c4(%rsp)
cmpl $0x2, 0x7b0(%rsp)
jge 0x8e193
movl $0x0, 0x7f0(%rsp)
cmpl $0x3, 0x7b0(%rsp)
jge 0x8e1a8
movl $0x0, 0x7c0(%rsp)
leaq 0x798(%rsp), %rdi
xorps %xmm2, %xmm2
movaps %xmm2, %xmm0
movaps %xmm2, %xmm1
callq 0x6eb50
leaq 0x780(%rsp), %rdi
xorps %xmm2, %xmm2
movaps %xmm2, %xmm0
movaps %xmm2, %xmm1
callq 0x6eb50
cmpl $0x0, 0x7c4(%rsp)
je 0x8e242
movq 0x7d8(%rsp), %rsi
movq 0x7c8(%rsp), %rdx
leaq 0x768(%rsp), %rdi
callq 0x8cd30
leaq 0x798(%rsp), %rdi
leaq 0x768(%rsp), %rsi
callq 0x6f1e0
movq 0x7d0(%rsp), %rsi
movq 0x7c8(%rsp), %rdx
leaq 0x750(%rsp), %rdi
callq 0x8cd30
leaq 0x780(%rsp), %rdi
leaq 0x750(%rsp), %rsi
callq 0x6f1e0
movsd 0xa4dbe(%rip), %xmm0    # 0x133008
movsd %xmm0, 0x748(%rsp)
movsd 0xa4dad(%rip), %xmm0    # 0x133008
movsd %xmm0, 0x740(%rsp)
cmpl $0x0, 0x7f0(%rsp)
je 0x8e2ba
movq 0x7d8(%rsp), %rsi
movq 0x7c8(%rsp), %rdx
leaq 0x798(%rsp), %rdi
callq 0x8d700
movsd %xmm0, 0x748(%rsp)
movq 0x7d0(%rsp), %rsi
movq 0x7c8(%rsp), %rdx
leaq 0x780(%rsp), %rdi
callq 0x8d700
movsd %xmm0, 0x740(%rsp)
leaq 0x6c0(%rsp), %rdi
leaq 0xea967(%rip), %rsi     # 0x178c30
callq 0x51b60
cmpl $0x0, 0x7c0(%rsp)
je 0x8eb85
leaq 0x640(%rsp), %rdi
leaq 0xea945(%rip), %rsi     # 0x178c30
callq 0x51b60
leaq 0x640(%rsp), %rdi
movl $0x2, %esi
callq 0x51ad0
xorps %xmm0, %xmm0
movsd %xmm0, 0x10(%rax)
leaq 0x640(%rsp), %rdi
movl $0x1, %esi
callq 0x51ad0
xorps %xmm0, %xmm0
movsd %xmm0, 0x8(%rax)
leaq 0x640(%rsp), %rdi
xorl %esi, %esi
callq 0x51ad0
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
xorps %xmm0, %xmm0
movsd %xmm0, 0x638(%rsp)
movl $0x0, 0x7bc(%rsp)
movl 0x7bc(%rsp), %eax
cmpl 0x7b0(%rsp), %eax
jge 0x8e578
cmpq $0x0, 0x7c8(%rsp)
je 0x8e38d
movq 0x7c8(%rsp), %rax
movslq 0x7bc(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
movsd %xmm0, 0x60(%rsp)
jmp 0x8e39d
movsd 0xa4c73(%rip), %xmm0    # 0x133008
movsd %xmm0, 0x60(%rsp)
jmp 0x8e39d
movsd 0x60(%rsp), %xmm0
movsd %xmm0, 0x630(%rsp)
movsd 0x630(%rsp), %xmm0
addsd 0x638(%rsp), %xmm0
movsd %xmm0, 0x638(%rsp)
movq 0x7d8(%rsp), %rdi
movl 0x7bc(%rsp), %esi
callq 0x6d3e0
movq %rax, 0x628(%rsp)
movq 0x7d0(%rsp), %rdi
movl 0x7bc(%rsp), %esi
callq 0x6d3e0
movq %rax, 0x620(%rsp)
movq 0x628(%rsp), %rsi
leaq 0x5f0(%rsp), %rdi
leaq 0x798(%rsp), %rdx
callq 0x6eeb0
movsd 0x748(%rsp), %xmm0
leaq 0x608(%rsp), %rdi
leaq 0x5f0(%rsp), %rsi
callq 0x711f0
movq 0x620(%rsp), %rsi
leaq 0x5c0(%rsp), %rdi
leaq 0x780(%rsp), %rdx
callq 0x6eeb0
movsd 0x740(%rsp), %xmm0
leaq 0x5d8(%rsp), %rdi
leaq 0x5c0(%rsp), %rsi
callq 0x711f0
movl $0x0, 0x7b8(%rsp)
cmpl $0x3, 0x7b8(%rsp)
jge 0x8e560
movl $0x0, 0x7b4(%rsp)
cmpl $0x3, 0x7b4(%rsp)
jge 0x8e548
movsd 0x630(%rsp), %xmm0
movsd %xmm0, 0x48(%rsp)
movl 0x7b8(%rsp), %esi
leaq 0x608(%rsp), %rdi
callq 0x36c00
movsd 0x48(%rsp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, 0x50(%rsp)
movl 0x7b4(%rsp), %esi
leaq 0x5d8(%rsp), %rdi
callq 0x36c00
movsd (%rax), %xmm0
movsd %xmm0, 0x58(%rsp)
movl 0x7b8(%rsp), %esi
leaq 0x640(%rsp), %rdi
callq 0x51ad0
movsd 0x50(%rsp), %xmm0
movsd 0x58(%rsp), %xmm2
movslq 0x7b4(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, (%rax,%rcx,8)
movl 0x7b4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7b4(%rsp)
jmp 0x8e499
jmp 0x8e54a
movl 0x7b8(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7b8(%rsp)
jmp 0x8e480
jmp 0x8e562
movl 0x7bc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7bc(%rsp)
jmp 0x8e351
movsd 0x638(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x8e5a2
jp 0x8e5a2
movq 0x68(%rsp), %rdi
leaq 0xea698(%rip), %rsi     # 0x178c30
callq 0x51b60
jmp 0x8ec4b
movl $0x0, 0x7b8(%rsp)
cmpl $0x3, 0x7b8(%rsp)
jge 0x8e636
movl $0x0, 0x7b4(%rsp)
cmpl $0x3, 0x7b4(%rsp)
jge 0x8e61e
movsd 0x638(%rsp), %xmm0
movsd %xmm0, 0x40(%rsp)
movl 0x7b8(%rsp), %esi
leaq 0x640(%rsp), %rdi
callq 0x51ad0
movsd 0x40(%rsp), %xmm1
movslq 0x7b4(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, (%rax,%rcx,8)
movl 0x7b4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7b4(%rsp)
jmp 0x8e5c2
jmp 0x8e620
movl 0x7b8(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7b8(%rsp)
jmp 0x8e5ad
leaq 0x540(%rsp), %rdi
leaq 0xea5eb(%rip), %rsi     # 0x178c30
callq 0x51b60
movl $0x0, 0x7b8(%rsp)
cmpl $0x3, 0x7b8(%rsp)
jge 0x8e725
movl $0x0, 0x7b4(%rsp)
cmpl $0x3, 0x7b4(%rsp)
jge 0x8e70d
movl 0x7b8(%rsp), %esi
leaq 0x640(%rsp), %rdi
callq 0x51ad0
movslq 0x7b4(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
movsd %xmm0, 0x30(%rsp)
movl 0x7b4(%rsp), %esi
leaq 0x640(%rsp), %rdi
callq 0x51ad0
movsd 0x30(%rsp), %xmm0
movslq 0x7b8(%rsp), %rcx
subsd (%rax,%rcx,8), %xmm0
movsd %xmm0, 0x38(%rsp)
movl 0x7b8(%rsp), %esi
leaq 0x540(%rsp), %rdi
callq 0x51ad0
movsd 0x38(%rsp), %xmm0
movslq 0x7b4(%rsp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl 0x7b4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7b4(%rsp)
jmp 0x8e66e
jmp 0x8e70f
movl 0x7b8(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7b8(%rsp)
jmp 0x8e655
leaq 0x640(%rsp), %rdi
xorl %esi, %esi
callq 0x51ad0
movsd (%rax), %xmm0
movsd %xmm0, 0x20(%rsp)
leaq 0x640(%rsp), %rdi
movl $0x1, %esi
callq 0x51ad0
movsd 0x20(%rsp), %xmm0
addsd 0x8(%rax), %xmm0
movsd %xmm0, 0x28(%rsp)
leaq 0x640(%rsp), %rdi
movl $0x2, %esi
callq 0x51ad0
movsd 0x28(%rsp), %xmm0
addsd 0x10(%rax), %xmm0
movsd %xmm0, 0x538(%rsp)
movsd 0x538(%rsp), %xmm0
movsd %xmm0, 0x4b0(%rsp)
leaq 0x540(%rsp), %rdi
movl $0x1, %esi
callq 0x51ad0
movsd 0x10(%rax), %xmm0
movsd %xmm0, 0x4d0(%rsp)
movsd %xmm0, 0x4b8(%rsp)
leaq 0x540(%rsp), %rdi
movl $0x2, %esi
callq 0x51ad0
movsd (%rax), %xmm0
movsd %xmm0, 0x4f0(%rsp)
movsd %xmm0, 0x4c0(%rsp)
leaq 0x540(%rsp), %rdi
xorl %esi, %esi
callq 0x51ad0
movsd 0x8(%rax), %xmm0
movsd %xmm0, 0x510(%rsp)
movsd %xmm0, 0x4c8(%rsp)
movl $0x0, 0x7b8(%rsp)
cmpl $0x3, 0x7b8(%rsp)
jge 0x8e91a
movl $0x0, 0x7b4(%rsp)
cmpl $0x3, 0x7b4(%rsp)
jge 0x8e8ca
movl 0x7b8(%rsp), %esi
leaq 0x640(%rsp), %rdi
callq 0x51ad0
movslq 0x7b4(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
movsd %xmm0, 0x18(%rsp)
movl 0x7b4(%rsp), %esi
leaq 0x640(%rsp), %rdi
callq 0x51ad0
movsd 0x18(%rsp), %xmm0
movslq 0x7b8(%rsp), %rcx
addsd (%rax,%rcx,8), %xmm0
movl 0x7b8(%rsp), %eax
addl $0x1, %eax
shll $0x2, %eax
movl 0x7b4(%rsp), %ecx
addl $0x1, %ecx
addl %ecx, %eax
cltq
movsd %xmm0, 0x4b0(%rsp,%rax,8)
movl 0x7b4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7b4(%rsp)
jmp 0x8e834
movsd 0x538(%rsp), %xmm1
movl 0x7b8(%rsp), %eax
addl $0x1, %eax
shll $0x2, %eax
movl 0x7b8(%rsp), %ecx
addl $0x1, %ecx
addl %ecx, %eax
cltq
movsd 0x4b0(%rsp,%rax,8), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x4b0(%rsp,%rax,8)
movl 0x7b8(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7b8(%rsp)
jmp 0x8e81b
leaq 0x4b0(%rsp), %rdx
leaq 0x430(%rsp), %rcx
leaq 0x410(%rsp), %r8
leaq 0x390(%rsp), %r9
movl $0x4, %esi
movl %esi, %edi
callq 0xd3dc0
movl $0x0, 0x38c(%rsp)
leaq 0x180(%rsp), %rax
movq %rax, %rcx
addq $0x200, %rcx            # imm = 0x200
movq %rcx, 0x8(%rsp)
movq %rax, 0x10(%rsp)
movq 0x10(%rsp), %rdi
movq %rdi, (%rsp)
callq 0x51b50
movq (%rsp), %rax
movq 0x8(%rsp), %rcx
addq $0x80, %rax
cmpq %rcx, %rax
movq %rax, 0x10(%rsp)
jne 0x8e96d
movl $0x0, 0x7bc(%rsp)
cmpl $0x4, 0x7bc(%rsp)
jge 0x8eb61
movl $0x0, 0x7b8(%rsp)
cmpl $0x4, 0x7b8(%rsp)
jge 0x8ea02
movl 0x7b8(%rsp), %eax
shll $0x2, %eax
addl 0x7bc(%rsp), %eax
cltq
movsd 0x430(%rsp,%rax,8), %xmm0
movslq 0x7b8(%rsp), %rax
movsd %xmm0, 0x140(%rsp,%rax,8)
movl 0x7b8(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7b8(%rsp)
jmp 0x8e9b8
xorps %xmm0, %xmm0
ucomisd 0x140(%rsp), %xmm0
jbe 0x8ea73
movl $0x0, 0x7b8(%rsp)
cmpl $0x4, 0x7b8(%rsp)
jge 0x8ea71
movslq 0x7b8(%rsp), %rax
movsd 0x140(%rsp,%rax,8), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movslq 0x7b8(%rsp), %rax
movsd %xmm0, 0x140(%rsp,%rax,8)
movl 0x7b8(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7b8(%rsp)
jmp 0x8ea1b
jmp 0x8ea73
leaq 0x140(%rsp), %rsi
leaq 0xc0(%rsp), %rdi
callq 0x8ec60
movslq 0x7bc(%rsp), %rax
leaq 0x180(%rsp), %rdi
shlq $0x7, %rax
addq %rax, %rdi
leaq 0xc0(%rsp), %rsi
callq 0x54260
movq 0x7d8(%rsp), %rdi
movq 0x7d0(%rsp), %rsi
movslq 0x7bc(%rsp), %rax
leaq 0x180(%rsp), %rdx
shlq $0x7, %rax
addq %rax, %rdx
movsd 0x748(%rsp), %xmm0
movsd 0x740(%rsp), %xmm1
movq 0x7c8(%rsp), %r9
leaq 0x798(%rsp), %rcx
leaq 0x780(%rsp), %r8
callq 0x8dc00
movslq 0x7bc(%rsp), %rax
movsd %xmm0, 0x160(%rsp,%rax,8)
movslq 0x7bc(%rsp), %rax
movsd 0x160(%rsp,%rax,8), %xmm1
movslq 0x38c(%rsp), %rax
movsd 0x160(%rsp,%rax,8), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x8eb49
movl 0x7bc(%rsp), %eax
movl %eax, 0x38c(%rsp)
jmp 0x8eb4b
movl 0x7bc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7bc(%rsp)
jmp 0x8e99f
movslq 0x38c(%rsp), %rax
leaq 0x180(%rsp), %rsi
shlq $0x7, %rax
addq %rax, %rsi
leaq 0x6c0(%rsp), %rdi
callq 0x54260
movq 0x68(%rsp), %rdi
leaq 0xea09f(%rip), %rsi     # 0x178c30
callq 0x51b60
cmpl $0x0, 0x7c4(%rsp)
je 0x8ebc7
leaq 0xa8(%rsp), %rdi
leaq 0x798(%rsp), %rsi
callq 0x36d10
movq 0x68(%rsp), %rdi
leaq 0xa8(%rsp), %rsi
callq 0x534b0
cmpl $0x0, 0x7f0(%rsp)
je 0x8ebed
movq 0x68(%rsp), %rdi
movsd 0x748(%rsp), %xmm0
divsd 0x740(%rsp), %xmm0
callq 0x53680
cmpl $0x0, 0x7c0(%rsp)
je 0x8ec09
movq 0x68(%rsp), %rdi
leaq 0x6c0(%rsp), %rsi
callq 0x4eac0
cmpl $0x0, 0x7c4(%rsp)
je 0x8ec49
leaq 0x78(%rsp), %rdi
leaq 0x780(%rsp), %rsi
callq 0x36d10
leaq 0x90(%rsp), %rdi
leaq 0x78(%rsp), %rsi
callq 0x70fa0
movq 0x68(%rsp), %rdi
leaq 0x90(%rsp), %rsi
callq 0x534b0
jmp 0x8ec4b
movq 0x70(%rsp), %rax
addq $0x7e8, %rsp            # imm = 0x7E8
retq
nopl (%rax,%rax)