// File: tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelImage.cpp
// Function: gaps::R3SurfelImage::TransformFromImageToCamera(gaps::R2Point const&, double) const
R3Point R3SurfelImage::
TransformFromImageToCamera(const R2Point& image_position, RNLength depth) const
{
  // Check camera parameters
  if ((xfocal <= 0) || (yfocal <= 0)) return R3unknown_point;

  // Undistort image position
  R2Point undistorted_image_position = UndistortImagePosition(image_position);
  if (undistorted_image_position == R2unknown_point) return R3unknown_point;

  // Get depth
  if (depth < 0) {
    if (channels.NEntries() <= R3_SURFEL_DEPTH_CHANNEL) return R3unknown_point;
    if (!channels[R3_SURFEL_DEPTH_CHANNEL]) return R3unknown_point;
    int ix = image_position.X() + 0.5;
    int iy = image_position.Y() + 0.5;
    depth = channels[R3_SURFEL_DEPTH_CHANNEL]->GridValue(ix, iy);
  }

  // Backproject from pixels into camera coordinate system
  const R2Point c = ImageCenter();
  RNCoord x = (undistorted_image_position.X() - c.X()) * depth / xfocal;
  RNCoord y = (undistorted_image_position.Y() - c.Y()) * depth / yfocal;
  return R3Point(x, y, -depth);
}

// Assembly:
subq $0x88, %rsp
movq %rdi, 0x18(%rsp)
movq %rdi, %rax
movq %rax, 0x20(%rsp)
movq %rdi, 0x80(%rsp)
movq %rsi, 0x78(%rsp)
movq %rdx, 0x70(%rsp)
movsd %xmm0, 0x68(%rsp)
movq 0x78(%rsp), %rax
movq %rax, 0x28(%rsp)
xorps %xmm0, %xmm0
ucomisd 0xb0(%rax), %xmm0
jae 0x6a155
movq 0x28(%rsp), %rax
xorps %xmm0, %xmm0
ucomisd 0xb8(%rax), %xmm0
jb 0x6a16b
movq 0x18(%rsp), %rdi
leaq 0x594027(%rip), %rsi    # 0x5fe188
callq 0xf8390
jmp 0x6a341
movq 0x28(%rsp), %rsi
movq 0x70(%rsp), %rdx
leaq 0x58(%rsp), %rdi
callq 0x6a350
leaq 0x58(%rsp), %rdi
leaq 0x594d7d(%rip), %rsi    # 0x5fef08
callq 0x134360
cmpl $0x0, %eax
je 0x6a1ab
movq 0x18(%rsp), %rdi
leaq 0x593fe7(%rip), %rsi    # 0x5fe188
callq 0xf8390
jmp 0x6a341
xorps %xmm0, %xmm0
ucomisd 0x68(%rsp), %xmm0
jbe 0x6a277
movq 0x28(%rsp), %rdi
addq $0x20, %rdi
callq 0x16d50
cmpl $0x3, %eax
jg 0x6a1e3
movq 0x18(%rsp), %rdi
leaq 0x593faf(%rip), %rsi    # 0x5fe188
callq 0xf8390
jmp 0x6a341
movq 0x28(%rsp), %rdi
addq $0x20, %rdi
movl $0x3, %esi
callq 0x45310
cmpq $0x0, %rax
jne 0x6a212
movq 0x18(%rsp), %rdi
leaq 0x593f80(%rip), %rsi    # 0x5fe188
callq 0xf8390
jmp 0x6a341
movq 0x70(%rsp), %rdi
callq 0x308e0
movsd 0x139f34(%rip), %xmm1   # 0x1a4158
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x54(%rsp)
movq 0x70(%rsp), %rdi
callq 0x30910
movq 0x28(%rsp), %rdi
movsd 0x139f11(%rip), %xmm1   # 0x1a4158
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x50(%rsp)
addq $0x20, %rdi
movl $0x3, %esi
callq 0x45310
movq %rax, %rdi
movl 0x54(%rsp), %esi
movl 0x50(%rsp), %edx
callq 0x45350
movsd %xmm0, 0x68(%rsp)
movq 0x28(%rsp), %rdi
callq 0x31580
movq %rax, %rsi
leaq 0x40(%rsp), %rdi
callq 0x133990
leaq 0x58(%rsp), %rdi
callq 0x308e0
movsd %xmm0, 0x8(%rsp)
leaq 0x40(%rsp), %rdi
callq 0x308e0
movq 0x28(%rsp), %rax
movaps %xmm0, %xmm1
movsd 0x8(%rsp), %xmm0
subsd %xmm1, %xmm0
mulsd 0x68(%rsp), %xmm0
divsd 0xb0(%rax), %xmm0
movsd %xmm0, 0x38(%rsp)
leaq 0x58(%rsp), %rdi
callq 0x30910
movsd %xmm0, 0x10(%rsp)
leaq 0x40(%rsp), %rdi
callq 0x30910
movq 0x28(%rsp), %rax
movq 0x18(%rsp), %rdi
movaps %xmm0, %xmm1
movsd 0x10(%rsp), %xmm0
subsd %xmm1, %xmm0
mulsd 0x68(%rsp), %xmm0
divsd 0xb8(%rax), %xmm0
movsd %xmm0, 0x30(%rsp)
movsd 0x38(%rsp), %xmm0
movsd 0x30(%rsp), %xmm1
movsd 0x68(%rsp), %xmm2
movq %xmm2, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm2
callq 0xf8350
movq 0x20(%rsp), %rax
addq $0x88, %rsp
retq
nop