// File: tomfunkhouser[P]gaps/pkgs/GSV/GSVImage.cpp
// Function: gaps::GSVImage::UndistortedPosition(gaps::R3Point const&) const
R2Point GSVImage::
UndistortedPosition(const R3Point& world_position) 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();
  RNCoord x = RN_UNKNOWN;
  RNCoord y = RN_UNKNOWN;
  R3Point camera_position;

  // Search for x coordinate (there is a different camera pose for every column)
  int column_index = Width() / 2;
  int last_column_index = column_index;
  for (int i = 0; i < Width()/2; i++) {
    // 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()));
    camera_position = cs.InverseMatrix() * world_position;
    if (RNIsPositiveOrZero(camera_position.Z())) return R2Point(-1,-1);

    // Compute coordinates of projection
    x = xcenter + camera_position.X() * xfocal / -(camera_position.Z());
    y = ycenter + camera_position.Y() * yfocal / -(camera_position.Z());

    // Get/check column index
    R2Point distorted_position = DistortedPosition(R2Point(x,y));
    column_index = (int) (distorted_position.X() + 0.5);
    if (column_index < 0) column_index = 0;
    if (column_index >= Width()-1) column_index = Width()-1;
    if (column_index == last_column_index) break;
    last_column_index = column_index;
  }

  // Fill in point projected onto image
  return R2Point(x, y);
}

