// File: 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:
subq $0xe8, %rsp
movq %rdi, 0xe0(%rsp)
movq %rsi, 0xd8(%rsp)
movq %rdx, 0xd0(%rsp)
movq %rcx, 0xc8(%rsp)
movq %r8, 0xc0(%rsp)
movq %r9, 0xb8(%rsp)
movq 0xe0(%rsp), %rsi
movq 0xd8(%rsp), %rdi
callq 0xb2f20
movq 0xd8(%rsp), %rdi
movsd 0x1355b4(%rip), %xmm0   # 0x166020
callq 0xb5a20
movq 0xd8(%rsp), %rsi
movq 0xd0(%rsp), %rdi
callq 0xb2f20
movq 0xd8(%rsp), %rsi
movq 0xc8(%rsp), %rdi
callq 0xb2f20
movl $0x0, 0xb4(%rsp)
movl 0xb4(%rsp), %eax
movl %eax, 0x5c(%rsp)
movq 0xe0(%rsp), %rdi
callq 0x27940
movl %eax, %ecx
movl 0x5c(%rsp), %eax
cmpl %ecx, %eax
jge 0x30d76
movl $0x0, 0xb0(%rsp)
movl 0xb0(%rsp), %eax
movl %eax, 0x58(%rsp)
movq 0xe0(%rsp), %rdi
callq 0x27960
movl %eax, %ecx
movl 0x58(%rsp), %eax
cmpl %ecx, %eax
jge 0x30d5e
movq 0xe0(%rsp), %rdi
movl 0xb4(%rsp), %esi
movl 0xb0(%rsp), %edx
callq 0x1a060
movsd %xmm0, 0xa8(%rsp)
movsd 0xa8(%rsp), %xmm0
leaq 0x18138f(%rip), %rax    # 0x1b1ec0
movsd (%rax), %xmm1
callq 0x1a3d0
cmpl $0x0, %eax
je 0x30b44
jmp 0x30d48
cvtsi2sdl 0xb4(%rsp), %xmm0
movsd 0x1354d3(%rip), %xmm1   # 0x166028
addsd %xmm1, %xmm0
movsd %xmm0, 0x8(%rsp)
movq 0xc0(%rsp), %rdi
xorl %esi, %esi
callq 0x238a0
movsd 0x8(%rsp), %xmm0
subsd 0x10(%rax), %xmm0
mulsd 0xa8(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
movq 0xc0(%rsp), %rdi
xorl %esi, %esi
callq 0x238a0
movsd 0x10(%rsp), %xmm0
divsd (%rax), %xmm0
movsd %xmm0, 0xa0(%rsp)
cvtsi2sdl 0xb0(%rsp), %xmm0
movsd 0x13546d(%rip), %xmm1   # 0x166028
addsd %xmm1, %xmm0
movsd %xmm0, 0x18(%rsp)
movq 0xc0(%rsp), %rdi
movl $0x1, %esi
callq 0x238a0
movsd 0x18(%rsp), %xmm0
subsd 0x10(%rax), %xmm0
mulsd 0xa8(%rsp), %xmm0
movsd %xmm0, 0x20(%rsp)
movq 0xc0(%rsp), %rdi
movl $0x1, %esi
callq 0x238a0
movsd 0x20(%rsp), %xmm0
divsd 0x8(%rax), %xmm0
movsd %xmm0, 0x98(%rsp)
movsd 0xa8(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, 0x90(%rsp)
movsd 0xa0(%rsp), %xmm0
movsd 0x98(%rsp), %xmm1
movsd 0x90(%rsp), %xmm2
leaq 0x78(%rsp), %rdi
callq 0x8a9a0
movq 0xb8(%rsp), %rsi
leaq 0x60(%rsp), %rdi
leaq 0x78(%rsp), %rdx
callq 0x87960
leaq 0x78(%rsp), %rdi
leaq 0x60(%rsp), %rsi
callq 0x8b030
movq 0xd8(%rsp), %rax
movq %rax, 0x28(%rsp)
movl 0xb4(%rsp), %eax
movl %eax, 0x30(%rsp)
movl 0xb0(%rsp), %eax
movl %eax, 0x34(%rsp)
leaq 0x78(%rsp), %rdi
callq 0x19ea0
movq 0x28(%rsp), %rdi
movl 0x30(%rsp), %esi
movl 0x34(%rsp), %edx
callq 0x1a1b0
movq 0xd0(%rsp), %rax
movq %rax, 0x38(%rsp)
movl 0xb4(%rsp), %eax
movl %eax, 0x40(%rsp)
movl 0xb0(%rsp), %eax
movl %eax, 0x44(%rsp)
leaq 0x78(%rsp), %rdi
callq 0x19eb0
movq 0x38(%rsp), %rdi
movl 0x40(%rsp), %esi
movl 0x44(%rsp), %edx
callq 0x1a1b0
movq 0xc8(%rsp), %rax
movq %rax, 0x48(%rsp)
movl 0xb4(%rsp), %eax
movl %eax, 0x50(%rsp)
movl 0xb0(%rsp), %eax
movl %eax, 0x54(%rsp)
leaq 0x78(%rsp), %rdi
callq 0x19ec0
movq 0x48(%rsp), %rdi
movl 0x50(%rsp), %esi
movl 0x54(%rsp), %edx
callq 0x1a1b0
movl 0xb0(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xb0(%rsp)
jmp 0x30ad7
jmp 0x30d60
movl 0xb4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xb4(%rsp)
jmp 0x30aa6
movl $0x1, %eax
addq $0xe8, %rsp
retq
nopw %cs:(%rax,%rax)