// File: /workspace/github_c_makefile_star2_O3/zpole[P]IICV/vl/rodrigues.c
// Function: vl_irodrigues
VL_EXPORT
void vl_irodrigues(double* om_pt, double* dom_pt, const double* R_pt)
{
  /*
                    tr R - 1          1    [ R32 - R23 ]
      th = cos^{-1} --------,  r =  ------ [ R13 - R31 ], w = th r.
                        2           2 sth  [ R12 - R21 ]

      sth = sin(th)

       dw    th*cth-sth      dw     th   [di3 dj2 - di2 dj3]
      ---- = ---------- r,  ---- = ----- [di1 dj3 - di3 dj1].
      dRii     2 sth^2      dRij   2 sth [di1 dj2 - di2 dj1]

      trace(A) < -1 only for small num. errors.
  */

#define OM(i)    om_pt[(i)]
#define DOM(i,j) dom_pt[(i)+3*(j)]
#define R(i,j)   R_pt[(i)+3*(j)]
#define W(i,j)   W_pt[(i)+3*(j)]

  const double small = 1e-6 ;

  double th = acos
    (0.5*(VL_MAX(R(0,0)+R(1,1)+R(2,2),-1.0) - 1.0)) ;

  double sth = sin(th) ;
  double cth = cos(th) ;

  if(fabs(sth) < small && cth < 0) {
    /*
      we have this singularity when the rotation  is about pi (or -pi)
      we use the fact that in this case

      hat( sqrt(1-cth) * r )^2 = W = (0.5*(R+R') - eye(3))

      which gives

      (1-cth) rx^2 = 0.5 * (W(1,1)-W(2,2)-W(3,3))
      (1-cth) ry^2 = 0.5 * (W(2,2)-W(3,3)-W(1,1))
      (1-cth) rz^2 = 0.5 * (W(3,3)-W(1,1)-W(2,2))
    */

    double W_pt [9], x, y, z ;
    W_pt[0] = 0.5*( R(0,0) + R(0,0) ) - 1.0 ;
    W_pt[1] = 0.5*( R(1,0) + R(0,1) ) ;
    W_pt[2] = 0.5*( R(2,0) + R(0,2) );

    W_pt[3] = 0.5*( R(0,1) + R(1,0) );
    W_pt[4] = 0.5*( R(1,1) + R(1,1) ) - 1.0;
    W_pt[5] = 0.5*( R(2,1) + R(1,2) );

    W_pt[6] =  0.5*( R(0,2) + R(2,0) ) ;
    W_pt[7] =  0.5*( R(1,2) + R(2,1) ) ;
    W_pt[8] =  0.5*( R(2,2) + R(2,2) ) - 1.0 ;

    /* these are only absolute values */
    x = sqrt( 0.5 * (W(0,0)-W(1,1)-W(2,2)) ) ;
    y = sqrt( 0.5 * (W(1,1)-W(2,2)-W(0,0)) ) ;
    z = sqrt( 0.5 * (W(2,2)-W(0,0)-W(1,1)) ) ;

    /* set the biggest component to + and use the element of the
    ** matrix W to determine the sign of the other components
    ** then the solution is either (x,y,z) or its opposite */
    if( x >= y && x >= z ) {
      y = (W(1,0) >=0) ? y : -y ;
      z = (W(2,0) >=0) ? z : -z ;
    } else if( y >= x && y >= z ) {
      z = (W(2,1) >=0) ? z : -z ;
      x = (W(1,0) >=0) ? x : -x ;
    } else {
      x = (W(2,0) >=0) ? x : -x ;
      y = (W(2,1) >=0) ? y : -y ;
    }

    /* we are left to chose between (x,y,z) and (-x,-y,-z)
    ** unfortunately we cannot (as the rotation is too close to pi) and
    ** we just keep what we have. */
    {
      double scale = th / sqrt( 1 - cth ) ;
      OM(0) = scale * x ;
      OM(1) = scale * y ;
      OM(2) = scale * z ;

      if( dom_pt ) {
        int k ;
        for(k=0; k<3*9; ++k)
          dom_pt [k] = VL_NAN_D ;
      }
      return ;
    }

  } else {
    double a = (fabs(sth) < small) ? 1 : th/sin(th) ;
    double b ;
    OM(0) = 0.5*a*(R(2,1) - R(1,2)) ;
    OM(1) = 0.5*a*(R(0,2) - R(2,0)) ;
    OM(2) = 0.5*a*(R(1,0) - R(0,1)) ;

    if( dom_pt ) {
      if( fabs(sth) < small ) {
        a = 0.5 ;
        b = 0 ;
      } else {
        a = th/(2*sth) ;
        b = (th*cth - sth)/(2*sth*sth)/th ;
      }

      DOM(0,0) = b*OM(0) ;
      DOM(1,0) = b*OM(1) ;
      DOM(2,0) = b*OM(2) ;

      DOM(0,1) = 0 ;
      DOM(1,1) = 0 ;
      DOM(2,1) = a ;

      DOM(0,2) = 0 ;
      DOM(1,2) = -a ;
      DOM(2,2) = 0 ;

      DOM(0,3) = 0 ;
      DOM(1,3) = 0 ;
      DOM(2,3) = -a ;

      DOM(0,4) = b*OM(0) ;
      DOM(1,4) = b*OM(1) ;
      DOM(2,4) = b*OM(2) ;

      DOM(0,5) = a ;
      DOM(1,5) = 0 ;
      DOM(2,5) = 0 ;

      DOM(0,6) = 0 ;
      DOM(1,6) = a ;
      DOM(2,6) = 0 ;

      DOM(0,7) = -a ;
      DOM(1,7) = 0 ;
      DOM(2,7) = 0 ;

      DOM(0,8) = b*OM(0) ;
      DOM(1,8) = b*OM(1) ;
      DOM(2,8) = b*OM(2) ;
    }
  }

#undef OM
#undef DOM
#undef R
#undef W
}

