// File: /workspace/github_c_makefile_star2_O2/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 0x7bc6(%rip), %xmm2     # 0x2d1c8
movapd %xmm1, %xmm3
addsd %xmm2, %xmm3
mulsd 0x7bae(%rip), %xmm3     # 0x2d1c0
movapd %xmm2, %xmm0
cmpltsd %xmm1, %xmm0
andpd %xmm0, %xmm3
andnpd %xmm2, %xmm0
orpd %xmm3, %xmm0
callq 0x94b0
movapd %xmm0, 0x20(%rsp)
callq 0x98d0
movapd %xmm0, (%rsp)
movapd 0x20(%rsp), %xmm0
callq 0x9470
movapd %xmm0, %xmm3
movapd 0x7a8d(%rip), %xmm5     # 0x2d0e0
andpd (%rsp), %xmm5
movsd 0x7d30(%rip), %xmm6     # 0x2d390
ucomisd %xmm5, %xmm6
jbe 0x2572c
xorpd %xmm1, %xmm1
ucomisd %xmm3, %xmm1
jbe 0x2572c
movsd 0x8(%r15), %xmm2
addsd 0x18(%r15), %xmm2
movsd 0x7b34(%rip), %xmm5     # 0x2d1c0
movsd (%r15), %xmm7
movhpd 0x20(%r15), %xmm7       # xmm7 = xmm7[0],mem[0]
addpd %xmm7, %xmm7
mulpd 0x7a0d(%rip), %xmm7     # 0x2d0b0
addpd 0x7a95(%rip), %xmm7     # 0x2d140
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 0x7af4(%rip), %xmm9     # 0x2d1c8
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 0x257e4
sqrtsd %xmm0, %xmm1
jmp 0x25801
ucomisd %xmm5, %xmm6
jbe 0x25742
movsd 0x7a86(%rip), %xmm2     # 0x2d1c0
movapd 0x20(%rsp), %xmm4
jmp 0x25783
movapd 0x20(%rsp), %xmm0
movsd %xmm3, 0x10(%rsp)
movapd %xmm5, 0x50(%rsp)
callq 0x98d0
movsd 0x7c2f(%rip), %xmm6     # 0x2d390
movapd 0x50(%rsp), %xmm5
movsd 0x10(%rsp), %xmm3
movapd 0x20(%rsp), %xmm4
movapd %xmm4, %xmm2
divsd %xmm0, %xmm2
mulsd 0x7a3d(%rip), %xmm2     # 0x2d1c0
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 0x25b6c
ucomisd %xmm5, %xmm6
jbe 0x2599a
movsd 0x79e1(%rip), %xmm4     # 0x2d1c0
jmp 0x259c7
callq 0x9a00
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 0x79a9(%rip), %xmm0     # 0x2d1c0
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm0
movapd %xmm1, 0x50(%rsp)
jb 0x2582e
sqrtsd %xmm0, %xmm8
jmp 0x25850
callq 0x9a00
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 0x794e(%rip), %xmm9     # 0x2d1c0
ucomisd %xmm2, %xmm9
jb 0x25880
sqrtsd %xmm9, %xmm7
jmp 0x258a6
movapd %xmm9, %xmm0
movapd %xmm6, (%rsp)
movapd %xmm8, 0x40(%rsp)
callq 0x9a00
movapd 0x40(%rsp), %xmm8
movapd (%rsp), %xmm6
movapd %xmm0, %xmm7
movapd 0x50(%rsp), %xmm5
movsd 0x38(%rsp), %xmm4
movsd 0x7906(%rip), %xmm0     # 0x2d1c0
movapd 0x77ee(%rip), %xmm1     # 0x2d0b0
mulsd %xmm0, %xmm4
mulpd %xmm1, %xmm6
ucomisd %xmm8, %xmm5
jb 0x25934
ucomisd %xmm7, %xmm5
jb 0x25934
movapd 0x77c1(%rip), %xmm0     # 0x2d0a0
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 0x25aee
ucomisd %xmm5, %xmm8
jb 0x25ac3
ucomisd %xmm7, %xmm8
jb 0x25ac3
movapd 0x774e(%rip), %xmm0     # 0x2d0a0
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 0x25aee
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)
andq $0x0, 0x30(%rbx)
movapd 0x7690(%rip), %xmm2     # 0x2d0a0
xorpd %xmm4, %xmm2
movlpd %xmm2, 0x38(%rbx)
andq $0x0, 0x50(%rbx)
movupd %xmm1, 0x40(%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)
andq $0x0, 0x90(%rbx)
movlpd %xmm4, 0x98(%rbx)
andq $0x0, 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 0x25b6c
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 0x75c2(%rip), %xmm0     # 0x2d0a0
xorpd %xmm5, %xmm0
andpd %xmm6, %xmm5
andnpd %xmm0, %xmm6
orpd %xmm5, %xmm6
movsd 0x10(%rsp), %xmm1
movsd 0x773c(%rip), %xmm0     # 0x2d238
subsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x25b10
sqrtsd %xmm0, %xmm0
jmp 0x25b2b
movapd %xmm7, (%rsp)
movapd %xmm6, 0x10(%rsp)
callq 0x9a00
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 0x25b6c
xorl %eax, %eax
movabsq $0x7ff8000000000000, %rcx # imm = 0x7FF8000000000000
cmpq $0x1b, %rax
je 0x25b6c
movq %rcx, (%rbx,%rax,8)
incq %rax
jmp 0x25b5d
addq $0x80, %rsp
popq %rbx
popq %r14
popq %r15
retq
nopl (%rax)