// File: /workspace/github_cpp_makefile_star2_O3/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:
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x100, %rsp            # imm = 0x100
movq %rdi, %rbx
movq 0x8(%rsi), %rax
testq %rax, %rax
je 0x3de2e
cmpq $0x0, 0x18(%rax)
je 0x3de2e
movq %rdx, %r12
movq %rsi, %r14
leaq 0x98(%rsp), %r15
movq %r15, %rdi
callq 0x3d898
leaq 0xa8(%rsp), %r13
movsd 0x10e25d(%rip), %xmm0   # 0x14c070
movq %r13, %rdi
movapd %xmm0, %xmm1
callq 0xf1712
movq %r15, %rdi
movq %r13, %rsi
callq 0xf19e2
testl %eax, %eax
je 0x3de51
leaq 0x5a156b(%rip), %rsi    # 0x5df3a0
movq %rbx, %rdi
callq 0xd3fc6
movq %rbx, %rax
addq $0x100, %rsp            # imm = 0x100
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
retq
movsd 0x98(%rsp), %xmm0
addsd 0x10e1ae(%rip), %xmm0   # 0x14c010
cvttsd2si %xmm0, %ecx
xorl %eax, %eax
testl %ecx, %ecx
cmovgl %ecx, %eax
movq 0x8(%r14), %rcx
movl $0xffffffff, %edx       # imm = 0xFFFFFFFF
testq %rcx, %rcx
je 0x3dea7
movq 0x18(%rcx), %rcx
testq %rcx, %rcx
je 0x3dea7
movl 0x2c(%rcx), %edx
testl %edx, %edx
movl $0x790, %esi            # imm = 0x790
cmovnsl %edx, %esi
movl %eax, %edx
cmpl %esi, %eax
jl 0x3dea7
movl 0x2c(%rcx), %eax
leal -0x1(%rax), %ecx
testl %eax, %eax
movl $0x78f, %edx            # imm = 0x78F
cmovnsl %ecx, %edx
leaq 0xa8(%rsp), %r15
movq %r15, %rdi
movq %r14, %rsi
callq 0x3d2fe
leaq 0x80(%rsp), %rdi
movq %r15, %rsi
callq 0x450fa
leaq 0x68(%rsp), %rdi
leaq 0xa8(%rsp), %rsi
callq 0x45140
leaq 0xe8(%rsp), %rdi
leaq 0x80(%rsp), %rsi
leaq 0x68(%rsp), %rdx
callq 0xd57a6
leaq 0xb0(%rsp), %r15
movsd (%r12), %xmm0
movq 0x8(%r14), %rax
testq %rax, %rax
je 0x3dfb7
movq 0x18(%rax), %rcx
xorpd %xmm1, %xmm1
movapd %xmm1, 0x10(%rsp)
testq %rcx, %rcx
je 0x3df4e
movd 0x2c(%rcx), %xmm1
pshufd $0x50, %xmm1, %xmm1     # xmm1 = xmm1[0,0,1,1]
pxor %xmm2, %xmm2
pcmpgtd %xmm1, %xmm2
movdqa %xmm2, %xmm3
pand 0x10f07e(%rip), %xmm2   # 0x14cfc0
pandn %xmm1, %xmm3
por %xmm3, %xmm2
cvtdq2pd %xmm2, %xmm1
movsd 0x10e192(%rip), %xmm2   # 0x14c0e8
mulsd %xmm1, %xmm2
addsd %xmm0, %xmm2
addsd %xmm2, %xmm2
unpckhpd %xmm1, %xmm1            # xmm1 = xmm1[1,1]
divsd %xmm1, %xmm2
movsd %xmm2, 0x8(%rsp)
movsd 0x8(%r12), %xmm0
movsd %xmm0, (%rsp)
movq 0x18(%rax), %rax
testq %rax, %rax
je 0x3dfdf
movd 0x30(%rax), %xmm0
pshufd $0x50, %xmm0, %xmm0     # xmm0 = xmm0[0,0,1,1]
xorpd %xmm1, %xmm1
pcmpgtd %xmm0, %xmm1
movdqa %xmm1, %xmm2
pand 0x10f02d(%rip), %xmm1   # 0x14cfd0
pandn %xmm0, %xmm2
por %xmm2, %xmm1
cvtdq2pd %xmm1, %xmm0
movapd %xmm0, 0x10(%rsp)
jmp 0x3dfdf
addsd %xmm0, %xmm0
xorpd %xmm1, %xmm1
divsd %xmm1, %xmm0
movq 0x8(%r12), %xmm1
movq %xmm1, (%rsp)
pxor %xmm1, %xmm1
movdqa %xmm1, 0x10(%rsp)
movsd %xmm0, 0x8(%rsp)
leaq 0x50(%rsp), %rdi
leaq 0x80(%rsp), %rdx
movq %r15, %rsi
callq 0xd4723
leaq 0x20(%rsp), %rdi
leaq 0xe8(%rsp), %rsi
movsd 0x8(%rsp), %xmm0
callq 0xd580e
movq 0x8(%r14), %rax
testq %rax, %rax
je 0x3e01b
movq 0x18(%rax), %rax
jmp 0x3e01d
xorl %eax, %eax
movsd 0x38(%rax), %xmm1
addsd 0x40(%rax), %xmm1
mulsd 0x10dfe1(%rip), %xmm1   # 0x14c010
leaq 0x198422(%rip), %r12    # 0x1d6458
movsd (%r12), %xmm2
movapd 0x10ec7c(%rip), %xmm0   # 0x14ccc0
xorpd %xmm2, %xmm0
ucomisd %xmm0, %xmm1
jb 0x3e058
xorpd %xmm0, %xmm0
ucomisd %xmm1, %xmm2
jae 0x3e089
movl 0x2c(%rax), %eax
testl %eax, %eax
movl $0x790, %ecx            # imm = 0x790
cmovnsl %eax, %ecx
xorps %xmm0, %xmm0
cvtsi2sd %ecx, %xmm0
mulsd 0x10df9c(%rip), %xmm0   # 0x14c010
divsd %xmm1, %xmm0
callq 0x27790
addsd %xmm0, %xmm0
mulsd 0x10df87(%rip), %xmm0   # 0x14c010
callq 0x274e0
leaq 0x38(%rsp), %rdi
leaq 0x20(%rsp), %rsi
callq 0xd580e
leaq 0x50(%rsp), %rdi
leaq 0x38(%rsp), %rsi
callq 0xd47c0
movsd 0x10e034(%rip), %xmm0   # 0x14c0e8
movapd 0x10(%rsp), %xmm1
mulsd %xmm1, %xmm0
addsd (%rsp), %xmm0
addsd %xmm0, %xmm0
unpckhpd %xmm1, %xmm1            # xmm1 = xmm1[1,1]
divsd %xmm1, %xmm0
leaq 0x20(%rsp), %rdi
leaq 0x68(%rsp), %rsi
callq 0xd580e
movq 0x8(%r14), %rax
testq %rax, %rax
je 0x3e0ed
movq 0x18(%rax), %rax
jmp 0x3e0ef
xorl %eax, %eax
movsd 0x38(%rax), %xmm1
addsd 0x40(%rax), %xmm1
mulsd 0x10df0f(%rip), %xmm1   # 0x14c010
movsd (%r12), %xmm2
movapd 0x10ebb1(%rip), %xmm0   # 0x14ccc0
xorpd %xmm2, %xmm0
ucomisd %xmm0, %xmm1
jb 0x3e123
xorpd %xmm0, %xmm0
ucomisd %xmm1, %xmm2
jae 0x3e154
movl 0x30(%rax), %eax
testl %eax, %eax
movl $0xa20, %ecx            # imm = 0xA20
cmovnsl %eax, %ecx
xorps %xmm0, %xmm0
cvtsi2sd %ecx, %xmm0
mulsd 0x10ded1(%rip), %xmm0   # 0x14c010
divsd %xmm1, %xmm0
callq 0x27790
addsd %xmm0, %xmm0
mulsd 0x10debc(%rip), %xmm0   # 0x14c010
callq 0x274e0
leaq 0x38(%rsp), %rdi
leaq 0x20(%rsp), %rsi
callq 0xd580e
leaq 0x50(%rsp), %rdi
leaq 0x38(%rsp), %rsi
callq 0xd47c0
leaq 0x50(%rsp), %rdx
movq %rbx, %rdi
movq %r15, %rsi
callq 0xd3fd2
leaq 0xa8(%rsp), %rdi
callq 0x450ee
jmp 0x3de3d
jmp 0x3e1a3
jmp 0x3e1a3
jmp 0x3e1a3
jmp 0x3e1a3
jmp 0x3e1a3
movq %rax, %rbx
leaq 0xa8(%rsp), %rdi
callq 0x450ee
movq %rbx, %rdi
callq 0x28150
nop