// Assembly:
pushq %r15
pushq %r14
pushq %rbx
subq $0x80, %rsp
movq %rdx, %r15
movq %rsi, %rbx
movq %rdi, %r14
movsd (%rdx), %xmm1
addsd 0x20(%rdx), %xmm1
addsd 0x40(%rdx), %xmm1
movsd 0x81ea(%rip), %xmm2     # 0x32208
movapd %xmm1, %xmm3
addsd %xmm2, %xmm3
mulsd 0x81b2(%rip), %xmm3     # 0x321e0
movapd %xmm2, %xmm0
cmpltsd %xmm1, %xmm0
andpd %xmm0, %xmm3
andnpd %xmm2, %xmm0
orpd %xmm3, %xmm0
callq 0x9480
movapd %xmm0, 0x20(%rsp)
callq 0x9860
movapd %xmm0, (%rsp)
movapd 0x20(%rsp), %xmm0
callq 0x9450
movapd %xmm0, %xmm3
movapd 0x8091(%rip), %xmm5     # 0x32100
andpd (%rsp), %xmm5
movsd 0x8344(%rip), %xmm6     # 0x323c0
ucomisd %xmm5, %xmm6
jbe 0x2a14b
xorpd %xmm1, %xmm1
ucomisd %xmm3, %xmm1
jbe 0x2a14b
movsd 0x8(%r15), %xmm2
addsd 0x18(%r15), %xmm2
movsd 0x8138(%rip), %xmm5     # 0x321e0
movsd (%r15), %xmm7
movhpd 0x20(%r15), %xmm7       # xmm7 = xmm7[0],mem[0]
addpd %xmm7, %xmm7
mulpd 0x8031(%rip), %xmm7     # 0x320f0
addpd 0x8099(%rip), %xmm7     # 0x32160
movups 0x28(%r15), %xmm4
movsd 0x38(%r15), %xmm6
movhps 0x10(%r15), %xmm6       # xmm6 = xmm6[0,1],mem[0,1]
movsd 0x40(%r15), %xmm9
addsd %xmm9, %xmm9
mulsd %xmm5, %xmm9
addsd 0x8118(%rip), %xmm9     # 0x32208
movapd %xmm7, %xmm10
unpckhpd %xmm7, %xmm10           # xmm10 = xmm10[1],xmm7[1]
movapd %xmm7, %xmm0
subsd %xmm10, %xmm0
subsd %xmm9, %xmm0
mulsd %xmm5, %xmm0
ucomisd %xmm1, %xmm0
movsd %xmm3, 0x10(%rsp)
movsd %xmm2, 0x38(%rsp)
movaps %xmm4, (%rsp)
movapd %xmm7, 0x40(%rsp)
movsd %xmm9, 0x30(%rsp)
movaps %xmm6, 0x70(%rsp)
movapd %xmm10, 0x60(%rsp)
jb 0x2a203
xorps %xmm1, %xmm1
sqrtsd %xmm0, %xmm1
jmp 0x2a220
ucomisd %xmm5, %xmm6
jbe 0x2a161
movsd 0x8087(%rip), %xmm2     # 0x321e0
movapd 0x20(%rsp), %xmm4
jmp 0x2a1a2
movapd 0x20(%rsp), %xmm0
movsd %xmm3, 0x10(%rsp)
movapd %xmm5, 0x50(%rsp)
callq 0x9860
movsd 0x8240(%rip), %xmm6     # 0x323c0
movapd 0x50(%rsp), %xmm5
movsd 0x10(%rsp), %xmm3
movapd 0x20(%rsp), %xmm4
movapd %xmm4, %xmm2
divsd %xmm0, %xmm2
mulsd 0x803e(%rip), %xmm2     # 0x321e0
movsd 0x28(%r15), %xmm1
subsd 0x38(%r15), %xmm1
mulsd %xmm2, %xmm1
movsd %xmm1, (%r14)
movsd 0x30(%r15), %xmm0
subsd 0x10(%r15), %xmm0
mulsd %xmm2, %xmm0
movsd %xmm0, 0x8(%r14)
movsd 0x8(%r15), %xmm0
subsd 0x18(%r15), %xmm0
mulsd %xmm2, %xmm0
movsd %xmm0, 0x10(%r14)
testq %rbx, %rbx
je 0x2a5ec
ucomisd %xmm5, %xmm6
jbe 0x2a3bc
movsd 0x7fe2(%rip), %xmm4     # 0x321e0
jmp 0x2a3e9
callq 0x9990
movapd 0x60(%rsp), %xmm10
movsd 0x30(%rsp), %xmm9
movapd 0x40(%rsp), %xmm7
movapd %xmm0, %xmm1
movapd %xmm10, %xmm0
subsd %xmm9, %xmm0
subsd %xmm7, %xmm0
mulsd 0x7faa(%rip), %xmm0     # 0x321e0
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm0
movapd %xmm1, 0x50(%rsp)
jb 0x2a24d
sqrtsd %xmm0, %xmm8
jmp 0x2a26f
callq 0x9990
xorpd %xmm2, %xmm2
movapd 0x60(%rsp), %xmm10
movsd 0x30(%rsp), %xmm9
movapd 0x40(%rsp), %xmm7
movapd %xmm0, %xmm8
movapd (%rsp), %xmm6
movapd 0x70(%rsp), %xmm0
addpd %xmm0, %xmm6
subsd %xmm7, %xmm9
subsd %xmm10, %xmm9
mulsd 0x7f4f(%rip), %xmm9     # 0x321e0
ucomisd %xmm2, %xmm9
jb 0x2a2a2
xorps %xmm7, %xmm7
sqrtsd %xmm9, %xmm7
jmp 0x2a2c8
movapd %xmm9, %xmm0
movapd %xmm6, (%rsp)
movapd %xmm8, 0x40(%rsp)
callq 0x9990
movapd 0x40(%rsp), %xmm8
movapd (%rsp), %xmm6
movapd %xmm0, %xmm7
movapd 0x50(%rsp), %xmm5
movsd 0x38(%rsp), %xmm4
movsd 0x7f04(%rip), %xmm0     # 0x321e0
movapd 0x7e0c(%rip), %xmm1     # 0x320f0
mulsd %xmm0, %xmm4
mulpd %xmm1, %xmm6
ucomisd %xmm8, %xmm5
jb 0x2a356
ucomisd %xmm7, %xmm5
jb 0x2a356
movapd 0x7dcf(%rip), %xmm0     # 0x320d0
movapd %xmm8, %xmm1
xorpd %xmm0, %xmm1
xorpd %xmm2, %xmm2
xorpd %xmm3, %xmm3
cmplesd %xmm4, %xmm3
movapd %xmm3, %xmm4
andnpd %xmm1, %xmm4
andpd %xmm8, %xmm3
orpd %xmm4, %xmm3
unpckhpd %xmm6, %xmm6            # xmm6 = xmm6[1,1]
xorpd %xmm7, %xmm0
cmplesd %xmm6, %xmm2
movapd %xmm2, %xmm1
andnpd %xmm0, %xmm1
andpd %xmm7, %xmm2
orpd %xmm1, %xmm2
unpcklpd %xmm3, %xmm5            # xmm5 = xmm5[0],xmm3[0]
movapd %xmm2, %xmm7
movapd %xmm5, %xmm6
jmp 0x2a50e
ucomisd %xmm5, %xmm8
jb 0x2a4e3
ucomisd %xmm7, %xmm8
jb 0x2a4e3
movapd 0x7d5c(%rip), %xmm0     # 0x320d0
xorpd %xmm1, %xmm1
cmplesd %xmm6, %xmm1
movapd %xmm1, %xmm2
andpd %xmm7, %xmm1
xorpd %xmm0, %xmm7
xorpd %xmm6, %xmm6
andnpd %xmm7, %xmm2
orpd %xmm2, %xmm1
xorpd %xmm5, %xmm0
cmplesd %xmm4, %xmm6
movapd %xmm6, %xmm2
andnpd %xmm0, %xmm2
andpd %xmm5, %xmm6
orpd %xmm2, %xmm6
unpcklpd %xmm8, %xmm6            # xmm6 = xmm6[0],xmm8[0]
movapd %xmm1, %xmm7
jmp 0x2a50e
movapd (%rsp), %xmm5
movapd %xmm5, %xmm0
addsd %xmm5, %xmm0
movapd %xmm4, %xmm2
mulsd %xmm3, %xmm2
subsd %xmm5, %xmm2
mulsd %xmm0, %xmm5
divsd %xmm5, %xmm2
unpcklpd %xmm4, %xmm0            # xmm0 = xmm0[0],xmm4[0]
unpcklpd %xmm2, %xmm4            # xmm4 = xmm4[0],xmm2[0]
divpd %xmm0, %xmm4
movapd %xmm4, %xmm0
unpckhpd %xmm4, %xmm0            # xmm0 = xmm0[1],xmm4[1]
mulsd %xmm0, %xmm1
movsd %xmm1, (%rbx)
movsd 0x8(%r14), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rbx)
movsd 0x10(%r14), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, 0x10(%rbx)
xorpd %xmm1, %xmm1
movupd %xmm1, 0x18(%rbx)
movlpd %xmm4, 0x28(%rbx)
xorl %eax, %eax
movq %rax, 0x30(%rbx)
movapd 0x7c9d(%rip), %xmm2     # 0x320d0
xorpd %xmm4, %xmm2
movlpd %xmm2, 0x38(%rbx)
movupd %xmm1, 0x40(%rbx)
movq %rax, 0x50(%rbx)
movlpd %xmm2, 0x58(%rbx)
movsd (%r14), %xmm3
mulsd %xmm0, %xmm3
movsd %xmm3, 0x60(%rbx)
movsd 0x8(%r14), %xmm3
mulsd %xmm0, %xmm3
movsd %xmm3, 0x68(%rbx)
movsd 0x10(%r14), %xmm3
mulsd %xmm0, %xmm3
movsd %xmm3, 0x70(%rbx)
movlpd %xmm4, 0x78(%rbx)
movupd %xmm1, 0x80(%rbx)
movq %rax, 0x90(%rbx)
movlpd %xmm4, 0x98(%rbx)
movq %rax, 0xa0(%rbx)
movlpd %xmm2, 0xa8(%rbx)
movupd %xmm1, 0xb0(%rbx)
movsd (%r14), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, 0xc0(%rbx)
movsd 0x8(%r14), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, 0xc8(%rbx)
movsd 0x10(%r14), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, 0xd0(%rbx)
jmp 0x2a5ec
xorpd %xmm0, %xmm0
cmplepd %xmm6, %xmm0
pshufd $0x4e, %xmm0, %xmm6     # xmm6 = xmm0[2,3,0,1]
unpcklpd %xmm8, %xmm5            # xmm5 = xmm5[0],xmm8[0]
movapd 0x7bd2(%rip), %xmm0     # 0x320d0
xorpd %xmm5, %xmm0
andpd %xmm6, %xmm5
andnpd %xmm0, %xmm6
orpd %xmm5, %xmm6
movsd 0x10(%rsp), %xmm1
movsd 0x7d54(%rip), %xmm0     # 0x32270
subsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x2a530
sqrtsd %xmm0, %xmm0
jmp 0x2a54b
movapd %xmm7, (%rsp)
movapd %xmm6, 0x10(%rsp)
callq 0x9990
movapd 0x10(%rsp), %xmm6
movapd (%rsp), %xmm7
movapd 0x20(%rsp), %xmm1
divsd %xmm0, %xmm1
mulsd %xmm1, %xmm7
unpcklpd %xmm1, %xmm1            # xmm1 = xmm1[0,0]
mulpd %xmm1, %xmm6
movupd %xmm6, (%r14)
movsd %xmm7, 0x10(%r14)
testq %rbx, %rbx
je 0x2a5ec
movdqa 0x7a87(%rip), %xmm0     # 0x32000
movl $0x1, %eax
movdqa 0x7a8a(%rip), %xmm1     # 0x32010
movdqa 0x8db2(%rip), %xmm2     # 0x33340
pshufd $0xf5, %xmm2, %xmm3     # xmm3 = xmm2[1,1,3,3]
movabsq $0x7ff8000000000000, %rcx # imm = 0x7FF8000000000000
movdqa 0x7a7b(%rip), %xmm4     # 0x32020
movdqa %xmm0, %xmm5
pxor %xmm1, %xmm5
movdqa %xmm2, %xmm6
pcmpgtd %xmm5, %xmm6
pshufd $0xf5, %xmm5, %xmm5     # xmm5 = xmm5[1,1,3,3]
pcmpeqd %xmm3, %xmm5
pand %xmm6, %xmm5
movd %xmm5, %edx
testb $0x1, %dl
je 0x2a5d0
movq %rcx, -0x8(%rbx,%rax,8)
pextrw $0x4, %xmm5, %edx
testb $0x1, %dl
je 0x2a5de
movq %rcx, (%rbx,%rax,8)
paddq %xmm4, %xmm0
addq $0x2, %rax
cmpq $0x1d, %rax
jne 0x2a5a5
addq $0x80, %rsp
popq %rbx
popq %r14
popq %r15
retq
nopl (%rax)