// Assembly:
subq $0x308, %rsp            # imm = 0x308
movq %rdi, 0x88(%rsp)
movq %rdi, %rax
movq %rax, 0x90(%rsp)
movq %rdi, 0x300(%rsp)
movq %rsi, 0x2f8(%rsp)
movq %rdx, 0x2f0(%rsp)
movq 0x2f8(%rsp), %rdi
movq %rdi, 0x98(%rsp)
callq 0x4f700
movq %rax, 0x2e8(%rsp)
cmpq $0x0, 0x2e8(%rsp)
jne 0x50ba0
movq 0x88(%rsp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x1b9a90
jmp 0x51069
movq 0x2e8(%rsp), %rdi
callq 0x44980
movsd %xmm0, 0x2e0(%rsp)
movq 0x2e8(%rsp), %rdi
callq 0x44990
movsd %xmm0, 0x2d8(%rsp)
movq 0x2e8(%rsp), %rdi
callq 0x44960
movsd %xmm0, 0x2d0(%rsp)
movq 0x2e8(%rsp), %rdi
callq 0x44970
movsd %xmm0, 0x2c8(%rsp)
movsd 0x215408(%rip), %xmm0   # 0x266008
movsd %xmm0, 0x2c0(%rsp)
movsd 0x2153f7(%rip), %xmm0   # 0x266008
movsd %xmm0, 0x2b8(%rsp)
leaq 0x2a0(%rsp), %rdi
callq 0x17fd80
movq 0x98(%rsp), %rdi
callq 0x4fa70
movl $0x2, %ecx
cltd
idivl %ecx
movl %eax, 0x29c(%rsp)
movl 0x29c(%rsp), %eax
movl %eax, 0x298(%rsp)
movl $0x0, 0x294(%rsp)
movq 0x98(%rsp), %rdi
movl 0x294(%rsp), %eax
movl %eax, 0x84(%rsp)
callq 0x4fa70
movl $0x2, %ecx
cltd
idivl %ecx
movl %eax, %ecx
movl 0x84(%rsp), %eax
cmpl %ecx, %eax
jge 0x5104a
movq 0x98(%rsp), %rsi
movl 0x29c(%rsp), %edx
leaq 0x248(%rsp), %rdi
movq %rdi, 0x70(%rsp)
callq 0x4faf0
movq 0x70(%rsp), %rax
movq %rax, 0x288(%rsp)
movq 0x288(%rsp), %rdi
callq 0x3a330
movq %rax, 0x78(%rsp)
jmp 0x50cd2
movq 0x288(%rsp), %rsi
leaq 0x178(%rsp), %rdi
callq 0x5e6c0
jmp 0x50ce9
movq 0x288(%rsp), %rsi
leaq 0x160(%rsp), %rdi
callq 0x5e730
jmp 0x50d00
leaq 0x190(%rsp), %rdi
leaq 0x178(%rsp), %rsi
leaq 0x160(%rsp), %rdx
callq 0x15d9b0
jmp 0x50d1f
movq 0x78(%rsp), %rsi
leaq 0x1e8(%rsp), %rdi
leaq 0x190(%rsp), %rdx
callq 0x15d1c0
jmp 0x50d3b
leaq 0xc8(%rsp), %rdi
leaq 0x1e8(%rsp), %rsi
callq 0x15d290
jmp 0x50d52
movq 0x2f0(%rsp), %rdx
leaq 0x148(%rsp), %rdi
leaq 0xc8(%rsp), %rsi
callq 0x163e40
jmp 0x50d71
leaq 0x2a0(%rsp), %rdi
leaq 0x148(%rsp), %rsi
callq 0x180420
jmp 0x50d88
leaq 0x2a0(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x68(%rsp)
jmp 0x50d9d
movsd 0x68(%rsp), %xmm0
leaq 0x2a807e(%rip), %rax    # 0x2f8e28
movsd (%rax), %xmm1
callq 0x51920
movl %eax, 0x64(%rsp)
jmp 0x50db9
movl 0x64(%rsp), %eax
cmpl $0x0, %eax
je 0x50e12
movq 0x88(%rsp), %rdi
movsd 0x215236(%rip), %xmm1   # 0x266008
movaps %xmm1, %xmm0
callq 0x1b9a30
jmp 0x50ddc
movl $0x1, 0xc4(%rsp)
jmp 0x51007
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1e0(%rsp)
movl %eax, 0x1dc(%rsp)
leaq 0x248(%rsp), %rdi
callq 0x5e680
jmp 0x51079
movsd 0x2e0(%rsp), %xmm0
movsd %xmm0, 0x50(%rsp)
leaq 0x2a0(%rsp), %rdi
callq 0x38bb0
movsd %xmm0, 0x58(%rsp)
jmp 0x50e36
movsd 0x58(%rsp), %xmm0
movsd 0x2d0(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x40(%rsp)
leaq 0x2a0(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x48(%rsp)
jmp 0x50e64
movsd 0x50(%rsp), %xmm0
movsd 0x48(%rsp), %xmm2
movsd 0x40(%rsp), %xmm1
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x2c0(%rsp)
movsd 0x2d8(%rsp), %xmm0
movsd %xmm0, 0x30(%rsp)
leaq 0x2a0(%rsp), %rdi
callq 0x38c10
movsd %xmm0, 0x38(%rsp)
jmp 0x50eab
movsd 0x38(%rsp), %xmm0
movsd 0x2c8(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x20(%rsp)
leaq 0x2a0(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x28(%rsp)
jmp 0x50ed9
movsd 0x30(%rsp), %xmm0
movsd 0x28(%rsp), %xmm2
movsd 0x20(%rsp), %xmm1
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x2b8(%rsp)
movsd 0x2c0(%rsp), %xmm0
movsd 0x2b8(%rsp), %xmm1
leaq 0xa0(%rsp), %rdi
callq 0x1b9a30
jmp 0x50f1d
movq 0x98(%rsp), %rsi
leaq 0xb0(%rsp), %rdi
leaq 0xa0(%rsp), %rdx
callq 0x506e0
jmp 0x50f3c
leaq 0xb0(%rsp), %rdi
callq 0x39990
movsd %xmm0, 0x18(%rsp)
jmp 0x50f51
movsd 0x18(%rsp), %xmm0
movsd 0x2150f1(%rip), %xmm1   # 0x266050
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x29c(%rsp)
cmpl $0x0, 0x29c(%rsp)
jge 0x50f83
movl $0x0, 0x29c(%rsp)
movq 0x98(%rsp), %rdi
movl 0x29c(%rsp), %eax
movl %eax, 0x10(%rsp)
callq 0x4fa70
movl %eax, 0x14(%rsp)
jmp 0x50fa1
movl 0x10(%rsp), %eax
movl 0x14(%rsp), %ecx
subl $0x1, %ecx
cmpl %ecx, %eax
jl 0x50fd1
movq 0x98(%rsp), %rdi
callq 0x4fa70
movl %eax, 0xc(%rsp)
jmp 0x50fc3
movl 0xc(%rsp), %eax
subl $0x1, %eax
movl %eax, 0x29c(%rsp)
movl 0x29c(%rsp), %eax
cmpl 0x298(%rsp), %eax
jne 0x50fee
movl $0x2, 0xc4(%rsp)
jmp 0x51007
movl 0x29c(%rsp), %eax
movl %eax, 0x298(%rsp)
movl $0x0, 0xc4(%rsp)
leaq 0x248(%rsp), %rdi
callq 0x5e680
movl 0xc4(%rsp), %eax
movl %eax, 0x8(%rsp)
testl %eax, %eax
je 0x51032
jmp 0x51025
movl 0x8(%rsp), %eax
subl $0x1, %eax
je 0x51069
jmp 0x51030
jmp 0x5104a
jmp 0x51034
movl 0x294(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x294(%rsp)
jmp 0x50c5c
movq 0x88(%rsp), %rdi
movsd 0x2c0(%rsp), %xmm0
movsd 0x2b8(%rsp), %xmm1
callq 0x1b9a30
movq 0x90(%rsp), %rax
addq $0x308, %rsp            # imm = 0x308
retq
movq 0x1e0(%rsp), %rdi
callq 0x29c10
nopw %cs:(%rax,%rax)