// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/RGBD/RGBDUtil.cpp
// Function: gaps::RGBDCreatePositionChannels(gaps::R2Grid const&, gaps::R2Grid&, gaps::R2Grid&, gaps::R2Grid&, gaps::R3Matrix const&, gaps::R4Matrix const&)
int RGBDCreatePositionChannels(
  const R2Grid& input_undistorted_depth_image,
  R2Grid& output_px_image, R2Grid& output_py_image, R2Grid& output_pz_image,
  const R3Matrix& intrinsics_matrix, const R4Matrix& camera_to_world_matrix)
{
  // Initialize position images
  output_px_image = input_undistorted_depth_image; 
  output_px_image.Clear(R2_GRID_UNKNOWN_VALUE);
  output_py_image = output_px_image;
  output_pz_image = output_px_image;

  // Fill position images
  for (int i = 0; i < input_undistorted_depth_image.XResolution(); i++) {
    for (int j = 0; j < input_undistorted_depth_image.YResolution(); j++) {
      // Get depth
      RNScalar depth = input_undistorted_depth_image.GridValue(i, j);
      if (RNIsNegativeOrZero(depth)) continue;

      // Get position in camera coordinate system 
      RNScalar x = ((i+0.5) - intrinsics_matrix[0][2]) * depth / intrinsics_matrix[0][0];
      RNScalar y = ((j+0.5) - intrinsics_matrix[1][2]) * depth / intrinsics_matrix[1][1];
      RNScalar z = -depth;
      R3Point position(x, y, z);

      // Transform by extrinsics matrix
      position = camera_to_world_matrix * position;

      // Fill position images
      output_px_image.SetGridValue(i, j, position.X());
      output_py_image.SetGridValue(i, j, position.Y());
      output_pz_image.SetGridValue(i, j, position.Z());
    }
  }

  // Return success 
  return 1;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x58, %rsp
movq %r9, 0x18(%rsp)
movq %r8, %r14
movq %rcx, %rbx
movq %rdx, %r15
movq %rsi, %r12
movq %rdi, %rbp
movq %rsi, %rdi
movq %rbp, %rsi
callq 0x5ffa2
movsd 0x99bd6(%rip), %xmm0    # 0xbb018
movq %r12, %rdi
callq 0x617b0
movq %r15, 0x8(%rsp)
movq %r15, %rdi
movq %r12, %rsi
callq 0x5ffa2
movq %rbx, 0x10(%rsp)
movq %rbx, %rdi
movq %r12, (%rsp)
movq %r12, %rsi
callq 0x5ffa2
movl 0xc0(%rbp), %ecx
testl %ecx, %ecx
jle 0x215dc
movl 0xc4(%rbp), %eax
xorl %ebx, %ebx
testl %eax, %eax
jle 0x215d2
xorps %xmm0, %xmm0
cvtsi2sd %ebx, %xmm0
addsd 0x99b85(%rip), %xmm0    # 0xbb020
movsd %xmm0, 0x20(%rsp)
leaq 0xe3830(%rip), %rcx     # 0x104cd8
movsd (%rcx), %xmm0
xorl %r15d, %r15d
movq 0xb8(%rbp), %rcx
movl 0xc8(%rbp), %edx
imull %r15d, %edx
addl %ebx, %edx
movslq %edx, %rdx
movsd (%rcx,%rdx,8), %xmm2
ucomisd %xmm2, %xmm0
jae 0x215c0
movsd 0x20(%rsp), %xmm0
subsd 0x10(%r14), %xmm0
mulsd %xmm2, %xmm0
divsd (%r14), %xmm0
xorps %xmm1, %xmm1
cvtsi2sd %r15d, %xmm1
addsd 0x99b27(%rip), %xmm1    # 0xbb020
subsd 0x28(%r14), %xmm1
mulsd %xmm2, %xmm1
divsd 0x20(%r14), %xmm1
xorpd 0x99b3f(%rip), %xmm2    # 0xbb050
leaq 0x28(%rsp), %r12
movq %r12, %rdi
callq 0x4b602
leaq 0x40(%rsp), %r13
movq %r13, %rdi
movq 0x18(%rsp), %rsi
movq %r12, %rdx
callq 0x49ffa
movq %r12, %rdi
movq %r13, %rsi
callq 0x4b96a
movsd 0x28(%rsp), %xmm0
movq (%rsp), %rcx
movq 0xb8(%rcx), %rax
movl 0xc8(%rcx), %ecx
imull %r15d, %ecx
addl %ebx, %ecx
movslq %ecx, %rcx
movsd %xmm0, (%rax,%rcx,8)
movsd 0x30(%rsp), %xmm0
movq 0x8(%rsp), %rcx
movq 0xb8(%rcx), %rax
movl 0xc8(%rcx), %ecx
imull %r15d, %ecx
addl %ebx, %ecx
movslq %ecx, %rcx
movsd %xmm0, (%rax,%rcx,8)
movsd 0x38(%rsp), %xmm0
movq 0x10(%rsp), %rcx
movq 0xb8(%rcx), %rax
movl 0xc8(%rcx), %ecx
imull %r15d, %ecx
addl %ebx, %ecx
movslq %ecx, %rcx
movsd %xmm0, (%rax,%rcx,8)
leaq 0xe3722(%rip), %rax     # 0x104cd8
movsd (%rax), %xmm0
movl 0xc4(%rbp), %eax
incl %r15d
cmpl %eax, %r15d
jl 0x214af
movl 0xc0(%rbp), %ecx
incl %ebx
cmpl %ecx, %ebx
jl 0x21484
movl $0x1, %eax
addq $0x58, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq