// File: tomfunkhouser[P]gaps/pkgs/GSV/GSVImage.cpp
// Function: gaps::GSVImage::RayThroughUndistortedPosition(gaps::R2Point const&) const
R3Ray GSVImage::
RayThroughUndistortedPosition(const R2Point& undistorted_image_position) const
{
  // Check camera
  if (!Camera()) return R3null_ray;

  // Compute distorted image position
  R2Point distorted_image_position = DistortedPosition(undistorted_image_position);
  if (distorted_image_position == R2Point(-1,-1)) return R3null_ray;
  
  // Get column index
  int column_index = (int) (distorted_image_position.X() + 0.5);
  if (column_index < 0) column_index = 0;
  if (column_index >= Width()) column_index = Width()-1;
      
  // Get camera pose
  GSVPose pose = Pose(column_index);
  const R3Point& viewpoint = pose.Viewpoint();
  const R3Vector& towards = pose.Towards();
  const R3Vector& up = pose.Up();
  R3Vector right = towards % up;

  // Compute point on ray
  RNScalar dx = (RNScalar) (2 * (undistorted_image_position.X() - 0.5 * Width())) / (RNScalar) Width();
  RNScalar dy = (RNScalar) (2 * (undistorted_image_position.Y() - 0.5 * Height())) / (RNScalar) Height();
  R3Point world_point = viewpoint + towards;
  world_point += dx * right * tan(0.5 * Camera()->XFov());
  world_point += dy * up * tan(0.5 * Camera()->YFov());

  // Return ray through undistorted image point
  return R3Ray(viewpoint, world_point);
}

// Assembly:
subq $0x1f8, %rsp            # imm = 0x1F8
movq %rdi, 0x68(%rsp)
movq %rdi, %rax
movq %rax, 0x70(%rsp)
movq %rdi, 0x1f0(%rsp)
movq %rsi, 0x1e8(%rsp)
movq %rdx, 0x1e0(%rsp)
movq 0x1e8(%rsp), %rdi
movq %rdi, 0x78(%rsp)
callq 0x4f700
cmpq $0x0, %rax
jne 0x5118a
movq 0x68(%rsp), %rdi
leaq 0x6b1e80(%rip), %rsi    # 0x703000
callq 0x17f650
jmp 0x5156c
movq 0x78(%rsp), %rsi
movq 0x1e0(%rsp), %rdx
leaq 0x1d0(%rsp), %rdi
callq 0x506e0
leaq 0x1c0(%rsp), %rdi
movsd 0x214e54(%rip), %xmm1   # 0x266008
movaps %xmm1, %xmm0
callq 0x1b9a30
leaq 0x1d0(%rsp), %rdi
leaq 0x1c0(%rsp), %rsi
callq 0x1ba430
cmpl $0x0, %eax
je 0x511ec
movq 0x68(%rsp), %rdi
leaq 0x6b1e1e(%rip), %rsi    # 0x703000
callq 0x17f650
jmp 0x5156c
leaq 0x1d0(%rsp), %rdi
callq 0x39990
movsd 0x214e4f(%rip), %xmm1   # 0x266050
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x1bc(%rsp)
cmpl $0x0, 0x1bc(%rsp)
jge 0x51225
movl $0x0, 0x1bc(%rsp)
movq 0x78(%rsp), %rdi
movl 0x1bc(%rsp), %eax
movl %eax, 0x64(%rsp)
callq 0x4fa70
movl %eax, %ecx
movl 0x64(%rsp), %eax
cmpl %ecx, %eax
jl 0x51258
movq 0x78(%rsp), %rdi
callq 0x4fa70
subl $0x1, %eax
movl %eax, 0x1bc(%rsp)
movq 0x78(%rsp), %rsi
movl 0x1bc(%rsp), %edx
leaq 0x178(%rsp), %rdi
movq %rdi, 0x50(%rsp)
callq 0x4faf0
movq 0x50(%rsp), %rdi
callq 0x3a330
movq %rax, 0x58(%rsp)
jmp 0x51287
movq 0x58(%rsp), %rax
movq %rax, 0x170(%rsp)
leaq 0x140(%rsp), %rdi
leaq 0x178(%rsp), %rsi
callq 0x5e6c0
jmp 0x512ab
leaq 0x140(%rsp), %rax
movq %rax, 0x158(%rsp)
leaq 0x120(%rsp), %rdi
leaq 0x178(%rsp), %rsi
callq 0x5e730
jmp 0x512d2
leaq 0x120(%rsp), %rax
movq %rax, 0x138(%rsp)
movq 0x158(%rsp), %rsi
movq 0x138(%rsp), %rdx
leaq 0x108(%rsp), %rdi
callq 0x181d30
jmp 0x51301
movq 0x1e0(%rsp), %rdi
callq 0x39990
movsd %xmm0, 0x48(%rsp)
jmp 0x51316
movq 0x78(%rsp), %rdi
callq 0x4fa70
movl %eax, 0x44(%rsp)
jmp 0x51326
movq 0x78(%rsp), %rdi
movsd 0x48(%rsp), %xmm1
movl 0x44(%rsp), %eax
cvtsi2sd %eax, %xmm0
movsd 0x214d97(%rip), %xmm2   # 0x2660d8
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
addsd %xmm0, %xmm0
movsd %xmm0, 0x38(%rsp)
callq 0x4fa70
movl %eax, 0x40(%rsp)
jmp 0x5135e
movsd 0x38(%rsp), %xmm0
movl 0x40(%rsp), %eax
cvtsi2sd %eax, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, 0x100(%rsp)
movq 0x1e0(%rsp), %rdi
callq 0x399a0
movsd %xmm0, 0x30(%rsp)
jmp 0x5138e
movq 0x78(%rsp), %rdi
callq 0x4fab0
movl %eax, 0x2c(%rsp)
jmp 0x5139e
movq 0x78(%rsp), %rdi
movsd 0x30(%rsp), %xmm1
movl 0x2c(%rsp), %eax
cvtsi2sd %eax, %xmm0
movsd 0x214d1f(%rip), %xmm2   # 0x2660d8
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
addsd %xmm0, %xmm0
movsd %xmm0, 0x20(%rsp)
callq 0x4fab0
movl %eax, 0x28(%rsp)
jmp 0x513d6
movsd 0x20(%rsp), %xmm0
movl 0x28(%rsp), %eax
cvtsi2sd %eax, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, 0xf8(%rsp)
movq 0x170(%rsp), %rsi
movq 0x158(%rsp), %rdx
leaq 0xe0(%rsp), %rdi
callq 0x180350
jmp 0x51410
movsd 0x100(%rsp), %xmm0
leaq 0xb0(%rsp), %rdi
leaq 0x108(%rsp), %rsi
callq 0x38f20
jmp 0x51430
movq 0x78(%rsp), %rdi
callq 0x4f700
movq %rax, 0x18(%rsp)
jmp 0x51441
movq 0x18(%rsp), %rdi
callq 0x51770
movsd %xmm0, 0x10(%rsp)
jmp 0x51453
movsd 0x10(%rsp), %xmm0
movsd 0x214bef(%rip), %xmm1   # 0x266050
mulsd %xmm1, %xmm0
callq 0x299c0
leaq 0xc8(%rsp), %rdi
leaq 0xb0(%rsp), %rsi
callq 0x181dc0
jmp 0x51481
leaq 0xe0(%rsp), %rdi
leaq 0xc8(%rsp), %rsi
callq 0x1804d0
jmp 0x51498
movsd 0xf8(%rsp), %xmm0
movq 0x138(%rsp), %rsi
leaq 0x80(%rsp), %rdi
callq 0x38f20
jmp 0x514b8
movq 0x78(%rsp), %rdi
callq 0x4f700
movq %rax, 0x8(%rsp)
jmp 0x514c9
movq 0x8(%rsp), %rdi
callq 0x51800
movsd %xmm0, (%rsp)
jmp 0x514da
movsd (%rsp), %xmm0
movsd 0x214b69(%rip), %xmm1   # 0x266050
mulsd %xmm1, %xmm0
callq 0x299c0
leaq 0x98(%rsp), %rdi
leaq 0x80(%rsp), %rsi
callq 0x181dc0
jmp 0x51507
leaq 0xe0(%rsp), %rdi
leaq 0x98(%rsp), %rsi
callq 0x1804d0
jmp 0x5151e
movq 0x68(%rsp), %rdi
movq 0x170(%rsp), %rsi
leaq 0xe0(%rsp), %rdx
callq 0x17f6c0
jmp 0x5153a
leaq 0x178(%rsp), %rdi
callq 0x5e680
jmp 0x5156c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x168(%rsp)
movl %eax, 0x164(%rsp)
leaq 0x178(%rsp), %rdi
callq 0x5e680
jmp 0x51579
movq 0x70(%rsp), %rax
addq $0x1f8, %rsp            # imm = 0x1F8
retq
movq 0x168(%rsp), %rdi
callq 0x29c10
nopw %cs:(%rax,%rax)