// File: /workspace/github_c_makefile_star2_O2/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 $0x198, %rsp            # imm = 0x198
movq %rsi, %rbx
leaq 0x1d0(%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
movq %rdi, %r14
leaq 0x40(%rsp), %r15
movaps (%rcx), %xmm1
movss 0x14(%rcx), %xmm2
shufps $0xf4, %xmm1, %xmm2     # xmm2 = xmm2[0,1],xmm1[3,3]
shufps $0xf2, %xmm1, %xmm2     # xmm2 = xmm2[2,0],xmm1[3,3]
cvtps2pd %xmm2, %xmm1
movaps %xmm0, (%r15)
movapd %xmm1, 0x10(%r15)
cvtps2pd (%rcx), %xmm0
leaq 0xe0(%rsp), %rax
movaps %xmm0, (%rax)
movq 0x10(%rcx), %rax
movq %rax, 0x10(%rsi)
movaps (%rcx), %xmm0
movups %xmm0, (%rsi)
leaq 0x104(%rdi), %r12
leaq 0x1b48(%rdi), %rax
movq %rax, 0x28(%rsp)
leaq 0x358c(%rdi), %rax
movq %rax, 0x20(%rsp)
xorl %ebp, %ebp
pushq $0x29
popq %r13
leaq 0x80(%rsp), %rdi
leaq 0xf0(%rsp), %rsi
leaq 0x170(%rsp), %rdx
movq %r15, %rcx
callq 0x9690
movsd 0x80(%rsp), %xmm2
movsd 0x98(%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
ucomisd 0x1fd7a(%rip), %xmm0    # 0x2d240
ja 0xd80e
movapd %xmm2, %xmm0
minsd %xmm3, %xmm0
movapd %xmm0, %xmm1
testq %rbp, %rbp
movsd (%rsp), %xmm4
je 0xd4e6
movapd %xmm4, %xmm1
movsd %xmm1, 0x10(%rsp)
movsd 0x1fd44(%rip), %xmm1    # 0x2d238
je 0xd4fe
divsd %xmm0, %xmm4
movapd %xmm4, %xmm1
mulsd %xmm1, %xmm2
movsd %xmm2, 0x80(%rsp)
mulsd %xmm1, %xmm3
movapd %xmm2, %xmm0
unpcklpd %xmm2, %xmm0            # xmm0 = xmm0[0],xmm2[0]
mulpd 0xf0(%rsp), %xmm0
movapd %xmm0, 0x40(%rsp)
movapd %xmm3, %xmm1
unpcklpd %xmm3, %xmm1            # xmm1 = xmm1[0],xmm3[0]
mulpd 0x100(%rsp), %xmm1
movapd %xmm1, 0x50(%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
movsd %xmm3, 0x98(%rsp)
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
movupd %xmm1, 0x8(%rbx)
cmpq $0xe, %rbp
je 0xd80e
subq $0x8, %rsp
movq %r14, %rdi
leaq 0x40(%rsp), %rsi
leaq 0x38(%rsp), %rdx
movq %r12, %rcx
pushq $0x14
popq %r8
movsd 0x1fc6c(%rip), %xmm0    # 0x2d1f8
movsd 0x1fca4(%rip), %xmm1    # 0x2d238
movq %r15, %r9
leaq 0xe8(%rsp), %rax
pushq %rax
callq 0xc952
addq $0x10, %rsp
testl %eax, %eax
jne 0xd966
cmpl $0x0, 0x100(%r14)
je 0xd61e
movsd 0x38(%rsp), %xmm0
mulsd %xmm0, %xmm0
movsd 0x1fc6b(%rip), %xmm2    # 0x2d238
movapd %xmm2, %xmm1
subsd %xmm0, %xmm1
xorpd %xmm3, %xmm3
maxsd %xmm3, %xmm1
sqrtsd %xmm1, %xmm0
movsd 0x30(%rsp), %xmm1
mulsd %xmm1, %xmm1
subsd %xmm1, %xmm2
maxsd %xmm3, %xmm2
sqrtsd %xmm2, %xmm1
movsd 0x1fc49(%rip), %xmm2    # 0x2d248
divsd %xmm2, %xmm0
divsd %xmm2, %xmm1
movq %r12, %rdi
movq %r13, %rsi
movq %r12, %rdx
movq %r13, %rcx
movq %r13, %r8
movq %r13, %r9
callq 0x97d0
incq %rbp
movq 0x28(%rsp), %rdi
movq 0x20(%rsp), %rsi
pushq $0x1
popq %rdx
movq %r13, %rcx
movq %r12, %r8
movq %r13, %r9
pushq %r13
pushq %r13
callq 0x9170
popq %rax
popq %rcx
xorpd %xmm1, %xmm1
xorpd %xmm0, %xmm0
movq $-0x1a44, %rax          # imm = 0xE5BC
testq %rax, %rax
je 0xd6a8
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
jmp 0xd651
movapd %xmm1, 0xc0(%rsp)
movhpd %xmm1, 0xd0(%rsp)
movsd %xmm0, 0xd8(%rsp)
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm1
xorpd %xmm1, %xmm1
jne 0xd6d7
jnp 0xd7f6
ucomisd %xmm1, %xmm0
jne 0xd6e3
jnp 0xd7f6
leaq 0xa0(%rsp), %rdi
leaq 0x60(%rsp), %rsi
leaq 0x150(%rsp), %rdx
leaq 0xc0(%rsp), %rcx
callq 0x9690
movsd 0xa0(%rsp), %xmm0
movsd 0xb8(%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 0x1fa70(%rip), %xmm1    # 0x2d1a0
movmskpd %xmm1, %eax
cmpl $0x3, %eax
je 0xd80e
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0xd74d
sqrtsd %xmm0, %xmm6
jmp 0xd75a
callq 0x9a00
xorpd %xmm1, %xmm1
movapd %xmm0, %xmm6
movsd 0xb8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jb 0xd76f
sqrtsd %xmm0, %xmm0
jmp 0xd77e
movapd %xmm6, (%rsp)
callq 0x9a00
movapd (%rsp), %xmm6
movapd 0x60(%rsp), %xmm2
movupd 0x68(%rsp), %xmm3
movapd 0x70(%rsp), %xmm1
movapd 0x40(%rsp), %xmm4
movapd 0x50(%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, 0x40(%rsp)
movsd 0x78(%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, 0x50(%rsp)
movsd 0x10(%rsp), %xmm0
movsd %xmm0, (%rsp)
jmp 0xd46c
leaq 0x1d0(%rsp), %rcx
movq 0x10(%rcx), %rax
movq %rax, 0x10(%rbx)
movupd (%rcx), %xmm0
movupd %xmm0, (%rbx)
movss 0x8(%rbx), %xmm0
movss 0x10(%rbx), %xmm1
unpcklps %xmm1, %xmm0            # xmm0 = xmm0[0],xmm1[0],xmm0[1],xmm1[1]
cvtps2pd %xmm0, %xmm0
leaq 0x130(%rsp), %r15
movaps %xmm0, (%r15)
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, 0x10(%r15)
xorl %eax, %eax
cmpl $0x0, 0x78(%r14)
sete %al
movd %eax, %xmm0
pshufd $0x44, %xmm0, %xmm0     # xmm0 = xmm0[0,1,0,1]
psllq $0x3f, %xmm0
psrad $0x1f, %xmm0
pshufd $0xf5, %xmm0, %xmm1     # xmm1 = xmm0[1,1,3,3]
xorps %xmm2, %xmm2
movhps 0x1f9cd(%rip), %xmm2    # xmm2 = xmm2[0,1],mem[0,1]
andps %xmm0, %xmm2
movq 0x1f9c2(%rip), %xmm0    # 0x2d238
pandn %xmm0, %xmm1
por %xmm2, %xmm1
leaq 0x120(%rsp), %r14
movdqa %xmm1, (%r14)
leaq 0x110(%rsp), %r12
movq %r12, %rdi
movq %r15, %rsi
movq %r14, %rdx
callq 0x9950
movsd (%r14), %xmm1
movsd 0x8(%r14), %xmm0
callq 0x9730
movsd %xmm0, 0x10(%rsp)
movsd (%r12), %xmm1
movsd 0x8(%r12), %xmm0
callq 0x9730
subsd 0x10(%rsp), %xmm0
movsd %xmm0, (%rsp)
callq 0x9470
movapd %xmm0, 0x10(%rsp)
movsd (%rsp), %xmm0
callq 0x98d0
movapd 0x1f7af(%rip), %xmm1    # 0x2d0a0
movsd (%r15), %xmm2
movsd 0x8(%r15), %xmm3
movapd %xmm2, %xmm4
xorpd %xmm1, %xmm4
xorpd %xmm3, %xmm1
movapd 0x10(%r15), %xmm5
movapd %xmm5, %xmm6
unpcklpd %xmm5, %xmm6            # xmm6 = xmm6[0],xmm5[0]
unpckhpd %xmm5, %xmm5            # xmm5 = xmm5[1,1]
movapd %xmm0, %xmm7
movapd 0x10(%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 $0x198, %rsp            # imm = 0x198
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq