// File: /workspace/github_cpp_makefile_star2_O2/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 $0x1a8, %rsp            # imm = 0x1A8
movq %r9, %r15
movq %r8, %rbx
movq %rcx, %r12
movq %rdx, 0x10(%rsp)
movl %esi, %ebp
movl %edi, %r14d
leaq 0x28(%rsp), %rdi
callq 0x39fe
leaq 0x148(%rsp), %r13
movq %r13, %rdi
movq %r15, %rsi
movq %rbx, %rdx
movq %r12, %rcx
callq 0x649d
movq %r13, %rdi
callq 0x664c
movq %r13, %rdi
leaq 0x28(%rsp), %rsi
callq 0x681f
cvtsi2sd %r14d, %xmm0
decl %r14d
cvtsi2sd %r14d, %xmm2
movsd 0x2857(%rip), %xmm1     # 0x8c28
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 0xe8(%rsp), %rbx
xorpd %xmm1, %xmm1
xorps %xmm3, %xmm3
xorps %xmm6, %xmm6
xorps %xmm7, %xmm7
movq %rbx, %rdi
callq 0x758c
leaq 0x18(%rsp), %r14
movq %rbx, (%r14)
leaq 0x28(%rsp), %rax
movq %rax, 0x8(%r14)
leaq 0x88(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
callq 0x7d32
movq %rbx, %rdi
movq 0x1e0(%rsp), %r15
movq %r15, %rsi
movq 0x1e8(%rsp), %rbx
movq %rbx, %rdx
callq 0x6a4d
pushq $0x2
popq %rdx
movq %r15, %rdi
movq %rdx, %rsi
callq 0x5bba
movq %r15, %rdi
movq %rax, %rsi
callq 0x7d76
movq %rbx, (%r14)
movq 0x10(%rsp), %rax
movq %rax, 0x8(%r14)
leaq 0x18(%rsp), %rsi
movq 0x1f0(%rsp), %rdi
callq 0x7dd1
addq $0x1a8, %rsp            # imm = 0x1A8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq