// File: tomfunkhouser[P]gaps/pkgs/GSV/GSVImage.cpp
// Function: gaps::GSVImage::UndistortedPosition(gaps::R3Point const&, int) const
R2Point GSVImage::
UndistortedPosition(const R3Point& world_position, int column_index) const
{
  // Get camera info
  GSVCamera *camera = Camera();
  if (!camera) return 0;
  RNCoord xcenter = camera->XCenter();
  RNCoord ycenter = camera->YCenter();
  RNAngle xfocal = camera->XFocal();
  RNAngle yfocal = camera->YFocal();

  // Transform world point into column's camera coordinate system
  const GSVPose& column_pose = Pose(column_index);
  R3CoordSystem cs(column_pose.Viewpoint(), R3Triad(column_pose.Towards(), column_pose.Up()));
  R3Point camera_position = cs.InverseMatrix() * world_position;
  if (RNIsPositiveOrZero(camera_position.Z())) return R2Point(-1,-1);

  // Compute coordinates of projection
  RNCoord x = xcenter + camera_position.X() * xfocal / -(camera_position.Z());
  RNCoord y = ycenter + camera_position.Y() * yfocal / -(camera_position.Z());
  
  // Fill in point projected onto image
  return R2Point(x, y);
}

// Assembly:
subq $0x2a8, %rsp            # imm = 0x2A8
movq %rdi, 0x68(%rsp)
movq %rdi, %rax
movq %rax, 0x70(%rsp)
movq %rdi, 0x2a0(%rsp)
movq %rsi, 0x298(%rsp)
movq %rdx, 0x290(%rsp)
movl %ecx, 0x28c(%rsp)
movq 0x298(%rsp), %rdi
movq %rdi, 0x78(%rsp)
callq 0x4f700
movq %rax, 0x280(%rsp)
cmpq $0x0, 0x280(%rsp)
jne 0x5082b
movq 0x68(%rsp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x1b9a90
jmp 0x50b08
movq 0x280(%rsp), %rdi
callq 0x44980
movsd %xmm0, 0x278(%rsp)
movq 0x280(%rsp), %rdi
callq 0x44990
movsd %xmm0, 0x270(%rsp)
movq 0x280(%rsp), %rdi
callq 0x44960
movsd %xmm0, 0x268(%rsp)
movq 0x280(%rsp), %rdi
callq 0x44970
movq 0x78(%rsp), %rsi
movsd %xmm0, 0x260(%rsp)
movl 0x28c(%rsp), %edx
leaq 0x218(%rsp), %rdi
movq %rdi, 0x58(%rsp)
callq 0x4faf0
movq 0x58(%rsp), %rax
movq %rax, 0x258(%rsp)
movq 0x258(%rsp), %rdi
callq 0x3a330
movq %rax, 0x60(%rsp)
jmp 0x508c2
movq 0x258(%rsp), %rsi
leaq 0x148(%rsp), %rdi
callq 0x5e6c0
jmp 0x508d9
movq 0x258(%rsp), %rsi
leaq 0x130(%rsp), %rdi
callq 0x5e730
jmp 0x508f0
leaq 0x160(%rsp), %rdi
leaq 0x148(%rsp), %rsi
leaq 0x130(%rsp), %rdx
callq 0x15d9b0
jmp 0x5090f
movq 0x60(%rsp), %rsi
leaq 0x1b8(%rsp), %rdi
leaq 0x160(%rsp), %rdx
callq 0x15d1c0
jmp 0x5092b
leaq 0x98(%rsp), %rdi
leaq 0x1b8(%rsp), %rsi
callq 0x15d290
jmp 0x50942
movq 0x290(%rsp), %rdx
leaq 0x118(%rsp), %rdi
leaq 0x98(%rsp), %rsi
callq 0x163e40
jmp 0x50961
leaq 0x118(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x50(%rsp)
jmp 0x50976
movsd 0x50(%rsp), %xmm0
leaq 0x2a84a5(%rip), %rax    # 0x2f8e28
movsd (%rax), %xmm1
callq 0x51920
movl %eax, 0x4c(%rsp)
jmp 0x50992
movl 0x4c(%rsp), %eax
cmpl $0x0, %eax
je 0x509e8
movq 0x68(%rsp), %rdi
movsd 0x215660(%rip), %xmm1   # 0x266008
movaps %xmm1, %xmm0
callq 0x1b9a30
jmp 0x509b2
movl $0x1, 0x94(%rsp)
jmp 0x50afb
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1b0(%rsp)
movl %eax, 0x1ac(%rsp)
leaq 0x218(%rsp), %rdi
callq 0x5e680
jmp 0x50b15
movsd 0x278(%rsp), %xmm0
movsd %xmm0, 0x38(%rsp)
leaq 0x118(%rsp), %rdi
callq 0x38bb0
movsd %xmm0, 0x40(%rsp)
jmp 0x50a0c
movsd 0x40(%rsp), %xmm0
movsd 0x268(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x28(%rsp)
leaq 0x118(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x30(%rsp)
jmp 0x50a3a
movsd 0x38(%rsp), %xmm0
movsd 0x30(%rsp), %xmm2
movsd 0x28(%rsp), %xmm1
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x88(%rsp)
movsd 0x270(%rsp), %xmm0
movsd %xmm0, 0x18(%rsp)
leaq 0x118(%rsp), %rdi
callq 0x38c10
movsd %xmm0, 0x20(%rsp)
jmp 0x50a81
movsd 0x20(%rsp), %xmm0
movsd 0x260(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x8(%rsp)
leaq 0x118(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x10(%rsp)
jmp 0x50aaf
movq 0x68(%rsp), %rdi
movsd 0x18(%rsp), %xmm0
movsd 0x10(%rsp), %xmm2
movsd 0x8(%rsp), %xmm1
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x80(%rsp)
movsd 0x88(%rsp), %xmm0
movsd 0x80(%rsp), %xmm1
callq 0x1b9a30
jmp 0x50af0
movl $0x1, 0x94(%rsp)
leaq 0x218(%rsp), %rdi
callq 0x5e680
movq 0x70(%rsp), %rax
addq $0x2a8, %rsp            # imm = 0x2A8
retq
movq 0x1b0(%rsp), %rdi
callq 0x29c10
nopw %cs:(%rax,%rax)