// File: /workspace/github_c_makefile_star2_O1/zpole[P]IICV/vl/covdet.c
// Function: vl_covdet_extract_affine_shape_for_frame
int
vl_covdet_extract_affine_shape_for_frame (VlCovDet * self,
                                          VlFrameOrientedEllipse * adapted,
                                          VlFrameOrientedEllipse frame)
{
  vl_index iter = 0 ;

  double A [2*2] = {frame.a11, frame.a21, frame.a12, frame.a22} ;
  double T [2] = {frame.x, frame.y} ;
  double U [2*2] ;
  double V [2*2] ;
  double D [2*2] ;
  double M [2*2] ;
  double P [2*2] ;
  double P_ [2*2] ;
  double Q [2*2] ;
  double sigma1, sigma2 ;
  double sigmaD = VL_COVDET_AA_RELATIVE_DERIVATIVE_SIGMA ;
  double factor ;
  double anisotropy ;
  double referenceScale ;
  vl_size const resolution = VL_COVDET_AA_PATCH_RESOLUTION ;
  vl_size const side = 2*VL_COVDET_AA_PATCH_RESOLUTION + 1 ;
  double const extent = VL_COVDET_AA_PATCH_EXTENT ;


  *adapted = frame ;

  while (1) {
    double lxx = 0, lxy = 0, lyy = 0 ;
    vl_index k ;
    int err ;

    /* A = U D V' */
    vl_svd2(D, U, V, A) ;
    anisotropy = VL_MAX(D[0]/D[3], D[3]/D[0]) ;

    /* VL_PRINTF("anisot: %g\n", anisotropy); */

    if (anisotropy > VL_COVDET_AA_MAX_ANISOTROPY) {
      /* diverged, give up with current solution */
      break ;
    }

    /* make sure that the smallest singluar value stays fixed
       after the first iteration */
    if (iter == 0) {
      referenceScale = VL_MIN(D[0], D[3]) ;
      factor = 1.0 ;
    } else {
      factor = referenceScale / VL_MIN(D[0],D[3]) ;
    }

    D[0] *= factor ;
    D[3] *= factor ;

    A[0] = U[0] * D[0] ;
    A[1] = U[1] * D[0] ;
    A[2] = U[2] * D[3] ;
    A[3] = U[3] * D[3] ;

    adapted->a11 = A[0] ;
    adapted->a21 = A[1] ;
    adapted->a12 = A[2] ;
    adapted->a22 = A[3] ;

    if (++iter >= VL_COVDET_AA_MAX_NUM_ITERATIONS) break ;

    err = vl_covdet_extract_patch_helper(self,
                                         &sigma1, &sigma2,
                                         self->aaPatch,
                                         resolution,
                                         extent,
                                         sigmaD,
                                         A, T, D[0], D[3]) ;
    if (err) return err ;

    if (self->aaAccurateSmoothing ) {
      double deltaSigma1 = sqrt(VL_MAX(sigmaD*sigmaD - sigma1*sigma1,0)) ;
      double deltaSigma2 = sqrt(VL_MAX(sigmaD*sigmaD - sigma2*sigma2,0)) ;
      double stephat = extent / resolution ;
      vl_imsmooth_f(self->aaPatch, side,
                    self->aaPatch, side, side, side,
                    deltaSigma1 / stephat, deltaSigma2 / stephat) ;
    }

    /* compute second moment matrix */
    vl_imgradient_f (self->aaPatchX, self->aaPatchY, 1, side,
                     self->aaPatch, side, side, side) ;

    for (k = 0 ; k < (signed)(side*side) ; ++k) {
      double lx = self->aaPatchX[k] ;
      double ly = self->aaPatchY[k] ;
      lxx += lx * lx * self->aaMask[k] ;
      lyy += ly * ly * self->aaMask[k] ;
      lxy += lx * ly * self->aaMask[k] ;
    }
    M[0] = lxx ;
    M[1] = lxy ;
    M[2] = lxy ;
    M[3] = lyy ;

    if (lxx == 0 || lyy == 0) {
      *adapted = frame ;
      break ;
    }

    /* decompose M = P * Q * P' */
    vl_svd2 (Q, P, P_, M) ;

    /*
     Setting A <- A * dA results in M to change approximatively as

     M --> dA'  M dA = dA' P Q P dA

     To make this proportional to the identity, we set

     dA ~= P Q^1/2

     we also make it so the smallest singular value of A is unchanged.
     */

    if (Q[3]/Q[0] < VL_COVDET_AA_CONVERGENCE_THRESHOLD &&
        Q[0]/Q[3] < VL_COVDET_AA_CONVERGENCE_THRESHOLD) {
      break ;
    }

    {
      double Ap [4] ;
      double q0 = sqrt(Q[0]) ;
      double q1 = sqrt(Q[3]) ;
      Ap[0] = (A[0] * P[0] + A[2] * P[1]) / q0 ;
      Ap[1] = (A[1] * P[0] + A[3] * P[1]) / q0 ;
      Ap[2] = (A[0] * P[2] + A[2] * P[3]) / q1 ;
      Ap[3] = (A[1] * P[2] + A[3] * P[3]) / q1 ;
      memcpy(A,Ap,4*sizeof(double)) ;
    }

  } /* next iteration */

  /*
   Make upright.

   Shape adaptation does not estimate rotation. This is fixed by default
   so that a selected axis is not rotated at all (usually this is the
   vertical axis for upright features). To do so, the frame is rotated
   as follows.
   */
  {
    double A [2*2] = {adapted->a11, adapted->a21, adapted->a12, adapted->a22} ;
    double ref [2] ;
    double ref_ [2] ;
    double angle ;
    double angle_ ;
    double dangle ;
    double r1, r2 ;

    if (self->transposed) {
      /* up is the x axis */
      ref[0] = 1 ;
      ref[1] = 0 ;
    } else {
      /* up is the y axis */
      ref[0] = 0 ;
      ref[1] = 1 ;
    }

    vl_solve_linear_system_2 (ref_, A, ref) ;
    angle = atan2(ref[1], ref[0]) ;
    angle_ = atan2(ref_[1], ref_[0]) ;
    dangle = angle_ - angle ;
    r1 = cos(dangle) ;
    r2 = sin(dangle) ;
    adapted->a11 = + A[0] * r1 + A[2] * r2 ;
    adapted->a21 = + A[1] * r1 + A[3] * r2 ;
    adapted->a12 = - A[0] * r2 + A[2] * r1 ;
    adapted->a22 = - A[1] * r2 + A[3] * r1 ;
  }

  return VL_ERR_OK ;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1a8, %rsp            # imm = 0x1A8
movq %rsi, %rbx
movq %rdi, %r14
leaq 0x1e0(%rsp), %rcx
movups 0x8(%rcx), %xmm0
movss 0x10(%rcx), %xmm1
movlhps %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
shufps $0xe2, %xmm0, %xmm1     # xmm1 = xmm1[2,0],xmm0[2,3]
cvtps2pd %xmm1, %xmm0
leaq 0x50(%rsp), %r15
movaps %xmm0, (%r15)
movaps (%rcx), %xmm0
movss 0x14(%rcx), %xmm1
shufps $0xf4, %xmm0, %xmm1     # xmm1 = xmm1[0,1],xmm0[3,3]
shufps $0xf2, %xmm0, %xmm1     # xmm1 = xmm1[2,0],xmm0[3,3]
cvtps2pd %xmm1, %xmm0
movaps %xmm0, 0x10(%r15)
cvtps2pd (%rcx), %xmm0
leaq 0xf0(%rsp), %rax
movaps %xmm0, (%rax)
movq 0x10(%rcx), %rax
movq %rax, 0x10(%rsi)
movaps (%rcx), %xmm0
movups %xmm0, (%rsi)
leaq 0x104(%rdi), %rax
movq %rax, 0x10(%rsp)
leaq 0x1b48(%rdi), %rax
movq %rax, 0x38(%rsp)
leaq 0x358c(%rdi), %rax
movq %rax, 0x30(%rsp)
xorl %r13d, %r13d
leaq 0x90(%rsp), %rdi
leaq 0x120(%rsp), %rsi
leaq 0x180(%rsp), %rdx
movq %r15, %rcx
callq 0x9650
movsd 0x90(%rsp), %xmm2
movsd 0xa8(%rsp), %xmm3
movapd %xmm2, %xmm0
unpcklpd %xmm3, %xmm0            # xmm0 = xmm0[0],xmm3[0]
movapd %xmm3, %xmm1
unpcklpd %xmm2, %xmm1            # xmm1 = xmm1[0],xmm2[0]
divpd %xmm1, %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
maxsd %xmm1, %xmm0
movl $0x3, %ebp
ucomisd 0x255e5(%rip), %xmm0    # 0x33268
jbe 0xdc92
movsd (%rsp), %xmm5
movq %r13, %r12
jmp 0xe036
movapd %xmm2, %xmm0
minsd %xmm3, %xmm0
movapd %xmm0, %xmm5
testq %r13, %r13
movsd (%rsp), %xmm4
je 0xdcac
movapd %xmm4, %xmm5
movsd 0x255ac(%rip), %xmm1    # 0x33260
je 0xdcbe
divsd %xmm0, %xmm4
movapd %xmm4, %xmm1
mulsd %xmm1, %xmm2
movsd %xmm2, 0x90(%rsp)
mulsd %xmm1, %xmm3
movsd %xmm3, 0xa8(%rsp)
movapd %xmm2, %xmm0
unpcklpd %xmm2, %xmm0            # xmm0 = xmm0[0],xmm2[0]
mulpd 0x120(%rsp), %xmm0
movapd %xmm0, 0x50(%rsp)
movapd %xmm3, %xmm1
unpcklpd %xmm3, %xmm1            # xmm1 = xmm1[0],xmm3[0]
mulpd 0x130(%rsp), %xmm1
movapd %xmm1, 0x60(%rsp)
movapd %xmm0, %xmm4
unpcklpd %xmm1, %xmm4            # xmm4 = xmm4[0],xmm1[0]
movhlps %xmm0, %xmm1            # xmm1 = xmm0[1],xmm1[1]
cvtpd2ps %xmm1, %xmm0
cvtpd2ps %xmm4, %xmm1
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
movupd %xmm1, 0x8(%rbx)
leaq 0x1(%r13), %r12
cmpq $0xd, %r13
jg 0xe036
movsd %xmm5, (%rsp)
subq $0x8, %rsp
movl $0x14, %r8d
movq %r14, %rdi
leaq 0x50(%rsp), %rsi
leaq 0x48(%rsp), %rdx
movq 0x18(%rsp), %rcx
movsd 0x254c7(%rip), %xmm0    # 0x33220
movsd 0x254ff(%rip), %xmm1    # 0x33260
movq %r15, %r9
leaq 0xf8(%rsp), %rax
pushq %rax
callq 0xd08b
addq $0x10, %rsp
testl %eax, %eax
je 0xdd88
movl $0x1, %ebp
movl %eax, 0x1c(%rsp)
jmp 0xe031
cmpl $0x0, 0x100(%r14)
je 0xde45
movsd 0x48(%rsp), %xmm1
mulsd %xmm1, %xmm1
movsd 0x254b8(%rip), %xmm0    # 0x33260
subsd %xmm1, %xmm0
xorpd %xmm2, %xmm2
maxsd %xmm2, %xmm0
ucomisd %xmm2, %xmm0
jb 0xddc3
xorps %xmm3, %xmm3
sqrtsd %xmm0, %xmm3
jmp 0xddd0
callq 0x9990
xorpd %xmm2, %xmm2
movapd %xmm0, %xmm3
movsd 0x40(%rsp), %xmm1
mulsd %xmm1, %xmm1
movsd 0x2547e(%rip), %xmm0    # 0x33260
subsd %xmm1, %xmm0
maxsd %xmm2, %xmm0
ucomisd %xmm2, %xmm0
jb 0xddf9
xorps %xmm1, %xmm1
sqrtsd %xmm0, %xmm1
jmp 0xde0e
movsd %xmm3, 0x20(%rsp)
callq 0x9990
movsd 0x20(%rsp), %xmm3
movapd %xmm0, %xmm1
movsd 0x2545a(%rip), %xmm0    # 0x33270
divsd %xmm0, %xmm3
divsd %xmm0, %xmm1
movl $0x29, %esi
movl $0x29, %ecx
movl $0x29, %r8d
movl $0x29, %r9d
movq 0x10(%rsp), %rdi
movq %rdi, %rdx
movapd %xmm3, %xmm0
callq 0x9770
movl $0x1, %edx
movl $0x29, %ecx
movl $0x29, %r9d
movq 0x38(%rsp), %rdi
movq 0x30(%rsp), %rsi
movq 0x10(%rsp), %r8
movl $0x29, %eax
pushq %rax
pushq %rax
callq 0x9170
addq $0x10, %rsp
xorpd %xmm1, %xmm1
xorpd %xmm0, %xmm0
movq $-0x1a44, %rax          # imm = 0xE5BC
xorps %xmm2, %xmm2
cvtss2sd 0x6a14(%r14,%rax), %xmm2
movss 0x4fd0(%r14,%rax), %xmm3
movss 0x358c(%r14,%rax), %xmm4
unpcklps %xmm3, %xmm4            # xmm4 = xmm4[0],xmm3[0],xmm4[1],xmm3[1]
cvtps2pd %xmm4, %xmm3
movapd %xmm3, %xmm4
unpcklpd %xmm3, %xmm4            # xmm4 = xmm4[0],xmm3[0]
mulpd %xmm3, %xmm4
mulpd %xmm3, %xmm3
unpckhpd %xmm3, %xmm3            # xmm3 = xmm3[1,1]
mulsd %xmm2, %xmm3
addsd %xmm3, %xmm0
unpcklpd %xmm2, %xmm2            # xmm2 = xmm2[0,0]
mulpd %xmm4, %xmm2
addpd %xmm2, %xmm1
addq $0x4, %rax
jne 0xde83
movapd %xmm1, 0xd0(%rsp)
movhpd %xmm1, 0xe0(%rsp)
movsd %xmm0, 0xe8(%rsp)
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm1
jne 0xdeff
jnp 0xdf71
ucomisd %xmm2, %xmm0
jne 0xdf07
jnp 0xdf71
leaq 0xb0(%rsp), %rdi
leaq 0x70(%rsp), %rsi
leaq 0x160(%rsp), %rdx
leaq 0xd0(%rsp), %rcx
callq 0x9650
movsd 0xb0(%rsp), %xmm0
movsd 0xc8(%rsp), %xmm1
movapd %xmm0, %xmm2
unpcklpd %xmm1, %xmm2            # xmm2 = xmm2[0],xmm1[0]
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
divpd %xmm2, %xmm1
cmpltpd 0x2526c(%rip), %xmm1    # 0x331c0
movmskpd %xmm1, %eax
cmpl $0x3, %eax
je 0xe031
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0xdf8e
sqrtsd %xmm0, %xmm6
jmp 0xdf9b
leaq 0x1e0(%rsp), %rcx
movq 0x10(%rcx), %rax
movq %rax, 0x10(%rbx)
movupd (%rcx), %xmm0
movupd %xmm0, (%rbx)
jmp 0xe031
callq 0x9990
xorpd %xmm1, %xmm1
movapd %xmm0, %xmm6
movsd 0xc8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jb 0xdfb0
sqrtsd %xmm0, %xmm0
jmp 0xdfc1
movapd %xmm6, 0x20(%rsp)
callq 0x9990
movapd 0x20(%rsp), %xmm6
movapd 0x70(%rsp), %xmm2
movupd 0x78(%rsp), %xmm3
movapd 0x80(%rsp), %xmm1
movapd 0x50(%rsp), %xmm4
movapd 0x60(%rsp), %xmm5
unpcklpd %xmm3, %xmm3            # xmm3 = xmm3[0,0]
mulpd %xmm5, %xmm3
unpcklpd %xmm2, %xmm2            # xmm2 = xmm2[0,0]
mulpd %xmm4, %xmm2
addpd %xmm3, %xmm2
unpcklpd %xmm6, %xmm6            # xmm6 = xmm6[0,0]
divpd %xmm6, %xmm2
movapd %xmm2, 0x50(%rsp)
movsd 0x88(%rsp), %xmm2
unpcklpd %xmm2, %xmm2            # xmm2 = xmm2[0,0]
mulpd %xmm5, %xmm2
unpcklpd %xmm1, %xmm1            # xmm1 = xmm1[0,0]
mulpd %xmm4, %xmm1
addpd %xmm2, %xmm1
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
divpd %xmm0, %xmm1
movapd %xmm1, 0x60(%rsp)
xorl %ebp, %ebp
movsd (%rsp), %xmm5
movsd %xmm5, (%rsp)
movq %r12, %r13
testl %ebp, %ebp
je 0xdc24
cmpl $0x3, %ebp
jne 0xe092
movss 0x8(%rbx), %xmm0
movss 0x10(%rbx), %xmm1
unpcklps %xmm1, %xmm0            # xmm0 = xmm0[0],xmm1[0],xmm0[1],xmm1[1]
cvtps2pd %xmm0, %xmm0
movaps %xmm0, 0x100(%rsp)
movss 0xc(%rbx), %xmm0
movss 0x14(%rbx), %xmm1
unpcklps %xmm1, %xmm0            # xmm0 = xmm0[0],xmm1[0],xmm0[1],xmm1[1]
cvtps2pd %xmm0, %xmm0
movaps %xmm0, 0x110(%rsp)
cmpl $0x0, 0x78(%r14)
xorps %xmm0, %xmm0
movsd 0x251d5(%rip), %xmm2    # 0x33260
je 0xe09b
movaps %xmm2, %xmm1
jmp 0xe0a1
movl 0x1c(%rsp), %eax
jmp 0xe195
xorps %xmm1, %xmm1
movaps %xmm2, %xmm0
leaq 0x150(%rsp), %r15
movsd %xmm1, (%r15)
movsd %xmm0, 0x8(%r15)
leaq 0x140(%rsp), %r12
leaq 0x100(%rsp), %r14
movq %r12, %rdi
movq %r14, %rsi
movq %r15, %rdx
callq 0x98e0
movsd (%r15), %xmm1
movsd 0x8(%r15), %xmm0
callq 0x96e0
movsd %xmm0, (%rsp)
movsd (%r12), %xmm1
movsd 0x8(%r12), %xmm0
callq 0x96e0
subsd (%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
callq 0x9450
movapd %xmm0, (%rsp)
movsd 0x10(%rsp), %xmm0
callq 0x9860
movapd 0x24faf(%rip), %xmm1    # 0x330d0
movsd (%r14), %xmm2
movsd 0x8(%r14), %xmm3
movapd %xmm2, %xmm4
xorpd %xmm1, %xmm4
xorpd %xmm3, %xmm1
movapd 0x10(%r14), %xmm5
movapd %xmm5, %xmm6
unpcklpd %xmm5, %xmm6            # xmm6 = xmm6[0],xmm5[0]
unpckhpd %xmm5, %xmm5            # xmm5 = xmm5[1,1]
movapd %xmm0, %xmm7
movapd (%rsp), %xmm8
unpcklpd %xmm8, %xmm7            # xmm7 = xmm7[0],xmm8[0]
mulpd %xmm7, %xmm5
mulpd %xmm6, %xmm7
unpcklpd %xmm1, %xmm3            # xmm3 = xmm3[0],xmm1[0]
unpcklpd %xmm4, %xmm2            # xmm2 = xmm2[0],xmm4[0]
movapd %xmm8, %xmm1
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
mulpd %xmm1, %xmm2
addpd %xmm7, %xmm2
mulpd %xmm3, %xmm1
addpd %xmm5, %xmm1
cvtpd2ps %xmm1, %xmm0
cvtpd2ps %xmm2, %xmm1
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
movupd %xmm1, 0x8(%rbx)
xorl %eax, %eax
addq $0x1a8, %rsp            # imm = 0x1A8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq