// File: tomfunkhouser[P]gaps/pkgs/RGBD/RGBDImage.cpp
// Function: gaps::RGBDImage::TransformImageToCamera(gaps::R2Point const&, gaps::R3Point&) const
int RGBDImage::
TransformImageToCamera(const R2Point& image_position, R3Point& camera_position) const
{
  // Check image position
  int ix = (int) image_position[0];
  if ((ix < 0) || (ix >= NPixels(RN_X))) return 0; 
  int iy = (int) image_position[1];
  if ((iy < 0) || (iy >= NPixels(RN_Y))) return 0; 
  
  // Get/check depth (point sample to avoid interpolation)
  RNScalar depth = PixelDepth(ix, iy);
  if ((RNIsZero(depth)) || (depth == R2_GRID_UNKNOWN_VALUE)) return 0;

  // Get/check intrinsics matrix
  const R3Matrix& intrinsics_matrix = Intrinsics();
  if (RNIsZero(intrinsics_matrix[0][0])) return 0;
  if (RNIsZero(intrinsics_matrix[1][1])) return 0;
  
  // Transform from position in image coordinates to camera coordinates (where camera is looking down -Z, up is +Y, right is +X)
  camera_position[0] = (image_position[0] - intrinsics_matrix[0][2]) * depth / intrinsics_matrix[0][0];
  camera_position[1] = (image_position[1] - intrinsics_matrix[1][2]) * depth / intrinsics_matrix[1][1];
  camera_position[2] = -depth;

  // Return success
  return 1;
}

// Assembly:
subq $0x88, %rsp
movq %rdi, 0x78(%rsp)
movq %rsi, 0x70(%rsp)
movq %rdx, 0x68(%rsp)
movq 0x78(%rsp), %rax
movq %rax, 0x48(%rsp)
movq 0x70(%rsp), %rdi
xorl %esi, %esi
callq 0x1a280
cvttsd2si %xmm0, %eax
movl %eax, 0x64(%rsp)
cmpl $0x0, 0x64(%rsp)
jl 0x2a799
movq 0x48(%rsp), %rdi
movl 0x64(%rsp), %eax
movl %eax, 0x44(%rsp)
xorl %esi, %esi
callq 0x1af30
movl %eax, %ecx
movl 0x44(%rsp), %eax
cmpl %ecx, %eax
jl 0x2a7a9
movl $0x0, 0x84(%rsp)
jmp 0x2a9f0
movq 0x70(%rsp), %rdi
movl $0x1, %esi
callq 0x1a280
cvttsd2si %xmm0, %eax
movl %eax, 0x60(%rsp)
cmpl $0x0, 0x60(%rsp)
jl 0x2a7e8
movq 0x48(%rsp), %rdi
movl 0x60(%rsp), %eax
movl %eax, 0x40(%rsp)
movl $0x1, %esi
callq 0x1af30
movl %eax, %ecx
movl 0x40(%rsp), %eax
cmpl %ecx, %eax
jl 0x2a7f8
movl $0x0, 0x84(%rsp)
jmp 0x2a9f0
movq 0x48(%rsp), %rdi
movl 0x64(%rsp), %esi
movl 0x60(%rsp), %edx
callq 0x30570
movsd %xmm0, 0x58(%rsp)
movsd 0x58(%rsp), %xmm0
leaq 0x1876a3(%rip), %rax    # 0x1b1ec0
movsd (%rax), %xmm1
callq 0x1a370
cmpl $0x0, %eax
jne 0x2a841
movsd 0x58(%rsp), %xmm0
movsd 0x13b7e7(%rip), %xmm1   # 0x166020
ucomisd %xmm1, %xmm0
jne 0x2a851
jp 0x2a851
movl $0x0, 0x84(%rsp)
jmp 0x2a9f0
movq 0x48(%rsp), %rdi
callq 0x226a0
movq %rax, 0x50(%rsp)
movq 0x50(%rsp), %rdi
xorl %esi, %esi
callq 0x238a0
movsd (%rax), %xmm0
leaq 0x187649(%rip), %rax    # 0x1b1ec0
movsd (%rax), %xmm1
callq 0x1a370
cmpl $0x0, %eax
je 0x2a895
movl $0x0, 0x84(%rsp)
jmp 0x2a9f0
movq 0x50(%rsp), %rdi
movl $0x1, %esi
callq 0x238a0
movsd 0x8(%rax), %xmm0
leaq 0x187610(%rip), %rax    # 0x1b1ec0
movsd (%rax), %xmm1
callq 0x1a370
cmpl $0x0, %eax
je 0x2a8ce
movl $0x0, 0x84(%rsp)
jmp 0x2a9f0
movq 0x70(%rsp), %rdi
xorl %esi, %esi
callq 0x1a280
movsd %xmm0, 0x8(%rsp)
movq 0x50(%rsp), %rdi
xorl %esi, %esi
callq 0x238a0
movsd 0x8(%rsp), %xmm0
subsd 0x10(%rax), %xmm0
mulsd 0x58(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
movq 0x50(%rsp), %rdi
xorl %esi, %esi
callq 0x238a0
movsd 0x10(%rsp), %xmm0
divsd (%rax), %xmm0
movsd %xmm0, 0x18(%rsp)
movq 0x68(%rsp), %rdi
xorl %esi, %esi
callq 0x19c00
movsd 0x18(%rsp), %xmm0
movsd %xmm0, (%rax)
movq 0x70(%rsp), %rdi
movl $0x1, %esi
callq 0x1a280
movsd %xmm0, 0x20(%rsp)
movq 0x50(%rsp), %rdi
movl $0x1, %esi
callq 0x238a0
movsd 0x20(%rsp), %xmm0
subsd 0x10(%rax), %xmm0
mulsd 0x58(%rsp), %xmm0
movsd %xmm0, 0x28(%rsp)
movq 0x50(%rsp), %rdi
movl $0x1, %esi
callq 0x238a0
movsd 0x28(%rsp), %xmm0
divsd 0x8(%rax), %xmm0
movsd %xmm0, 0x30(%rsp)
movq 0x68(%rsp), %rdi
movl $0x1, %esi
callq 0x19c00
movsd 0x30(%rsp), %xmm0
movsd %xmm0, (%rax)
movsd 0x58(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, 0x38(%rsp)
movq 0x68(%rsp), %rdi
movl $0x2, %esi
callq 0x19c00
movsd 0x38(%rsp), %xmm0
movsd %xmm0, (%rax)
movl $0x1, 0x84(%rsp)
movl 0x84(%rsp), %eax
addq $0x88, %rsp
retq
nop