// File: /workspace/github_cpp_makefile_star2_O3/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:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1c8, %rsp            # imm = 0x1C8
movq %rdx, 0x18(%rsp)
movq %rdi, %rbx
movq 0x8(%rsi), %rax
testq %rax, %rax
je 0x3dcd8
movq 0x18(%rax), %rax
testq %rax, %rax
je 0x3dcd8
movq %rbx, 0x8(%rsp)
movups 0x38(%rax), %xmm0
movaps %xmm0, 0x60(%rsp)
movups 0x48(%rax), %xmm0
movaps %xmm0, 0x50(%rsp)
leaq 0x30(%rsp), %rdi
movq %rsi, %rbx
callq 0xd43dc
movq %rbx, %rsi
movq 0x8(%rbx), %rax
movl $0x0, %edx
testq %rax, %rax
je 0x3db0f
movq 0x18(%rax), %rcx
testq %rcx, %rcx
je 0x3db0f
movl 0x2c(%rcx), %ecx
movl %ecx, %edi
shrl $0x1f, %edi
addl %ecx, %edi
sarl %edi
testl %ecx, %ecx
movl $0x3c8, %edx            # imm = 0x3C8
cmovnsl %edi, %edx
movapd 0x10f3f9(%rip), %xmm0   # 0x14cf10
movapd %xmm0, 0x20(%rsp)
leaq 0x78(%rsp), %rbp
leaq 0xd0(%rsp), %rbx
leaq 0xb8(%rsp), %r14
leaq 0xe8(%rsp), %r13
leaq 0x168(%rsp), %r12
xorl %edi, %edi
movq %rsi, 0x10(%rsp)
xorl %ecx, %ecx
testq %rax, %rax
je 0x3db6f
movq 0x18(%rax), %rax
testq %rax, %rax
je 0x3db6f
movl 0x2c(%rax), %eax
movl %eax, %ecx
shrl $0x1f, %ecx
addl %eax, %ecx
sarl %ecx
testl %eax, %eax
movl $0x3c8, %eax            # imm = 0x3C8
cmovsl %eax, %ecx
cmpl %ecx, %edi
jge 0x3dce4
movl %edi, (%rsp)
movq %rbp, %rdi
movl %edx, 0x4(%rsp)
callq 0x3d2fe
movq %rbx, %rdi
movq %rbp, %rsi
callq 0x450fa
movq %r14, %rdi
movq %rbp, %rsi
callq 0x45140
movq %r13, %rdi
movq %rbx, %rsi
movq %r14, %rdx
callq 0xc12c2
movq %r12, %rdi
leaq 0x80(%rsp), %rsi
movq %r13, %rdx
callq 0xc0dc4
movq %r13, %rdi
movq %r12, %rsi
callq 0xc0e72
movq %rbx, %rdi
movq %r13, %rsi
movq 0x18(%rsp), %rdx
callq 0xc4a56
leaq 0x30(%rsp), %rdi
movq %rbx, %rsi
callq 0xd4746
movsd 0x40(%rsp), %xmm0
leaq 0x198866(%rip), %rax    # 0x1d6458
movsd (%rax), %xmm1
xorpd 0x10f0c2(%rip), %xmm1   # 0x14ccc0
ucomisd %xmm1, %xmm0
jae 0x3dd01
movq %r14, %r15
movq %rbp, %r14
movapd 0x30(%rsp), %xmm1
mulpd 0x60(%rsp), %xmm1
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
divpd %xmm0, %xmm1
movapd 0x50(%rsp), %xmm0
subpd %xmm1, %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
movq %rbx, %rdi
movapd %xmm0, 0x20(%rsp)
callq 0xf1712
movq 0x10(%rsp), %rbp
movq %r13, %rdi
movq %rbp, %rsi
movq %rbx, %rdx
callq 0x3d898
movq 0x8(%rbp), %rax
movl $0xffffffff, %ebp       # imm = 0xFFFFFFFF
testq %rax, %rax
je 0x3dcaf
movq 0x18(%rax), %rax
testq %rax, %rax
je 0x3dcaf
movl 0x2c(%rax), %ecx
leal -0x1(%rcx), %edx
testl %ecx, %ecx
movl $0x78f, %ecx            # imm = 0x78F
cmovsl %ecx, %edx
movsd 0xe8(%rsp), %xmm0
addsd 0x10e383(%rip), %xmm0   # 0x14c010
cvttsd2si %xmm0, %ebp
testl %ebp, %ebp
movl $0x0, %ecx
cmovlel %ecx, %ebp
cmpl %edx, %ebp
jl 0x3dcaf
movl 0x2c(%rax), %eax
leal -0x1(%rax), %ebp
testl %eax, %eax
movl $0x78f, %eax            # imm = 0x78F
cmovsl %eax, %ebp
movq %r14, %rdi
callq 0x450ee
cmpl 0x4(%rsp), %ebp
je 0x3dce4
movl (%rsp), %edi
incl %edi
movq 0x10(%rsp), %rsi
movq 0x8(%rsi), %rax
movl %ebp, %edx
movq %r14, %rbp
movq %r15, %r14
jmp 0x3db49
movq %rbx, %rdi
xorl %esi, %esi
callq 0xf1730
jmp 0x3dd24
movapd 0x20(%rsp), %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
movq 0x8(%rsp), %rbx
movq %rbx, %rdi
callq 0xf1712
jmp 0x3dd24
movsd 0x10e367(%rip), %xmm0   # 0x14c070
movq 0x8(%rsp), %rbx
movq %rbx, %rdi
movapd %xmm0, %xmm1
callq 0xf1712
leaq 0x78(%rsp), %rdi
callq 0x450ee
movq %rbx, %rax
addq $0x1c8, %rsp            # imm = 0x1C8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x3dd41
jmp 0x3dd41
jmp 0x3dd41
jmp 0x3dd41
movq %rax, %rbx
leaq 0x78(%rsp), %rdi
callq 0x450ee
movq %rbx, %rdi
callq 0x28150