// File: /workspace/github_c_makefile_star2_O3/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 $0x188, %rsp            # imm = 0x188
movq %rsi, %rbx
movq %rdi, %r14
movups 0x1c8(%rsp), %xmm0
movss 0x1d0(%rsp), %xmm1
movlhps %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
shufps $0xe2, %xmm0, %xmm1     # xmm1 = xmm1[2,0],xmm0[2,3]
cvtps2pd %xmm1, %xmm0
leaq 0x10(%rsp), %rcx
movaps %xmm0, (%rcx)
movaps 0x1c0(%rsp), %xmm0
movss 0x1d4(%rsp), %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(%rcx)
cvtps2pd 0x1c0(%rsp), %xmm0
movaps %xmm0, 0xf0(%rsp)
movq 0x1d0(%rsp), %rax
movq %rax, 0x10(%rsi)
movaps 0x1c0(%rsp), %xmm0
movups %xmm0, (%rsi)
leaq 0x50(%rsp), %r15
leaq 0xd0(%rsp), %rsi
leaq 0x120(%rsp), %rdx
movq %r15, %rdi
callq 0x9650
movsd (%r15), %xmm2
movsd 0x18(%r15), %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 0x24578(%rip), %xmm0    # 0x32278
jbe 0xde71
movss 0x8(%rbx), %xmm0
movss 0x10(%rbx), %xmm1
unpcklps %xmm1, %xmm0            # xmm0 = xmm0[0],xmm1[0],xmm0[1],xmm1[1]
cvtps2pd %xmm0, %xmm0
leaq 0x140(%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]
xorpd %xmm2, %xmm2
movhpd 0x2450b(%rip), %xmm2    # xmm2 = xmm2[0],mem[0]
andpd %xmm0, %xmm2
movq 0x244ff(%rip), %xmm0    # 0x32270
pandn %xmm0, %xmm1
por %xmm2, %xmm1
leaq 0x110(%rsp), %r14
movdqa %xmm1, (%r14)
leaq 0x100(%rsp), %r12
movq %r12, %rdi
movq %r15, %rsi
movq %r14, %rdx
callq 0x98e0
movsd (%r14), %xmm1
movsd 0x8(%r14), %xmm0
callq 0x96e0
movsd %xmm0, (%rsp)
movsd (%r12), %xmm1
movsd 0x8(%r12), %xmm0
callq 0x96e0
subsd (%rsp), %xmm0
movsd %xmm0, 0x30(%rsp)
callq 0x9450
movapd %xmm0, (%rsp)
movsd 0x30(%rsp), %xmm0
callq 0x9860
movapd 0x242e5(%rip), %xmm1    # 0x320d0
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 (%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 $0x188, %rsp            # imm = 0x188
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
leaq 0x104(%r14), %r15
leaq 0x1b48(%r14), %r12
leaq 0x358c(%r14), %r13
xorl %ebp, %ebp
movapd %xmm2, %xmm1
minsd %xmm3, %xmm1
movapd %xmm1, %xmm4
testq %rbp, %rbp
je 0xde9d
movapd %xmm0, %xmm4
movsd %xmm4, (%rsp)
movsd 0x243c6(%rip), %xmm4    # 0x32270
je 0xdeb4
divsd %xmm1, %xmm0
movapd %xmm0, %xmm4
mulsd %xmm4, %xmm2
movsd %xmm2, 0x50(%rsp)
mulsd %xmm4, %xmm3
movapd %xmm2, %xmm0
unpcklpd %xmm2, %xmm0            # xmm0 = xmm0[0],xmm2[0]
mulpd 0xd0(%rsp), %xmm0
movapd %xmm0, 0x10(%rsp)
movapd %xmm3, %xmm1
unpcklpd %xmm3, %xmm1            # xmm1 = xmm1[0],xmm3[0]
mulpd 0xe0(%rsp), %xmm1
movapd %xmm1, 0x20(%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, 0x68(%rsp)
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
movupd %xmm1, 0x8(%rbx)
cmpq $0xe, %rbp
je 0xdd06
subq $0x8, %rsp
movl $0x14, %r8d
movq %r14, %rdi
leaq 0x50(%rsp), %rsi
leaq 0x48(%rsp), %rdx
movq %r15, %rcx
movsd 0x242f2(%rip), %xmm0    # 0x32230
movsd 0x2432a(%rip), %xmm1    # 0x32270
leaq 0x18(%rsp), %r9
leaq 0xf8(%rsp), %rax
pushq %rax
callq 0xd0fe
addq $0x10, %rsp
testl %eax, %eax
jne 0xde5f
cmpl $0x0, 0x100(%r14)
je 0xdfe2
movsd 0x48(%rsp), %xmm0
mulsd %xmm0, %xmm0
movsd 0x242ef(%rip), %xmm2    # 0x32270
movapd %xmm2, %xmm1
subsd %xmm0, %xmm1
xorpd %xmm3, %xmm3
maxsd %xmm3, %xmm1
xorps %xmm0, %xmm0
sqrtsd %xmm1, %xmm0
movsd 0x40(%rsp), %xmm1
mulsd %xmm1, %xmm1
subsd %xmm1, %xmm2
maxsd %xmm3, %xmm2
xorps %xmm1, %xmm1
sqrtsd %xmm2, %xmm1
movsd 0x242c7(%rip), %xmm2    # 0x32280
divsd %xmm2, %xmm0
divsd %xmm2, %xmm1
movl $0x29, %esi
movl $0x29, %ecx
movl $0x29, %r8d
movl $0x29, %r9d
movq %r15, %rdi
movq %r15, %rdx
callq 0x9770
incq %rbp
movl $0x1, %edx
movl $0x29, %ecx
movl $0x29, %r9d
movq %r12, %rdi
movq %r13, %rsi
movq %r15, %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 0xe01d
movapd %xmm1, 0xb0(%rsp)
movhpd %xmm1, 0xc0(%rsp)
movsd %xmm0, 0xc8(%rsp)
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm1
xorpd %xmm1, %xmm1
jne 0xe0a1
jnp 0xe21b
ucomisd %xmm1, %xmm0
jne 0xe0ad
jnp 0xe21b
leaq 0x90(%rsp), %rdi
leaq 0x70(%rsp), %rsi
leaq 0x160(%rsp), %rdx
leaq 0xb0(%rsp), %rcx
callq 0x9650
movsd 0x90(%rsp), %xmm0
movsd 0xa8(%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 0x240d6(%rip), %xmm1    # 0x321d0
movmskpd %xmm1, %eax
cmpl $0x3, %eax
je 0xdd06
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0xe117
sqrtsd %xmm0, %xmm6
jmp 0xe124
callq 0x9990
xorpd %xmm1, %xmm1
movapd %xmm0, %xmm6
movsd 0xa8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jb 0xe139
sqrtsd %xmm0, %xmm0
jmp 0xe14a
movapd %xmm6, 0x30(%rsp)
callq 0x9990
movapd 0x30(%rsp), %xmm6
movapd 0x70(%rsp), %xmm2
movupd 0x78(%rsp), %xmm3
movapd 0x80(%rsp), %xmm1
movapd 0x10(%rsp), %xmm4
movapd 0x20(%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, 0x10(%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, 0x20(%rsp)
leaq 0x50(%rsp), %rdi
leaq 0xd0(%rsp), %rsi
leaq 0x120(%rsp), %rdx
leaq 0x10(%rsp), %rcx
callq 0x9650
movsd 0x50(%rsp), %xmm2
movsd 0x68(%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 0x2406d(%rip), %xmm0    # 0x32278
movsd (%rsp), %xmm0
jbe 0xde88
jmp 0xdd06
leaq 0x1c0(%rsp), %rcx
movq 0x10(%rcx), %rax
movq %rax, 0x10(%rbx)
movupd (%rcx), %xmm0
movupd %xmm0, (%rbx)
jmp 0xdd06