// File: /workspace/github_cpp_makefile_star2_O3/devernay[P]vlpovutils/vec2camera.cpp
// Function: vlpov_vec2camera(int, int, boost::numeric::ublas::vector3 const&, boost::numeric::ublas::vector3 const&, boost::numeric::ublas::vector3 const&, boost::numeric::ublas::vector3 const&, boost::numeric::ublas::matrix33&, boost::numeric::ublas::matrix33&, boost::numeric::ublas::vector3&)
void vlpov_vec2camera(int width,
                      int height,
                      const boost::numeric::ublas::vector3 &pos,
                      const boost::numeric::ublas::vector3 &dir,
                      const boost::numeric::ublas::vector3 &up,
                      const boost::numeric::ublas::vector3 &right,
                      boost::numeric::ublas::matrix33 &K,
                      boost::numeric::ublas::matrix33 &R,
                      boost::numeric::ublas::vector3 &T)
{
    // Summary:
    // any 3-D point M=(X,Y,Z) can be decomposed in the (right, up, dir) frame, as
    // M = pos + x*right + y*up + z*dir,
    // or M = pos + [right,up,dir]*m, with m=(x,y,z)
    // camera coordinates are (x/z,y/z), in [-0.5,0.5]x[-0.5,0.5], and are then mapped
    // to pixel coordinates in the range [-0.5,width-0.5]x[height-0.5,-0.5] (the y-axis is inverted)
    // the projection matrix can then be written as:
    // P = B[A -A*pos] = BA[I3 -pos], with A = inverse([right,up,dir]) and B = [[width;0;0.5*(width-1)][0;-height;0.5*(height-1)][0;0;1]] 
    // BA can then be decomposed into KR (using RQ decomposition), so that
    // P = K[R t], with t = -R*pos

    matrix33 A;
    matrix33 RUD = matrix33_from_columns(right,up,dir);
    double det = determinant(RUD);
    inverse(RUD, det, A);
    matrix33 B = matrix33_from_coeffs(width, 0, 0.5*(width-1),
                                      0, -height, 0.5*(height-1),
                                      0, 0, 1);
    matrix33 BA = prod(B,A);
    rqdecomp33(BA,K,R);
#   ifdef DEBUG
    //cout << "BA=" << endl << BA << endl << "KR=" << endl <<  prod(K,R) << endl;
#   endif
    K /= K(2,2); // normalize K
    noalias(T) = -prod(R,pos);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x198, %rsp            # imm = 0x198
movq %rdx, %rbx
movl %esi, %ebp
movl %edi, %r14d
movq 0x1d0(%rsp), %r12
movl $0x3, %eax
leaq 0x138(%rsp), %r13
movq %rax, (%r13)
movq %rax, 0x8(%r13)
movq $0x9, 0x10(%r13)
leaq 0xd8(%rsp), %r15
movq %r15, %rdi
movq %r9, %rsi
movq %r8, %rdx
callq 0x810b
movq %r15, %rdi
callq 0x8387
movq %r15, %rdi
movq %r13, %rsi
callq 0x866c
xorps %xmm0, %xmm0
cvtsi2sd %r14d, %xmm0
decl %r14d
cvtsi2sd %r14d, %xmm2
movsd 0x3bd5(%rip), %xmm1     # 0xbc10
mulsd %xmm1, %xmm2
movl %ebp, %eax
negl %eax
cvtsi2sd %eax, %xmm4
decl %ebp
cvtsi2sd %ebp, %xmm5
mulsd %xmm1, %xmm5
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, (%rsp)
leaq 0x78(%rsp), %r14
xorpd %xmm1, %xmm1
xorps %xmm3, %xmm3
xorps %xmm6, %xmm6
xorps %xmm7, %xmm7
movq %r14, %rdi
callq 0x9e50
leaq 0x8(%rsp), %rbp
movq %r14, (%rbp)
movq %r13, 0x8(%rbp)
leaq 0x18(%rsp), %r14
movq %r14, %rdi
movq %rbp, %rsi
callq 0xa75c
movq %r14, %rdi
movq %r12, %rsi
movq 0x1d8(%rsp), %r15
movq %r15, %rdx
callq 0x896c
leaq 0x10(%r12), %r14
movq (%r12), %rsi
movq 0x8(%r12), %rcx
movl $0x2, %edi
movl $0x2, %edx
callq 0x5792
movq %r14, %rdi
movq %rax, %rsi
callq 0x75be
movq %r12, %rdi
movq %rax, %rsi
callq 0xa7a0
movq %r15, (%rbp)
movq %rbx, 0x8(%rbp)
leaq 0x8(%rsp), %rsi
movq 0x1e0(%rsp), %rdi
callq 0xa819
addq $0x198, %rsp            # imm = 0x198
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq