// File: /workspace/github_cpp_makefile_star2_O2/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, 0x10(%rsp)
movq %rdi, %r14
movq 0x8(%rsi), %rax
testq %rax, %rax
je 0x3b600
movq 0x18(%rax), %rax
testq %rax, %rax
je 0x3b600
movq %rsi, %rbx
movq %r14, 0x8(%rsp)
movups 0x38(%rax), %xmm0
movaps %xmm0, 0x60(%rsp)
movups 0x48(%rax), %xmm0
movaps %xmm0, 0x50(%rsp)
leaq 0x30(%rsp), %rdi
callq 0xc0dfa
movq %rbx, %rdi
callq 0x3ac8e
pushq $0x2
popq %rcx
cltd
idivl %ecx
movl %eax, (%rsp)
movapd 0xf0aa6(%rip), %xmm0    # 0x12bf10
movapd %xmm0, 0x20(%rsp)
leaq 0x78(%rsp), %rbp
leaq 0xd0(%rsp), %r12
leaq 0xe8(%rsp), %r14
leaq 0x168(%rsp), %r15
xorl %r13d, %r13d
movq %rbx, %rdi
callq 0x3ac8e
cltd
pushq $0x2
popq %rcx
idivl %ecx
cmpl %eax, %r13d
jge 0x3b60f
movl %r13d, 0x4(%rsp)
movq %rbp, %rdi
movq %rbx, %rsi
movl (%rsp), %edx
callq 0x3acd2
movq %r12, %rdi
movq %rbp, %rsi
callq 0x41e74
leaq 0xb8(%rsp), %r13
movq %r13, %rdi
movq %rbp, %rsi
callq 0x41eba
movq %r14, %rdi
movq %r12, %rsi
movq %r13, %rdx
callq 0xb0020
movq %r15, %rdi
leaq 0x80(%rsp), %rsi
movq %r14, %rdx
callq 0xafbbe
movq %r14, %rdi
movq %r15, %rsi
callq 0xafc48
movq %r12, %rdi
movq %r14, %rsi
movq 0x10(%rsp), %rdx
callq 0xb2fc7
leaq 0x30(%rsp), %rdi
movq %r12, %rsi
callq 0xc10e6
movsd 0x40(%rsp), %xmm0
leaq 0x183f4a(%rip), %rax    # 0x1bf478
movsd (%rax), %xmm1
xorpd 0xf0786(%rip), %xmm1    # 0x12bcc0
ucomisd %xmm1, %xmm0
jae 0x3b62c
movq %rbx, %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 %r12, %rdi
movapd %xmm0, 0x20(%rsp)
callq 0xdb258
leaq 0xe8(%rsp), %rdi
movq %r15, %rsi
movq %r12, %rdx
callq 0x3b20a
movsd 0xe8(%rsp), %xmm0
movsd %xmm0, 0x18(%rsp)
movq %r15, %rbx
movq %r15, %rdi
callq 0x3ac8e
movsd 0x18(%rsp), %xmm0
addsd 0xefa7f(%rip), %xmm0    # 0x12b038
cvttsd2si %xmm0, %ecx
movl %eax, %ebp
testl %ecx, %ecx
movl $0x0, %eax
cmovlel %eax, %ecx
decl %ebp
cmpl %ebp, %ecx
cmovll %ecx, %ebp
movq %r14, %rdi
callq 0x41e6a
cmpl (%rsp), %ebp
je 0x3b60f
movl 0x4(%rsp), %r13d
incl %r13d
movl %ebp, (%rsp)
movq %r14, %rbp
leaq 0xe8(%rsp), %r14
leaq 0x168(%rsp), %r15
jmp 0x3b490
movq %r14, %rdi
xorl %esi, %esi
callq 0xdb276
movq %r14, %rax
jmp 0x3b652
movapd 0x20(%rsp), %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
movq 0x8(%rsp), %rbx
movq %rbx, %rdi
callq 0xdb258
jmp 0x3b64f
movsd 0xefa34(%rip), %xmm0    # 0x12b068
movq 0x8(%rsp), %rbx
movq %rbx, %rdi
movapd %xmm0, %xmm1
callq 0xdb258
leaq 0x78(%rsp), %rdi
callq 0x41e6a
movq %rbx, %rax
addq $0x1c8, %rsp            # imm = 0x1C8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x3b66c
jmp 0x3b66c
jmp 0x3b66c
jmp 0x3b66c
movq %rax, %rbx
leaq 0x78(%rsp), %rdi
callq 0x41e6a
movq %rbx, %rdi
callq 0x28170
nop