// File: /workspace/github_c_makefile_star2_O1/zpole[P]IICV/vl/rodrigues.c
// Function: vl_rodrigues
void
vl_rodrigues(double* R_pt, double* dR_pt, const double* om_pt)
{
  /*
    Let

       th = |om|,  r=w/th,
       sth=sin(th),  cth=cos(th),
       ^om = hat(om)

    Then the rodrigues formula is an expansion of the exponential
    function:

     rodrigues(om) = exp ^om = I + ^r sth + ^r^2 (1 - cth).

    The derivative can be computed by elementary means and
    results:

    d(vec rodrigues(om))    sth  d ^r    1 - cth  d (^r)^2
    -------------------- =  ---- ----- + -------  -------- +
          d om^T             th  d r^T     th      d r^T

                          sth                     1 - cth
          + vec^r (cth - -----) + vec^r^2 (sth - 2-------)r^T
                          th                         th
  */

#define OM(i)   om_pt[(i)]
#define R(i,j)  R_pt[(i)+3*(j)]
#define DR(i,j) dR_pt[(i)+9*(j)]
#undef small

  const double small = 1e-6 ;

  double th = sqrt( OM(0)*OM(0) +
                    OM(1)*OM(1) +
                    OM(2)*OM(2) ) ;

  if( th < small ) {
    R(0,0) = 1.0 ; R(0,1) = 0.0 ; R(0,2) = 0.0 ;
    R(1,0) = 0.0 ; R(1,1) = 1.0 ; R(1,2) = 0.0 ;
    R(2,0) = 0.0 ; R(2,1) = 0.0 ; R(2,2) = 1.0 ;

    if(dR_pt) {
      DR(0,0) = 0  ; DR(0,1) = 0   ; DR(0,2) = 0 ;
      DR(1,0) = 0  ; DR(1,1) = 0   ; DR(1,2) = 1 ;
      DR(2,0) = 0  ; DR(2,1) = -1  ; DR(2,2) = 0 ;

      DR(3,0) = 0  ; DR(3,1) = 0   ; DR(3,2) = -1 ;
      DR(4,0) = 0  ; DR(4,1) = 0   ; DR(4,2) = 0 ;
      DR(5,0) = 1  ; DR(5,1) = 0   ; DR(5,2) = 0 ;

      DR(6,0) = 0  ; DR(6,1) = 1   ; DR(6,2) = 0 ;
      DR(7,0) = -1 ; DR(7,1) = 0   ; DR(7,2) = 0 ;
      DR(8,0) = 0  ; DR(8,1) = 0   ; DR(8,2) = 0 ;
    }
    return ;
  }

  {
    double x = OM(0) / th ;
    double y = OM(1) / th ;
    double z = OM(2) / th ;

    double xx = x*x ;
    double xy = x*y ;
    double xz = x*z ;
    double yy = y*y ;
    double yz = y*z ;
    double zz = z*z ;

    const double yx = xy ;
    const double zx = xz ;
    const double zy = yz ;

    double sth  = sin(th) ;
    double cth  = cos(th) ;
    double mcth = 1.0 - cth ;

    R(0,0) = 1          - mcth * (yy+zz) ;
    R(1,0) =     sth*z  + mcth * xy ;
    R(2,0) =   - sth*y  + mcth * xz ;

    R(0,1) =   - sth*z  + mcth * yx ;
    R(1,1) = 1          - mcth * (zz+xx) ;
    R(2,1) =     sth*x  + mcth * yz ;

    R(0,2) =     sth*y  + mcth * xz ;
    R(1,2) =   - sth*x  + mcth * yz ;
    R(2,2) = 1          - mcth * (xx+yy) ;

    if(dR_pt) {
      double a =  sth / th ;
      double b = mcth / th ;
      double c = cth - a ;
      double d = sth - 2*b ;

      DR(0,0) =                         - d * (yy+zz) * x ;
      DR(1,0) =        b*y   + c * zx   + d * xy      * x ;
      DR(2,0) =        b*z   - c * yx   + d * xz      * x ;

      DR(3,0) =        b*y   - c * zx   + d * xy      * x ;
      DR(4,0) =     -2*b*x              - d * (zz+xx) * x ;
      DR(5,0) =  a           + c * xx   + d * yz      * x ;

      DR(6,0) =        b*z   + c * yx   + d * zx      * x ;
      DR(7,0) = -a           - c * xx   + d * zy      * x ;
      DR(8,0) =     -2*b*x              - d * (yy+xx) * x ;

      DR(0,1) =     -2*b*y              - d * (yy+zz) * y ;
      DR(1,1) =        b*x   + c * zy   + d * xy      * y ;
      DR(2,1) = -a           - c * yy   + d * xz      * y ;

      DR(3,1) =        b*x   - c * zy   + d * xy      * y ;
      DR(4,1) =                         - d * (zz+xx) * y ;
      DR(5,1) =        b*z   + c * xy   + d * yz      * y ;

      DR(6,1) = a            + c * yy   + d * zx      * y ;
      DR(7,1) =        b*z   - c * xy   + d * zy      * y ;
      DR(8,1) =     -2*b*y              - d * (yy+xx) * y ;

      DR(0,2) =     -2*b*z              - d * (yy+zz) * z ;
      DR(1,2) =  a           + c * zz   + d * xy      * z ;
      DR(2,2) =        b*x   - c * yz   + d * xz      * z ;

      DR(3,2) =  -a          - c * zz   + d * xy      * z ;
      DR(4,2) =     -2*b*z              - d * (zz+xx) * z ;
      DR(5,2) =        b*y   + c * xz   + d * yz      * z ;

      DR(6,2) =        b*x   + c * yz   + d * zx      * z ;
      DR(7,2) =        b*y   - c * xz   + d * zy      * z ;
      DR(8,2) =                         - d * (yy+xx) * z ;
    }
  }

#undef OM
#undef R
#undef DR

}

