// File: tomfunkhouser[P]gaps/apps/mshalign/mshalign.cpp
// Function: RansacAlignmentTransformation(gaps::R3Mesh*, gaps::R3Point const*, int, gaps::R3Mesh*, gaps::R3Point const*, int, gaps::R3Affine&, gaps::R3Affine&, int, int, int, double, int)
static int
RansacAlignmentTransformation(
  R3Mesh *mesh1, const R3Point *points1, int npoints1,
  R3Mesh *mesh2, const R3Point *points2, int npoints2, 
  R3Affine& affine12, R3Affine& affine21,
  int translation, int rotation, int scale,
  RNLength support_sigma = 0, int max_iterations = 0)
{
  // Check number of points
  if (npoints1 < 3) return 0;
  if (npoints2 < 3) return 0;

  // Start statistics
  RNTime start_time;
  start_time.Read();

  // Initialize result
  affine12 = R3identity_affine;
  affine21 = R3identity_affine;

  // Create temporary memory
  int max_correspondences = npoints1 + npoints2;
  R3Point *correspondences1 = new R3Point [ max_correspondences ];
  R3Point *correspondences2 = new R3Point [ max_correspondences ];

  // Compute useful variables
  if (max_iterations == 0) max_iterations = 0.1 * npoints1 * npoints2; // should be n^3
  if (support_sigma <= 0) support_sigma = 0.5 * mesh1->AverageRadius();
  RNScalar support_factor = -1.0 / (2 * support_sigma * support_sigma);

  // Generate random alignments and keep the one with best support
  RNScalar best_support = 0;
  R3Affine best_affine21 = R3identity_affine;
  for (int iteration = 0; iteration < max_iterations; iteration++) {
    int a1, b1, c1, a2, b2, c2;
    a1 = (int) (RNRandomScalar() * npoints1); 
    do { b1 = (int) (RNRandomScalar() * npoints1); } while (b1 == a1);
    do { c1 = (int) (RNRandomScalar() * npoints1); } while ((c1 == a1) || (c1 == b1));
    a2 = (int) (RNRandomScalar() * npoints2); 
    do { b2 = (int) (RNRandomScalar() * npoints2); } while (b2 == a2);
    do { c2 = (int) (RNRandomScalar() * npoints2); } while ((c2 == a2) || (c2 == b2));

    // Create triplets of points
    static RNArray<R3Point *> triplet1; 
    triplet1.Empty();
    triplet1.Insert((R3Point *) &points1[a1]);
    triplet1.Insert((R3Point *) &points1[b1]);
    triplet1.Insert((R3Point *) &points1[c1]);
    static RNArray<R3Point *> triplet2;
    triplet2.Empty();
    triplet2.Insert((R3Point *) &points2[a2]);
    triplet2.Insert((R3Point *) &points2[b2]);
    triplet2.Insert((R3Point *) &points2[c2]);

    // Compute transformation aligning triplet2 to triplet1
    R4Matrix matrix21 = R3AlignPoints(triplet1, triplet2, NULL, translation, rotation, scale);
    R3Affine aff21(matrix21);
    R3Affine aff12 = aff21.Inverse();

    // Compute correspondences
    int ncorrespondences = CreatePointCorrespondences(mesh1, points1, npoints1, mesh2, points2, npoints2, 
      aff12, aff21, correspondences1, correspondences2, max_correspondences);

    // Compute support
    RNScalar support = 0;
    for (int i = 0; i < ncorrespondences; i++) {
      R3Point& point1 = correspondences1[i];
      R3Point point2 = correspondences2[i];
      point2.Transform(aff21);
      RNScalar dd = R3SquaredDistance(point1, point2);
      RNScalar s = exp(support_factor * dd);
      support += s;
    }

    // Check if best support so far
    if (support > best_support) {
      best_affine21 = aff21;
      best_support = support;
    }
  }

  // Assign results
  affine21 = best_affine21;
  affine12 = best_affine21.Inverse();
  
  // Delete temporary data
  delete [] correspondences1;
  delete [] correspondences2;

  // Return success
  return 1;
}

