// File: tomfunkhouser[P]gaps/apps/mshalign/mshalign.cpp
// Function: ICPAlignmentTransformation(gaps::R3Mesh*, gaps::R3Point const*, int, gaps::R3Mesh*, gaps::R3Point const*, int, gaps::R3Affine&, gaps::R3Affine&, int, int, int)
static int
ICPAlignmentTransformation(
  R3Mesh *mesh1, const R3Point *points1, int npoints1,
  R3Mesh *mesh2, const R3Point *points2, int npoints2, 
  R3Affine& affine12, R3Affine& affine21,
  int translation, int rotation, int scale)
{
  // Start statistics
  RNTime start_time;
  start_time.Read();

  // Allocate arrays of correspondences
  int ncorrespondences = npoints1 + npoints2;
  R3Point *correspondences_buffer1 = new R3Point [2 * ncorrespondences];
  R3Point *correspondences_buffer2 = new R3Point [2 * ncorrespondences];
  assert(correspondences_buffer1 && correspondences_buffer2);

  // Iterate until max_iterations or converged
  RNBoolean converged = FALSE;
  const int max_iterations = 128;
  for (int iteration = 0; iteration < max_iterations; iteration++) {
    // Get arrays of correspondences
    R3Point *correspondences1 = &correspondences_buffer1[iteration%2 * ncorrespondences];
    R3Point *correspondences2 = &correspondences_buffer2[iteration%2 * ncorrespondences];

    // Update correspondences for aligning transformation
    CreatePointCorrespondences(
      mesh1, points1, npoints1, 
      mesh2, points2, npoints2, 
      affine12, affine21, 
      correspondences1, correspondences2, 
      ncorrespondences);

    // Update aligning transformation for correspondences
    R4Matrix matrix = R3AlignPoints(ncorrespondences, correspondences2, correspondences1, 
      NULL, translation, rotation, scale);
    affine12.Reset(matrix);
    affine21 = affine12.Inverse();

    // Check for convergence
    converged = FALSE;
    if (iteration > 0) {
      converged = TRUE;
      R3Point *prev_correspondences1 = &correspondences_buffer1[(1-(iteration%2)) * ncorrespondences];
      R3Point *prev_correspondences2 = &correspondences_buffer2[(1-(iteration%2)) * ncorrespondences];
      for (int i = 0; i < ncorrespondences; i++) {
        if (!R3Contains(correspondences1[i], prev_correspondences1[i])) { converged = FALSE; break; }
        if (!R3Contains(correspondences2[i], prev_correspondences2[i])) { converged = FALSE; break; }
      }
    }
    if (converged) break;
  }

  // Return whether converged
  return converged;
}