// Assembly:
pushq %r15
pushq %r14
pushq %rbx
subq $0x120, %rsp            # imm = 0x120
movq %rdx, %r15
movq %rsi, %rbx
movq %rdi, %r14
movsd (%rdx), %xmm1
movsd 0x8(%rdx), %xmm0
mulsd %xmm0, %xmm0
mulsd %xmm1, %xmm1
addsd %xmm0, %xmm1
movsd 0x10(%rdx), %xmm0
mulsd %xmm0, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x29b4f
sqrtsd %xmm0, %xmm0
jmp 0x29b54
callq 0x9990
movsd 0x9854(%rip), %xmm1     # 0x333b0
ucomisd %xmm0, %xmm1
jbe 0x29c20
movsd 0x96f2(%rip), %xmm1     # 0x33260
movupd %xmm1, (%r14)
xorpd %xmm0, %xmm0
movupd %xmm0, 0x30(%r14)
movupd %xmm0, 0x10(%r14)
movupd %xmm1, 0x20(%r14)
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, 0x40(%r14)
testq %rbx, %rbx
je 0x2a212
movupd %xmm0, (%rbx)
movupd %xmm0, 0x48(%rbx)
xorpd %xmm1, %xmm1
movhpd 0x96ab(%rip), %xmm1     # xmm1 = xmm1[0],mem[0]
movupd %xmm1, 0x90(%rbx)
movupd %xmm0, 0x10(%rbx)
movsd 0x962e(%rip), %xmm2     # 0x331f8
xorpd %xmm3, %xmm3
movhpd 0x9622(%rip), %xmm3     # xmm3 = xmm3[0],mem[0]
movupd %xmm2, 0x58(%rbx)
movupd %xmm3, 0xa0(%rbx)
movupd %xmm1, 0x20(%rbx)
movupd %xmm0, 0x68(%rbx)
movupd %xmm0, 0xb0(%rbx)
movq %rax, 0x78(%rbx)
xorl %eax, %eax
movq %rax, 0xc0(%rbx)
movupd %xmm3, 0x30(%rbx)
movq %rax, 0x40(%rbx)
movupd %xmm0, 0x80(%rbx)
movupd %xmm0, 0xc8(%rbx)
jmp 0x2a212
movsd (%r15), %xmm2
divsd %xmm0, %xmm2
movapd %xmm2, 0x50(%rsp)
movapd %xmm2, %xmm1
mulsd %xmm2, %xmm1
movapd %xmm1, 0x90(%rsp)
movupd 0x8(%r15), %xmm3
movapd %xmm0, %xmm1
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
divpd %xmm1, %xmm3
movapd %xmm2, %xmm1
unpcklpd %xmm2, %xmm1            # xmm1 = xmm1[0],xmm2[0]
movapd %xmm1, 0x60(%rsp)
movapd %xmm3, %xmm2
movapd %xmm3, 0x80(%rsp)
mulpd %xmm3, %xmm1
movapd %xmm1, 0x20(%rsp)
movapd %xmm3, %xmm1
unpckhpd %xmm3, %xmm1            # xmm1 = xmm1[1],xmm3[1]
movapd %xmm1, 0x110(%rsp)
mulsd %xmm1, %xmm3
movapd %xmm3, 0x30(%rsp)
mulpd %xmm2, %xmm2
movapd %xmm2, 0x70(%rsp)
movapd %xmm0, (%rsp)
callq 0x9860
movapd %xmm0, 0x10(%rsp)
movapd (%rsp), %xmm0
callq 0x9450
movsd 0x95a2(%rip), %xmm6     # 0x33260
movapd %xmm6, %xmm12
subsd %xmm0, %xmm12
movapd 0x70(%rsp), %xmm14
movapd %xmm14, %xmm3
unpckhpd %xmm14, %xmm3           # xmm3 = xmm3[1],xmm14[1]
movapd %xmm14, %xmm7
addsd %xmm3, %xmm7
movapd 0x93e6(%rip), %xmm4     # 0x330d0
movapd %xmm12, %xmm8
xorpd %xmm4, %xmm8
movapd 0x10(%rsp), %xmm15
unpcklpd %xmm15, %xmm8           # xmm8 = xmm8[0],xmm15[0]
movapd 0x80(%rsp), %xmm13
movapd %xmm13, %xmm2
movapd %xmm7, 0x40(%rsp)
movsd %xmm7, %xmm2            # xmm2 = xmm7[0],xmm2[1]
movapd %xmm12, %xmm7
unpcklpd %xmm12, %xmm7           # xmm7 = xmm7[0],xmm12[0]
mulpd 0x20(%rsp), %xmm7
movapd %xmm7, %xmm9
shufpd $0x1, %xmm7, %xmm9      # xmm9 = xmm9[1],xmm7[0]
movapd %xmm6, %xmm10
unpcklpd %xmm7, %xmm10           # xmm10 = xmm10[0],xmm7[0]
mulpd %xmm8, %xmm2
addpd %xmm10, %xmm2
movupd %xmm2, (%r14)
movapd %xmm15, %xmm2
unpcklpd %xmm15, %xmm2           # xmm2 = xmm2[0],xmm15[0]
mulpd %xmm13, %xmm2
subpd %xmm2, %xmm9
movupd %xmm9, 0x10(%r14)
movapd 0x90(%rsp), %xmm1
movapd %xmm1, %xmm11
movapd %xmm3, 0x100(%rsp)
addsd %xmm3, %xmm11
movapd 0x30(%rsp), %xmm9
mulsd %xmm12, %xmm9
movapd %xmm11, %xmm10
movapd 0x50(%rsp), %xmm5
unpcklpd %xmm5, %xmm10           # xmm10 = xmm10[0],xmm5[0]
mulpd %xmm8, %xmm10
movapd 0xa583(%rip), %xmm8     # 0x34330
unpcklpd %xmm9, %xmm8            # xmm8 = xmm8[0],xmm9[0]
addpd %xmm8, %xmm10
xorpd %xmm15, %xmm4
movupd %xmm10, 0x20(%r14)
movapd %xmm15, %xmm2
unpcklpd %xmm4, %xmm15           # xmm15 = xmm15[0],xmm4[0]
unpcklpd %xmm5, %xmm13           # xmm13 = xmm13[0],xmm5[0]
mulpd %xmm15, %xmm13
shufpd $0x1, %xmm9, %xmm7      # xmm7 = xmm7[1],xmm9[0]
addpd %xmm13, %xmm7
movupd %xmm7, 0x30(%r14)
movapd %xmm1, %xmm8
addsd %xmm14, %xmm8
movapd %xmm12, %xmm7
mulsd %xmm8, %xmm7
subsd %xmm7, %xmm6
movsd %xmm6, 0x40(%r14)
testq %rbx, %rbx
je 0x2a212
movapd %xmm14, %xmm4
movapd %xmm2, %xmm10
movapd %xmm8, 0xf0(%rsp)
movapd (%rsp), %xmm1
divsd %xmm1, %xmm2
divsd %xmm1, %xmm12
movapd %xmm12, %xmm6
addsd %xmm12, %xmm6
subsd %xmm6, %xmm10
movapd 0x9289(%rip), %xmm6     # 0x330d0
movapd %xmm10, %xmm9
xorpd %xmm6, %xmm9
movapd %xmm6, %xmm1
movapd 0x40(%rsp), %xmm3
mulsd %xmm9, %xmm3
movapd %xmm3, 0x40(%rsp)
movapd %xmm5, %xmm6
mulsd %xmm3, %xmm6
movsd %xmm6, (%rbx)
subsd %xmm2, %xmm0
movapd %xmm2, %xmm7
movapd %xmm2, 0xe0(%rsp)
movapd 0x20(%rsp), %xmm6
movapd %xmm6, %xmm2
unpckhpd %xmm6, %xmm2            # xmm2 = xmm2[1],xmm6[1]
mulsd %xmm0, %xmm2
movapd %xmm2, %xmm13
movapd %xmm2, (%rsp)
movapd %xmm11, 0x10(%rsp)
movapd %xmm6, %xmm11
mulsd %xmm0, %xmm11
movapd %xmm7, %xmm8
unpcklpd %xmm11, %xmm8           # xmm8 = xmm8[0],xmm11[0]
xorpd %xmm1, %xmm11
movapd %xmm0, %xmm1
unpcklpd %xmm11, %xmm13          # xmm13 = xmm13[0],xmm11[0]
movapd %xmm12, %xmm14
unpcklpd %xmm12, %xmm14          # xmm14 = xmm14[0],xmm12[0]
unpcklpd %xmm5, %xmm4            # xmm4 = xmm4[0],xmm5[0]
movapd %xmm4, 0xc0(%rsp)
movapd %xmm5, 0xd0(%rsp)
movapd %xmm5, 0xb0(%rsp)
movapd 0x80(%rsp), %xmm7
unpcklpd %xmm7, %xmm5            # xmm5 = xmm5[0],xmm7[0]
mulpd %xmm14, %xmm5
movapd %xmm5, 0x50(%rsp)
mulpd %xmm7, %xmm14
addpd %xmm13, %xmm14
movapd %xmm10, %xmm4
unpcklpd %xmm10, %xmm4           # xmm4 = xmm4[0],xmm10[0]
mulpd %xmm6, %xmm4
movapd %xmm4, %xmm13
movapd 0x60(%rsp), %xmm0
mulpd %xmm0, %xmm13
addpd %xmm14, %xmm13
movupd %xmm13, 0x8(%rbx)
movapd %xmm1, %xmm2
movapd %xmm1, %xmm13
movapd %xmm12, 0xa0(%rsp)
unpcklpd %xmm12, %xmm13          # xmm13 = xmm13[0],xmm12[0]
movapd %xmm7, %xmm11
movapd 0x90(%rsp), %xmm5
movsd %xmm5, %xmm11           # xmm11 = xmm5[0],xmm11[1]
mulpd %xmm13, %xmm11
movapd %xmm8, %xmm1
addpd %xmm11, %xmm1
movapd %xmm1, 0x20(%rsp)
movapd 0x10(%rsp), %xmm6
mulsd %xmm9, %xmm6
movapd %xmm6, 0x10(%rsp)
movapd 0xf0(%rsp), %xmm15
mulsd %xmm9, %xmm15
movapd 0x30(%rsp), %xmm8
movapd %xmm8, %xmm9
unpcklpd %xmm12, %xmm9           # xmm9 = xmm9[0],xmm12[0]
shufpd $0x2, 0xa389(%rip), %xmm10 # xmm10 = xmm10[0],mem[1]
mulpd %xmm9, %xmm10
movapd %xmm4, %xmm9
movsd %xmm10, %xmm9           # xmm9 = xmm10[0],xmm9[1]
movapd %xmm9, %xmm11
mulpd %xmm0, %xmm11
addpd %xmm1, %xmm11
movupd %xmm11, 0x28(%rbx)
movapd 0x90ed(%rip), %xmm6     # 0x330d0
movapd (%rsp), %xmm11
xorpd %xmm6, %xmm11
movapd %xmm11, (%rsp)
movapd %xmm2, %xmm11
xorpd %xmm6, %xmm11
movapd 0xe0(%rsp), %xmm12
xorpd %xmm12, %xmm6
mulsd %xmm2, %xmm5
movapd %xmm2, %xmm1
movapd %xmm6, %xmm13
subsd %xmm5, %xmm13
movapd 0xb0(%rsp), %xmm2
mulsd %xmm15, %xmm2
unpcklpd %xmm2, %xmm13           # xmm13 = xmm13[0],xmm2[0]
movapd %xmm10, %xmm3
mulpd %xmm0, %xmm3
addpd %xmm13, %xmm3
movupd %xmm3, 0x38(%rbx)
unpcklpd 0xa0(%rsp), %xmm11      # xmm11 = xmm11[0],mem[0]
movapd 0xc0(%rsp), %xmm2
mulpd %xmm11, %xmm2
mulsd %xmm1, %xmm8
movapd %xmm8, 0x30(%rsp)
movapd %xmm12, %xmm3
unpcklpd %xmm8, %xmm3            # xmm3 = xmm3[0],xmm8[0]
subpd %xmm3, %xmm2
movapd %xmm4, %xmm3
shufpd $0x1, %xmm4, %xmm3      # xmm3 = xmm3[1],xmm4[0]
movapd %xmm7, %xmm5
unpcklpd %xmm7, %xmm5            # xmm5 = xmm5[0],xmm7[0]
mulpd %xmm5, %xmm3
addpd %xmm2, %xmm3
movupd %xmm3, 0x58(%rbx)
movapd %xmm7, %xmm3
movapd 0x10(%rsp), %xmm11
mulsd %xmm11, %xmm3
movsd %xmm3, 0x68(%rbx)
movapd %xmm1, %xmm3
mulsd 0x70(%rsp), %xmm3
addsd %xmm12, %xmm3
movapd 0x20(%rsp), %xmm8
shufpd $0x1, %xmm3, %xmm8      # xmm8 = xmm8[1],xmm3[0]
mulpd %xmm5, %xmm9
addpd %xmm8, %xmm9
movupd %xmm9, 0x70(%rbx)
movapd 0xd0(%rsp), %xmm13
mulsd %xmm11, %xmm13
movapd %xmm7, %xmm3
mulsd %xmm15, %xmm3
movapd 0x110(%rsp), %xmm9
mulsd %xmm9, %xmm11
unpcklpd %xmm14, %xmm11          # xmm11 = xmm11[0],xmm14[0]
shufpd $0x1, %xmm3, %xmm14     # xmm14 = xmm14[1],xmm3[0]
movapd %xmm10, %xmm3
mulpd %xmm5, %xmm3
addpd %xmm14, %xmm3
movupd %xmm3, 0x80(%rbx)
mulsd 0x100(%rsp), %xmm1
movapd 0x40(%rsp), %xmm8
mulsd %xmm8, %xmm7
mulsd %xmm9, %xmm8
addsd %xmm1, %xmm12
unpcklpd %xmm12, %xmm8           # xmm8 = xmm8[0],xmm12[0]
movapd %xmm8, %xmm0
movapd %xmm10, %xmm8
shufpd $0x1, %xmm4, %xmm8      # xmm8 = xmm8[1],xmm4[0]
mulpd %xmm8, %xmm5
mulpd %xmm9, %xmm8
addpd %xmm0, %xmm8
movupd %xmm8, 0x90(%rbx)
subsd %xmm1, %xmm6
shufpd $0x1, %xmm6, %xmm2      # xmm2 = xmm2[1],xmm6[0]
movapd %xmm4, %xmm0
mulpd %xmm9, %xmm0
shufpd $0x1, %xmm0, %xmm0      # xmm0 = xmm0[1,0]
addpd %xmm2, %xmm0
movupd %xmm0, 0xa0(%rbx)
movapd %xmm10, %xmm0
mulpd %xmm9, %xmm0
shufpd $0x1, %xmm0, %xmm0      # xmm0 = xmm0[1,0]
addpd %xmm11, %xmm0
movupd %xmm0, 0xb0(%rbx)
movapd 0x30(%rsp), %xmm0
unpcklpd (%rsp), %xmm0           # xmm0 = xmm0[0],mem[0]
movapd 0x50(%rsp), %xmm6
addpd %xmm0, %xmm6
movapd %xmm6, %xmm0
shufpd $0x1, %xmm13, %xmm0     # xmm0 = xmm0[1],xmm13[0]
movapd %xmm10, %xmm1
movsd %xmm4, %xmm1            # xmm1 = xmm4[0],xmm1[1]
mulpd 0x60(%rsp), %xmm1
addpd %xmm0, %xmm1
movupd %xmm1, 0x18(%rbx)
unpcklpd %xmm6, %xmm7            # xmm7 = xmm7[0],xmm6[0]
addpd %xmm7, %xmm5
movupd %xmm5, 0x48(%rbx)
shufpd $0x1, %xmm10, %xmm4     # xmm4 = xmm4[1],xmm10[0]
mulpd %xmm9, %xmm4
addpd %xmm6, %xmm4
movupd %xmm4, 0xc0(%rbx)
mulsd %xmm9, %xmm15
movsd %xmm15, 0xd0(%rbx)
addq $0x120, %rsp            # imm = 0x120
popq %rbx
popq %r14
popq %r15
retq