// Assembly:
pushq %rbx
subq $0x690, %rsp            # imm = 0x690
movl 0x6c8(%rsp), %eax
movl 0x6c0(%rsp), %eax
movl 0x6b8(%rsp), %eax
movl 0x6b0(%rsp), %eax
movq 0x6a8(%rsp), %rax
movq 0x6a0(%rsp), %rax
movq %rdi, 0x680(%rsp)
movq %rsi, 0x678(%rsp)
movl %edx, 0x674(%rsp)
movq %rcx, 0x668(%rsp)
movq %r8, 0x660(%rsp)
movl %r9d, 0x65c(%rsp)
movsd %xmm0, 0x650(%rsp)
cmpl $0x3, 0x674(%rsp)
jge 0x14ae6
movl $0x0, 0x68c(%rsp)
jmp 0x15531
cmpl $0x3, 0x65c(%rsp)
jge 0x14b00
movl $0x0, 0x68c(%rsp)
jmp 0x15531
leaq 0x640(%rsp), %rdi
callq 0xa2b30
leaq 0x640(%rsp), %rdi
callq 0x15df0
movq 0x6a0(%rsp), %rdi
leaq 0xb96c7(%rip), %rsi     # 0xce1f0
callq 0x16070
movq 0x6a8(%rsp), %rdi
leaq 0xb96b3(%rip), %rsi     # 0xce1f0
callq 0x16070
movl 0x674(%rsp), %eax
addl 0x65c(%rsp), %eax
movl %eax, 0x63c(%rsp)
movslq 0x63c(%rsp), %rax
movq %rax, 0xd0(%rsp)
movl $0x18, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
movq %rdi, 0xc8(%rsp)
callq 0xd030
movq %rax, %rcx
movq 0xd0(%rsp), %rax
movq %rcx, 0xd8(%rsp)
cmpq $0x0, %rax
je 0x14c0b
movq 0xd8(%rsp), %rax
movq 0xd0(%rsp), %rcx
imulq $0x18, %rcx, %rdx
movq %rax, %rcx
addq %rdx, %rcx
movq %rcx, 0xb8(%rsp)
movq %rax, 0xc0(%rsp)
movq 0xc0(%rsp), %rdi
movq %rdi, 0xb0(%rsp)
callq 0x7d340
jmp 0x14bea
movq 0xb8(%rsp), %rcx
movq 0xb0(%rsp), %rax
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0xc0(%rsp)
jne 0x14bd3
movq 0xd8(%rsp), %rax
movq %rax, 0x630(%rsp)
movslq 0x63c(%rsp), %rax
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 0x14ccf
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 0x14cae
movq 0x88(%rsp), %rcx
movq 0x80(%rsp), %rax
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0x90(%rsp)
jne 0x14c97
movq 0xa8(%rsp), %rax
movq %rax, 0x618(%rsp)
cmpl $0x0, 0x6c8(%rsp)
jne 0x14d78
cvtsi2sdl 0x674(%rsp), %xmm1
movsd 0x92312(%rip), %xmm0    # 0xa7010
mulsd %xmm1, %xmm0
cvtsi2sdl 0x65c(%rsp), %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x6c8(%rsp)
jmp 0x14d78
movq 0xc8(%rsp), %rsi
movq 0xd8(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x628(%rsp)
movl %eax, 0x624(%rsp)
callq 0xd760
jmp 0x15541
movq 0x98(%rsp), %rsi
movq 0xa8(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x628(%rsp)
movl %eax, 0x624(%rsp)
callq 0xd760
jmp 0x15541
xorps %xmm0, %xmm0
ucomisd 0x650(%rsp), %xmm0
jb 0x14daf
movq 0x680(%rsp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x38790
movaps %xmm0, %xmm1
movsd 0x92276(%rip), %xmm0    # 0xa7018
mulsd %xmm1, %xmm0
movsd %xmm0, 0x650(%rsp)
movsd 0x92271(%rip), %xmm1    # 0xa7028
mulsd 0x650(%rsp), %xmm1
mulsd 0x650(%rsp), %xmm1
movsd 0x9224f(%rip), %xmm0    # 0xa7020
divsd %xmm1, %xmm0
movsd %xmm0, 0x610(%rsp)
xorps %xmm0, %xmm0
movsd %xmm0, 0x608(%rsp)
leaq 0x4f8(%rsp), %rdi
leaq 0xb93f7(%rip), %rsi     # 0xce1f0
callq 0x15e80
movl $0x0, 0x4f4(%rsp)
movl 0x4f4(%rsp), %eax
cmpl 0x6c8(%rsp), %eax
jge 0x1545b
callq 0xa6530
movsd %xmm0, 0x78(%rsp)
jmp 0x14e2a
movsd 0x78(%rsp), %xmm0
cvtsi2sdl 0x674(%rsp), %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x4f0(%rsp)
callq 0xa6530
movsd %xmm0, 0x70(%rsp)
jmp 0x14e55
movsd 0x70(%rsp), %xmm0
cvtsi2sdl 0x674(%rsp), %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x4ec(%rsp)
movl 0x4ec(%rsp), %eax
cmpl 0x4f0(%rsp), %eax
je 0x14e48
jmp 0x14e85
callq 0xa6530
movsd %xmm0, 0x68(%rsp)
jmp 0x14e92
movsd 0x68(%rsp), %xmm0
cvtsi2sdl 0x674(%rsp), %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x4e8(%rsp)
movl 0x4e8(%rsp), %ecx
movb $0x1, %al
cmpl 0x4f0(%rsp), %ecx
movb %al, 0x67(%rsp)
je 0x14edb
movl 0x4e8(%rsp), %eax
cmpl 0x4ec(%rsp), %eax
sete %al
movb %al, 0x67(%rsp)
movb 0x67(%rsp), %al
testb $0x1, %al
jne 0x14e85
callq 0xa6530
movsd %xmm0, 0x58(%rsp)
jmp 0x14ef0
movsd 0x58(%rsp), %xmm0
cvtsi2sdl 0x65c(%rsp), %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x4e4(%rsp)
callq 0xa6530
movsd %xmm0, 0x50(%rsp)
jmp 0x14f1b
movsd 0x50(%rsp), %xmm0
cvtsi2sdl 0x65c(%rsp), %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x4e0(%rsp)
movl 0x4e0(%rsp), %eax
cmpl 0x4e4(%rsp), %eax
je 0x14f0e
jmp 0x14f4b
callq 0xa6530
movsd %xmm0, 0x48(%rsp)
jmp 0x14f58
movsd 0x48(%rsp), %xmm0
cvtsi2sdl 0x65c(%rsp), %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x4dc(%rsp)
movl 0x4dc(%rsp), %ecx
movb $0x1, %al
cmpl 0x4e4(%rsp), %ecx
movb %al, 0x47(%rsp)
je 0x14fa1
movl 0x4dc(%rsp), %eax
cmpl 0x4e0(%rsp), %eax
sete %al
movb %al, 0x47(%rsp)
movb 0x47(%rsp), %al
testb $0x1, %al
jne 0x14f4b
cmpb $0x0, 0xb8ca8(%rip)     # 0xcdc58
jne 0x14ff7
leaq 0xb8c9f(%rip), %rdi     # 0xcdc58
callq 0xd6f0
cmpl $0x0, %eax
je 0x14ff7
leaq 0xb8c7e(%rip), %rdi     # 0xcdc48
callq 0x182f0
jmp 0x14fd1
leaq 0x3328(%rip), %rdi      # 0x18300
leaq 0xb8c69(%rip), %rsi     # 0xcdc48
leaq 0xb83f2(%rip), %rdx     # 0xcd3d8
callq 0xd350
leaq 0xb8c66(%rip), %rdi     # 0xcdc58
callq 0xd290
leaq 0xb8c4a(%rip), %rdi     # 0xcdc48
xorl %esi, %esi
callq 0xa3b50
jmp 0x15007
movq 0x678(%rsp), %rax
movslq 0x4f0(%rsp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rsi
leaq 0xb8c22(%rip), %rdi     # 0xcdc48
callq 0x18310
jmp 0x1502d
movq 0x678(%rsp), %rax
movslq 0x4ec(%rsp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rsi
leaq 0xb8bfc(%rip), %rdi     # 0xcdc48
callq 0x18310
jmp 0x15053
movq 0x678(%rsp), %rax
movslq 0x4e8(%rsp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rsi
leaq 0xb8bd6(%rip), %rdi     # 0xcdc48
callq 0x18310
jmp 0x15079
cmpb $0x0, 0xb8bf0(%rip)     # 0xcdc70
jne 0x150c7
leaq 0xb8be7(%rip), %rdi     # 0xcdc70
callq 0xd6f0
cmpl $0x0, %eax
je 0x150c7
leaq 0xb8bc6(%rip), %rdi     # 0xcdc60
callq 0x182f0
jmp 0x150a1
leaq 0x3258(%rip), %rdi      # 0x18300
leaq 0xb8bb1(%rip), %rsi     # 0xcdc60
leaq 0xb8322(%rip), %rdx     # 0xcd3d8
callq 0xd350
leaq 0xb8bae(%rip), %rdi     # 0xcdc70
callq 0xd290
leaq 0xb8b92(%rip), %rdi     # 0xcdc60
xorl %esi, %esi
callq 0xa3b50
jmp 0x150d7
movq 0x660(%rsp), %rax
movslq 0x4e4(%rsp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rsi
leaq 0xb8b6a(%rip), %rdi     # 0xcdc60
callq 0x18310
jmp 0x150fd
movq 0x660(%rsp), %rax
movslq 0x4e0(%rsp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rsi
leaq 0xb8b44(%rip), %rdi     # 0xcdc60
callq 0x18310
jmp 0x15123
movq 0x660(%rsp), %rax
movslq 0x4dc(%rsp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rsi
leaq 0xb8b1e(%rip), %rdi     # 0xcdc60
callq 0x18310
jmp 0x15149
movl 0x6b0(%rsp), %r8d
movl 0x6b8(%rsp), %r9d
movl 0x6c0(%rsp), %ecx
movq %rsp, %rax
movl %ecx, (%rax)
leaq 0xb8adc(%rip), %rsi     # 0xcdc48
leaq 0xb8aed(%rip), %rdx     # 0xcdc60
xorl %eax, %eax
movl %eax, %ecx
leaq 0x458(%rsp), %rdi
callq 0x34e00
jmp 0x15186
leaq 0x348(%rsp), %rdi
leaq 0x458(%rsp), %rsi
xorl %edx, %edx
callq 0x72510
jmp 0x1519f
leaq 0x238(%rsp), %rdi
leaq 0x348(%rsp), %rsi
callq 0x16010
jmp 0x151b6
movq 0x680(%rsp), %rdi
movq 0x678(%rsp), %rsi
movl 0x674(%rsp), %edx
movq 0x668(%rsp), %rcx
movq 0x660(%rsp), %r8
movl 0x65c(%rsp), %r9d
movq 0x630(%rsp), %r10
movq 0x618(%rsp), %r11
movl 0x63c(%rsp), %ebx
movq %rsp, %rax
movl %ebx, 0x20(%rax)
movq %r11, 0x18(%rax)
movq %r10, 0x10(%rax)
leaq 0x348(%rsp), %r10
movq %r10, 0x8(%rax)
leaq 0x238(%rsp), %r10
movq %r10, (%rax)
callq 0x15550
movl %eax, 0x40(%rsp)
jmp 0x1522c
movl 0x40(%rsp), %eax
movl %eax, 0x234(%rsp)
xorps %xmm0, %xmm0
movsd %xmm0, 0x228(%rsp)
movl $0x0, 0x224(%rsp)
movl 0x224(%rsp), %eax
cmpl 0x234(%rsp), %eax
jge 0x153dc
movq 0x630(%rsp), %rax
movslq 0x224(%rsp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rax
movq %rax, 0x218(%rsp)
movq 0x618(%rsp), %rax
movslq 0x224(%rsp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rsi
leaq 0x200(%rsp), %rdi
callq 0x7d390
jmp 0x152a9
leaq 0x200(%rsp), %rdi
leaq 0x348(%rsp), %rsi
callq 0x7dfc0
jmp 0x152c0
movq 0x218(%rsp), %rdi
leaq 0x200(%rsp), %rsi
callq 0x2d840
movsd %xmm0, 0x38(%rsp)
jmp 0x152dd
movsd 0x38(%rsp), %xmm0
movsd %xmm0, 0x1f8(%rsp)
movsd 0x610(%rsp), %xmm0
mulsd 0x1f8(%rsp), %xmm0
callq 0xd6d0
movsd %xmm0, 0x1f0(%rsp)
movsd 0x1f0(%rsp), %xmm0
addsd 0x228(%rsp), %xmm0
movsd %xmm0, 0x228(%rsp)
movl 0x224(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x224(%rsp)
jmp 0x1524e
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x628(%rsp)
movl %eax, 0x624(%rsp)
jmp 0x15522
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x628(%rsp)
movl %eax, 0x624(%rsp)
leaq 0xb88e7(%rip), %rdi     # 0xcdc58
callq 0xd280
jmp 0x15522
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x628(%rsp)
movl %eax, 0x624(%rsp)
leaq 0xb88da(%rip), %rdi     # 0xcdc70
callq 0xd280
jmp 0x15522
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x628(%rsp)
movl %eax, 0x624(%rsp)
jmp 0x15449
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x628(%rsp)
movl %eax, 0x624(%rsp)
leaq 0x238(%rsp), %rdi
callq 0x725b0
jmp 0x15449
movsd 0x228(%rsp), %xmm0
ucomisd 0x608(%rsp), %xmm0
jbe 0x15419
leaq 0x4f8(%rsp), %rdi
leaq 0x348(%rsp), %rsi
callq 0x16070
jmp 0x15407
movsd 0x228(%rsp), %xmm0
movsd %xmm0, 0x608(%rsp)
leaq 0x238(%rsp), %rdi
callq 0x725b0
leaq 0x348(%rsp), %rdi
callq 0x725b0
movl 0x4f4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x4f4(%rsp)
jmp 0x14e09
leaq 0x348(%rsp), %rdi
callq 0x725b0
jmp 0x15522
movq 0x6a8(%rsp), %rdi
leaq 0x4f8(%rsp), %rsi
callq 0x16070
jmp 0x15472
leaq 0xe0(%rsp), %rdi
leaq 0x4f8(%rsp), %rsi
callq 0x16010
jmp 0x15489
movq 0x6a0(%rsp), %rdi
leaq 0xe0(%rsp), %rsi
callq 0x16070
jmp 0x154a0
leaq 0xe0(%rsp), %rdi
callq 0x725b0
movq 0x630(%rsp), %rax
movq %rax, 0x30(%rsp)
cmpq $0x0, %rax
je 0x154ca
movq 0x30(%rsp), %rdi
callq 0xd530
movq 0x618(%rsp), %rax
movq %rax, 0x28(%rsp)
cmpq $0x0, %rax
je 0x154e7
movq 0x28(%rsp), %rdi
callq 0xd530
movl $0x1, 0x68c(%rsp)
leaq 0x4f8(%rsp), %rdi
callq 0x725b0
jmp 0x15531
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x628(%rsp)
movl %eax, 0x624(%rsp)
leaq 0xe0(%rsp), %rdi
callq 0x725b0
leaq 0x4f8(%rsp), %rdi
callq 0x725b0
jmp 0x15541
movl 0x68c(%rsp), %eax
addq $0x690, %rsp            # imm = 0x690
popq %rbx
retq
movq 0x628(%rsp), %rdi
callq 0xd6a0
nop