// File: /workspace/github_cpp_makefile_star2_O1/devernay[P]vlpovutils/motionfield.cpp
// Function: vlpov_motionfield(VLPovCamera const&, VLPovImage<double> const&, VLPovCamera const&, VLPovImage<double>&, VLPovImage<double>&)
void vlpov_motionfield(const VLPovCamera &cam1, const VLPovImage<double> &z1, const VLPovCamera &cam2, VLPovImage<double> &m1x, VLPovImage<double> &m1y)
{
    assert(cam1.width == z1.width() && cam1.height == z1.height());
    m1x.resize(cam1.width, cam1.height);
    m1y.resize(cam1.width, cam1.height);
    
    double fx = cam1.K(0,0);
    double fy = cam1.K(1,1);
    double s = cam1.K(0,1);
    double cx = cam1.K(0,2);
    double cy = cam1.K(1,2);
    
    // compute inverse intrinsic matrix;
    double ifx = 1./fx;
    double ify = 1./fy;
    double is = -s/(fx*fy);
    double icx = is*cy -cx/fx;
    double icy = -cy/fy;
    
    for(int y1 = 0; y1 < z1.height(); ++y1) {
        for(int x1 = 0; x1 < z1.width(); ++x1) {
            // apply inverse intrisics
            double x1i = ifx*x1 + is*y1 + icx;
            double y1i = ify*y1 + icy;

            // compute the 3-D reconstruction P1 in camera 1 reference frame
            double Z1 = z1(x1,y1);
            vector3 P1 = vector3_from_coeffs(x1i * Z1, y1i * Z1, Z1);
#           ifdef DEBUG
            // project
            vector3 p1 = prod(cam1.K , P1);
            // compute coordinates
            double x1p = p1(0)/p1(2);
            double y1p = p1(1)/p1(2);
            assert(std::abs(x1p - x1) < 1e-3 && std::abs(y1p - y1) < 1e-3);
#           endif
            // reconstruct in the global reference frame
            vector3 P = prod(trans(cam1.R),P1-cam1.t);
            // reconstruct in camera 2 reference frame
            vector3 P2 = prod(cam2.R,P)+cam2.t;
            // project
            vector3 p2 = prod(cam2.K , P2);
            // compute coordinates
            double x2 = p2(0)/p2(2);
            double y2 = p2(1)/p2(2);
            // compute motion field
            m1x(x1,y1) = x2-x1;
            m1y(x1,y1) = y2-y1;
        }
    }
    
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x178, %rsp            # imm = 0x178
movq %rsi, %rax
movl (%rdi), %esi
cmpl (%rax), %esi
jne 0x52ec
movq %rdx, %r14
movq %rdi, %r15
movl 0x4(%rdi), %edx
cmpl 0x4(%rax), %edx
jne 0x52ec
movq %r8, %rbx
movq %rcx, 0x50(%rsp)
movq %rcx, %rdi
movq %rax, 0x28(%rsp)
callq 0x530c
movl (%r15), %esi
movl 0x4(%r15), %edx
movq %rbx, 0x88(%rsp)
movq %rbx, %rdi
callq 0x530c
leaq 0x108(%r15), %rbx
movq 0xf8(%r15), %rsi
movq 0x100(%r15), %rcx
xorl %edi, %edi
xorl %edx, %edx
callq 0x5830
movq %rbx, %rdi
movq %rax, %rsi
callq 0x56f2
movsd (%rax), %xmm0
movsd %xmm0, 0x8(%rsp)
movq 0xf8(%r15), %rsi
movq 0x100(%r15), %rcx
movl $0x1, %edi
movl $0x1, %edx
callq 0x5830
movq %rbx, %rdi
movq %rax, %rsi
callq 0x56f2
movsd (%rax), %xmm0
movsd %xmm0, 0x18(%rsp)
movq 0xf8(%r15), %rsi
movq 0x100(%r15), %rcx
movl $0x1, %edx
xorl %edi, %edi
callq 0x5830
movq %rbx, %rdi
movq %rax, %rsi
callq 0x56f2
movq 0xf8(%r15), %rsi
movq 0x100(%r15), %rcx
movsd (%rax), %xmm0
movaps %xmm0, 0x30(%rsp)
movl $0x2, %edx
xorl %edi, %edi
callq 0x5830
movq %rbx, %rdi
movq %rax, %rsi
callq 0x56f2
movsd (%rax), %xmm0
movsd %xmm0, 0x10(%rsp)
movq 0xf8(%r15), %rsi
movq %r15, 0x20(%rsp)
movq 0x100(%r15), %rcx
movl $0x1, %edi
movl $0x2, %edx
callq 0x5830
movq %rbx, %rdi
movq %rax, %rsi
callq 0x56f2
xorl %esi, %esi
movq 0x28(%rsp), %rdx
cmpl $0x0, 0x4(%rdx)
jle 0x52da
movsd (%rax), %xmm2
movsd 0x603c(%rip), %xmm0     # 0xb008
movapd %xmm0, %xmm3
movsd 0x8(%rsp), %xmm4
divsd %xmm4, %xmm3
movsd 0x18(%rsp), %xmm5
divsd %xmm5, %xmm0
movsd %xmm0, 0x48(%rsp)
movapd 0x64de(%rip), %xmm0     # 0xb4d0
movapd 0x30(%rsp), %xmm1
xorpd %xmm0, %xmm1
movsd 0x10(%rsp), %xmm6
divsd %xmm4, %xmm6
mulsd %xmm5, %xmm4
divsd %xmm4, %xmm1
movapd %xmm1, 0x30(%rsp)
mulsd %xmm2, %xmm1
subsd %xmm6, %xmm1
xorpd %xmm0, %xmm2
divsd %xmm5, %xmm2
movapd %xmm2, 0xc0(%rsp)
movl $0x158, %ecx            # imm = 0x158
movq 0x20(%rsp), %rax
leaq (%rax,%rcx), %rdi
movq %rdi, 0x68(%rsp)
movl $0x1b8, %edi            # imm = 0x1B8
addq %rdi, %rax
movq %rax, 0x58(%rsp)
addq %r14, %rcx
movq %rcx, 0x70(%rsp)
addq %r14, %rdi
movq %rdi, 0x60(%rsp)
addq $0xf8, %r14
movq %r14, 0x80(%rsp)
leaq 0xf8(%rsp), %rbx
movsd %xmm3, 0x78(%rsp)
movapd %xmm1, 0xd0(%rsp)
movq 0x50(%rsp), %r14
movl (%rdx), %eax
testl %eax, %eax
leaq 0x118(%rsp), %r12
leaq 0xb0(%rsp), %rbp
jle 0x52cf
xorps %xmm0, %xmm0
cvtsi2sd %esi, %xmm0
movapd 0x30(%rsp), %xmm2
mulsd %xmm0, %xmm2
movapd %xmm2, 0xe0(%rsp)
movsd 0x48(%rsp), %xmm2
movsd %xmm0, 0xa8(%rsp)
mulsd %xmm0, %xmm2
addsd 0xc0(%rsp), %xmm2
movsd %xmm2, 0x98(%rsp)
movslq %esi, %rcx
movq %rcx, 0x90(%rsp)
xorl %r15d, %r15d
movq %rsi, 0xa0(%rsp)
xorps %xmm2, %xmm2
cvtsi2sd %r15d, %xmm2
movsd %xmm2, 0x18(%rsp)
movapd %xmm3, %xmm0
mulsd %xmm2, %xmm0
addsd 0xe0(%rsp), %xmm0
addsd %xmm1, %xmm0
movq 0x8(%rdx), %rcx
imull %esi, %eax
cltq
addq %r15, %rax
movsd (%rcx,%rax,8), %xmm2
mulsd %xmm2, %xmm0
movsd 0x98(%rsp), %xmm1
mulsd %xmm2, %xmm1
leaq 0x138(%rsp), %rdi
callq 0x535c
movq 0x68(%rsp), %rax
movq %rax, 0x118(%rsp)
leaq 0x138(%rsp), %rax
movq %rax, 0x120(%rsp)
movq 0x58(%rsp), %rax
movq %rax, 0x128(%rsp)
movq 0x20(%rsp), %rax
movq 0x160(%rax), %rsi
movq %rbx, %r13
leaq 0x158(%rsp), %rbx
movq %rbx, %rdi
callq 0x5500
movq %rbx, %rdi
movq %r12, %rsi
callq 0x5b83
movq 0x70(%rsp), %rax
movq %rax, 0xf8(%rsp)
movq %rbx, 0x100(%rsp)
movq %r13, %rbx
movq 0x60(%rsp), %rax
movq %rax, 0x108(%rsp)
movq %r12, %rdi
movq %r13, %rsi
callq 0x6026
movq 0x80(%rsp), %rax
movq %rax, 0xb0(%rsp)
movq %r12, 0xb8(%rsp)
movq (%rax), %rsi
movq %r13, %rdi
callq 0x5500
movq %r13, %rdi
movq %rbp, %rsi
callq 0x6214
movq %r13, %rdi
xorl %esi, %esi
callq 0x53c2
movsd (%rax), %xmm0
movsd %xmm0, 0x8(%rsp)
movl $0x2, %esi
movq %r13, %rdi
callq 0x53c2
movsd 0x8(%rsp), %xmm0
divsd (%rax), %xmm0
movsd %xmm0, 0x8(%rsp)
movl $0x1, %esi
movq %r13, %rdi
callq 0x53c2
movsd (%rax), %xmm0
movsd %xmm0, 0x10(%rsp)
movl $0x2, %esi
movq %r13, %rdi
callq 0x53c2
movq 0x88(%rsp), %rdi
movapd 0xd0(%rsp), %xmm1
movsd 0x78(%rsp), %xmm3
movq 0xa0(%rsp), %rsi
movq 0x28(%rsp), %rdx
movsd 0x10(%rsp), %xmm2
divsd (%rax), %xmm2
movsd 0x8(%rsp), %xmm0
subsd 0x18(%rsp), %xmm0
movq 0x8(%r14), %rax
movslq (%r14), %rcx
movq 0x90(%rsp), %r8
imulq %r8, %rcx
addq %r15, %rcx
movsd %xmm0, (%rax,%rcx,8)
subsd 0xa8(%rsp), %xmm2
movq 0x8(%rdi), %rax
movslq (%rdi), %rcx
imulq %r8, %rcx
addq %r15, %rcx
movsd %xmm2, (%rax,%rcx,8)
movl (%rdx), %eax
incq %r15
cmpl %eax, %r15d
jl 0x50f7
incl %esi
cmpl 0x4(%rdx), %esi
jl 0x5088
addq $0x178, %rsp            # imm = 0x178
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
leaq 0x61ed(%rip), %rdi      # 0xb4e0
leaq 0x621d(%rip), %rsi      # 0xb517
leaq 0x6226(%rip), %rcx      # 0xb527
movl $0xf, %edx
callq 0x20d0
nop