// File: 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
movq %rsp, %rbp
subq $0x210, %rsp            # imm = 0x210
movq 0x20(%rbp), %rax
movq 0x18(%rbp), %rax
movq 0x10(%rbp), %rax
movl %edi, -0x4(%rbp)
movl %esi, -0x8(%rbp)
movq %rdx, -0x10(%rbp)
movq %rcx, -0x18(%rbp)
movq %r8, -0x20(%rbp)
movq %r9, -0x28(%rbp)
leaq -0x88(%rbp), %rdi
callq 0x6190
movq -0x28(%rbp), %rsi
movq -0x20(%rbp), %rdx
movq -0x18(%rbp), %rcx
leaq -0xe8(%rbp), %rdi
callq 0xcfa0
jmp 0xccc3
leaq -0xe8(%rbp), %rdi
callq 0xd300
movsd %xmm0, -0x200(%rbp)
jmp 0xccd9
movsd -0x200(%rbp), %xmm0
movsd %xmm0, -0x100(%rbp)
movsd -0x100(%rbp), %xmm0
leaq -0xe8(%rbp), %rdi
leaq -0x88(%rbp), %rsi
callq 0xd580
jmp 0xcd06
movl -0x4(%rbp), %eax
cvtsi2sd %eax, %xmm0
decl %eax
cvtsi2sd %eax, %xmm2
movsd 0x5a55(%rip), %xmm1     # 0x12770
mulsd %xmm1, %xmm2
movl -0x8(%rbp), %eax
movl %eax, %ecx
negl %ecx
cvtsi2sd %ecx, %xmm4
decl %eax
cvtsi2sd %eax, %xmm5
mulsd %xmm1, %xmm5
movq %rsp, %rax
movabsq $0x3ff0000000000000, %rcx # imm = 0x3FF0000000000000
movq %rcx, (%rax)
leaq -0x160(%rbp), %rdi
xorps %xmm7, %xmm7
movaps %xmm7, %xmm1
movaps %xmm7, %xmm3
movaps %xmm7, %xmm6
callq 0xd800
jmp 0xcd5e
leaq -0x1d0(%rbp), %rdi
leaq -0x160(%rbp), %rsi
leaq -0x88(%rbp), %rdx
callq 0xd890
jmp 0xcd7a
leaq -0x1c0(%rbp), %rdi
leaq -0x1d0(%rbp), %rsi
callq 0xd8d0
jmp 0xcd8f
leaq -0x1d0(%rbp), %rdi
callq 0xd900
movq 0x10(%rbp), %rsi
movq 0x18(%rbp), %rdx
leaq -0x1c0(%rbp), %rdi
callq 0xd940
jmp 0xcdb1
movq 0x10(%rbp), %rdi
movl $0x2, %edx
movq %rdx, %rsi
callq 0xb0b0
movq %rax, -0x208(%rbp)
jmp 0xcdcb
movq -0x208(%rbp), %rsi
movq 0x10(%rbp), %rdi
callq 0xf4e0
jmp 0xcddd
movq 0x18(%rbp), %rsi
movq -0x10(%rbp), %rdx
leaq -0x1f0(%rbp), %rdi
callq 0x7ad0
jmp 0xcdf3
leaq -0x1e0(%rbp), %rdi
leaq -0x1f0(%rbp), %rsi
callq 0xf510
jmp 0xce08
movq 0x20(%rbp), %rsi
leaq -0x1f8(%rbp), %rdi
callq 0xf550
jmp 0xce1a
leaq -0x1f8(%rbp), %rdi
leaq -0x1e0(%rbp), %rsi
callq 0xf580
jmp 0xce2f
leaq -0x1f8(%rbp), %rdi
callq 0xf5b0
leaq -0x1e0(%rbp), %rdi
callq 0xf5e0
leaq -0x1f0(%rbp), %rdi
callq 0x7b80
leaq -0x1c0(%rbp), %rdi
callq 0x61b0
leaq -0x160(%rbp), %rdi
callq 0x61b0
leaq -0xe8(%rbp), %rdi
callq 0x61b0
leaq -0x88(%rbp), %rdi
callq 0x61b0
addq $0x210, %rsp            # imm = 0x210
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xf0(%rbp)
movl %eax, -0xf4(%rbp)
jmp 0xcf87
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xf0(%rbp)
movl %eax, -0xf4(%rbp)
jmp 0xcf7b
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xf0(%rbp)
movl %eax, -0xf4(%rbp)
jmp 0xcf6f
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xf0(%rbp)
movl %eax, -0xf4(%rbp)
leaq -0x1d0(%rbp), %rdi
callq 0xd900
jmp 0xcf6f
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xf0(%rbp)
movl %eax, -0xf4(%rbp)
jmp 0xcf63
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xf0(%rbp)
movl %eax, -0xf4(%rbp)
jmp 0xcf57
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xf0(%rbp)
movl %eax, -0xf4(%rbp)
jmp 0xcf4b
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xf0(%rbp)
movl %eax, -0xf4(%rbp)
leaq -0x1f8(%rbp), %rdi
callq 0xf5b0
leaq -0x1e0(%rbp), %rdi
callq 0xf5e0
leaq -0x1f0(%rbp), %rdi
callq 0x7b80
leaq -0x1c0(%rbp), %rdi
callq 0x61b0
leaq -0x160(%rbp), %rdi
callq 0x61b0
leaq -0xe8(%rbp), %rdi
callq 0x61b0
leaq -0x88(%rbp), %rdi
callq 0x61b0
movq -0xf0(%rbp), %rdi
callq 0x33e0
nop