// Assembly:
pushq %rbp
pushq %r14
pushq %rbx
subq $0x2e0, %rsp            # imm = 0x2E0
movl 0x320(%rsp), %eax
movl 0x318(%rsp), %eax
movl 0x310(%rsp), %eax
movq 0x308(%rsp), %rax
movq 0x300(%rsp), %rax
movq %rdi, 0x2d8(%rsp)
movq %rsi, 0x2d0(%rsp)
movl %edx, 0x2cc(%rsp)
movq %rcx, 0x2c0(%rsp)
movq %r8, 0x2b8(%rsp)
movl %r9d, 0x2b4(%rsp)
leaq 0x2a0(%rsp), %rdi
callq 0xa2b30
leaq 0x2a0(%rsp), %rdi
callq 0x15df0
movl 0x2cc(%rsp), %eax
addl 0x2b4(%rsp), %eax
movl %eax, 0x29c(%rsp)
movl 0x29c(%rsp), %eax
shll %eax
cltq
movq %rax, 0xa0(%rsp)
movl $0x18, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
movq %rdi, 0x98(%rsp)
callq 0xd030
movq %rax, %rcx
movq 0xa0(%rsp), %rax
movq %rcx, 0xa8(%rsp)
cmpq $0x0, %rax
je 0x14195
movq 0xa8(%rsp), %rax
movq 0xa0(%rsp), %rcx
imulq $0x18, %rcx, %rdx
movq %rax, %rcx
addq %rdx, %rcx
movq %rcx, 0x88(%rsp)
movq %rax, 0x90(%rsp)
movq 0x90(%rsp), %rdi
movq %rdi, 0x80(%rsp)
callq 0x7d340
jmp 0x14174
movq 0x88(%rsp), %rcx
movq 0x80(%rsp), %rax
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0x90(%rsp)
jne 0x1415d
movq 0xa8(%rsp), %rax
movq %rax, 0x290(%rsp)
movl 0x29c(%rsp), %eax
shll %eax
cltq
movq %rax, 0x70(%rsp)
movl $0x18, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
movq %rdi, 0x68(%rsp)
callq 0xd030
movq %rax, %rcx
movq 0x70(%rsp), %rax
movq %rcx, 0x78(%rsp)
cmpq $0x0, %rax
je 0x14235
movq 0x78(%rsp), %rax
movq 0x70(%rsp), %rcx
imulq $0x18, %rcx, %rdx
movq %rax, %rcx
addq %rdx, %rcx
movq %rcx, 0x58(%rsp)
movq %rax, 0x60(%rsp)
movq 0x60(%rsp), %rdi
movq %rdi, 0x50(%rsp)
callq 0x7d340
jmp 0x1421d
movq 0x58(%rsp), %rcx
movq 0x50(%rsp), %rax
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0x60(%rsp)
jne 0x1420c
movq 0x78(%rsp), %rax
movq %rax, 0x278(%rsp)
movl $0x0, 0x274(%rsp)
movl $0x80, 0x270(%rsp)
movl $0x0, 0x26c(%rsp)
cmpl $0x80, 0x26c(%rsp)
jge 0x1460d
movq 0x290(%rsp), %rax
movl 0x26c(%rsp), %ecx
movl %ecx, %esi
shrl $0x1f, %esi
movl %ecx, %edx
addl %esi, %edx
andl $-0x2, %edx
subl %edx, %ecx
movl 0x29c(%rsp), %edx
imull %edx, %ecx
movslq %ecx, %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rax
movq %rax, 0x260(%rsp)
movq 0x278(%rsp), %rax
movl 0x26c(%rsp), %ecx
movl %ecx, %esi
shrl $0x1f, %esi
movl %ecx, %edx
addl %esi, %edx
andl $-0x2, %edx
subl %edx, %ecx
movl 0x29c(%rsp), %edx
imull %edx, %ecx
movslq %ecx, %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rax
movq %rax, 0x258(%rsp)
movq 0x2d8(%rsp), %rdi
movq 0x2d0(%rsp), %rsi
movl 0x2cc(%rsp), %edx
movq 0x2c0(%rsp), %rcx
movq 0x2b8(%rsp), %r8
movl 0x2b4(%rsp), %r9d
movq 0x300(%rsp), %r10
movq 0x308(%rsp), %r11
movq 0x260(%rsp), %rbx
movq 0x258(%rsp), %r14
movl 0x29c(%rsp), %ebp
movq %rsp, %rax
movl %ebp, 0x20(%rax)
movq %r14, 0x18(%rax)
movq %rbx, 0x10(%rax)
movq %r11, 0x8(%rax)
movq %r10, (%rax)
callq 0x15550
movl 0x29c(%rsp), %esi
movq 0x258(%rsp), %rdx
movq 0x260(%rsp), %rcx
movl 0x310(%rsp), %r9d
movl 0x318(%rsp), %edi
movl 0x320(%rsp), %r8d
movq %rsp, %rax
movl %r8d, 0x8(%rax)
movl %edi, (%rax)
xorl %eax, %eax
movl %eax, 0x44(%rsp)
movl %eax, %r8d
leaq 0x1d8(%rsp), %rdi
movq %rdi, 0x38(%rsp)
callq 0x36ff0
movq 0x38(%rsp), %rsi
movl 0x44(%rsp), %edx
movq 0x300(%rsp), %rdi
callq 0x73340
movq 0x300(%rsp), %rsi
leaq 0xc8(%rsp), %rdi
movq %rdi, 0x48(%rsp)
callq 0x16010
movq 0x48(%rsp), %rsi
movq 0x308(%rsp), %rdi
callq 0x16070
jmp 0x143ee
leaq 0xc8(%rsp), %rdi
callq 0x725b0
movl $0x0, 0x274(%rsp)
cmpl $0x0, 0x26c(%rsp)
jle 0x145e9
movl $0x1, 0x274(%rsp)
movq 0x290(%rsp), %rax
movq %rax, 0x28(%rsp)
movl 0x26c(%rsp), %eax
movl $0x2, %ecx
cltd
idivl %ecx
movq 0x28(%rsp), %rax
movl $0x1, %ecx
subl %edx, %ecx
imull 0x29c(%rsp), %ecx
movslq %ecx, %rcx
imulq $0x18, %rcx, %rcx
addq %rcx, %rax
movq %rax, 0xc0(%rsp)
movq 0x278(%rsp), %rax
movq %rax, 0x30(%rsp)
movl 0x26c(%rsp), %eax
movl $0x2, %ecx
cltd
idivl %ecx
movq 0x30(%rsp), %rax
movl $0x1, %ecx
subl %edx, %ecx
imull 0x29c(%rsp), %ecx
movslq %ecx, %rcx
imulq $0x18, %rcx, %rcx
addq %rcx, %rax
movq %rax, 0xb8(%rsp)
movl $0x0, 0xb4(%rsp)
movl 0xb4(%rsp), %eax
cmpl 0x29c(%rsp), %eax
jge 0x145e7
movq 0x260(%rsp), %rdi
movslq 0xb4(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
movq 0xc0(%rsp), %rsi
movslq 0xb4(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rsi
callq 0x28920
cmpl $0x0, %eax
jne 0x1458a
movl $0x0, 0x274(%rsp)
jmp 0x145e7
movq 0x98(%rsp), %rsi
movq 0xa8(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x288(%rsp)
movl %eax, 0x284(%rsp)
callq 0xd760
jmp 0x14620
movq 0x68(%rsp), %rsi
movq 0x78(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x288(%rsp)
movl %eax, 0x284(%rsp)
callq 0xd760
jmp 0x14620
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x288(%rsp)
movl %eax, 0x284(%rsp)
leaq 0xc8(%rsp), %rdi
callq 0x725b0
jmp 0x14620
movq 0x258(%rsp), %rdi
movslq 0xb4(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
movq 0xb8(%rsp), %rsi
movslq 0xb4(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rsi
callq 0x28920
cmpl $0x0, %eax
jne 0x145cf
movl $0x0, 0x274(%rsp)
jmp 0x145e7
jmp 0x145d1
movl 0xb4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xb4(%rsp)
jmp 0x144ae
jmp 0x145e9
cmpl $0x0, 0x274(%rsp)
je 0x145f5
jmp 0x1460d
jmp 0x145f7
movl 0x26c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x26c(%rsp)
jmp 0x14263
movl 0x274(%rsp), %eax
addq $0x2e0, %rsp            # imm = 0x2E0
popq %rbx
popq %r14
popq %rbp
retq
movq 0x288(%rsp), %rdi
callq 0xd6a0
nopl (%rax)