// File: /workspace/github_cpp_makefile_star2_O1/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 %rsi, %r15
movq %rdi, 0x10(%rsp)
movq 0x8(%rsi), %rax
testq %rax, %rax
je 0x3de93
movq 0x18(%rax), %rax
jmp 0x3de95
xorl %eax, %eax
testq %rax, %rax
je 0x3dec8
movups 0x38(%rax), %xmm0
movaps %xmm0, 0x60(%rsp)
movups 0x48(%rax), %xmm0
movaps %xmm0, 0x50(%rsp)
leaq 0x30(%rsp), %rdi
callq 0xd5e2c
movq 0x8(%r15), %rax
xorl %r12d, %r12d
testq %rax, %rax
je 0x3dedf
movq 0x18(%rax), %rax
jmp 0x3dee1
movq 0x10(%rsp), %rbx
movq %rbx, %rdi
xorl %esi, %esi
callq 0xf3324
movq %rbx, %rax
jmp 0x3e13d
xorl %eax, %eax
testq %rax, %rax
je 0x3defe
movl 0x2c(%rax), %eax
movl %eax, %ecx
shrl $0x1f, %ecx
addl %eax, %ecx
sarl %ecx
testl %eax, %eax
movl $0x3c8, %r12d           # imm = 0x3C8
cmovnsl %ecx, %r12d
movapd 0x11100a(%rip), %xmm0   # 0x14ef10
movapd %xmm0, 0x20(%rsp)
movl $0xffffffff, %r14d      # imm = 0xFFFFFFFF
leaq 0xd0(%rsp), %rbp
leaq 0x148(%rsp), %rbx
movl %r12d, 0xc(%rsp)
movq 0x8(%r15), %rax
testq %rax, %rax
je 0x3df36
movq 0x18(%rax), %rax
jmp 0x3df38
xorl %eax, %eax
leaq 0x90(%rsp), %r13
testq %rax, %rax
je 0x3df5d
movl 0x2c(%rax), %ecx
movl %ecx, %eax
shrl $0x1f, %eax
addl %ecx, %eax
sarl %eax
testl %ecx, %ecx
movl $0x3c8, %ecx            # imm = 0x3C8
cmovsl %ecx, %eax
jmp 0x3df5f
xorl %eax, %eax
incl %r14d
cmpl %eax, %r14d
jge 0x3e114
movq %r13, %rdi
movq %r15, %rsi
movl %r12d, %edx
callq 0x3d698
movq %rbp, %rdi
movq %r13, %rsi
callq 0x455c6
leaq 0x78(%rsp), %rdi
movq %r13, %rsi
callq 0x4560c
movq %rbx, %rdi
movq %rbp, %rsi
leaq 0x78(%rsp), %rdx
callq 0xc29b6
leaq 0xe8(%rsp), %rdi
leaq 0x98(%rsp), %rsi
movq %rbx, %rdx
callq 0xc24b8
movq %rbx, %rdi
leaq 0xe8(%rsp), %rsi
callq 0xc2566
movq %rbp, %rdi
movq %rbx, %rsi
movq 0x18(%rsp), %rdx
callq 0xc61fe
leaq 0x30(%rsp), %rdi
movq %rbp, %rsi
callq 0xd619c
movsd 0x40(%rsp), %xmm0
leaq 0x19a475(%rip), %rax    # 0x1d8468
movsd (%rax), %xmm1
xorpd 0x110cb1(%rip), %xmm1   # 0x14ecb0
ucomisd %xmm1, %xmm0
jae 0x3e056
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 %rbp, %rdi
movapd %xmm0, 0x20(%rsp)
callq 0xf3306
movq %rbx, %rdi
movq %r15, %rsi
movq %rbp, %rdx
callq 0x3dc72
movq 0x8(%r15), %rax
testq %rax, %rax
je 0x3e077
movq 0x18(%rax), %rcx
jmp 0x3e079
movl $0x1, %r13d
movq 0x10(%rsp), %rdi
movsd 0x110007(%rip), %xmm0   # 0x14e070
movapd %xmm0, %xmm1
callq 0xf3306
jmp 0x3e0fc
xorl %ecx, %ecx
testq %rcx, %rcx
je 0x3e090
movl 0x2c(%rcx), %edx
leal -0x1(%rdx), %ecx
testl %edx, %edx
movl $0x78f, %edx            # imm = 0x78F
cmovsl %edx, %ecx
jmp 0x3e095
movl $0xffffffff, %ecx       # imm = 0xFFFFFFFF
movsd 0x148(%rsp), %xmm0
addsd 0x10ff6a(%rip), %xmm0   # 0x14e010
cvttsd2si %xmm0, %r12d
testl %r12d, %r12d
movl $0x0, %edx
cmovlel %edx, %r12d
cmpl %ecx, %r12d
jl 0x3e0e8
testq %rax, %rax
je 0x3e0c7
movq 0x18(%rax), %rax
jmp 0x3e0c9
xorl %eax, %eax
testq %rax, %rax
je 0x3e0e2
movl 0x2c(%rax), %eax
leal -0x1(%rax), %r12d
testl %eax, %eax
movl $0x78f, %eax            # imm = 0x78F
cmovsl %eax, %r12d
jmp 0x3e0e8
movl $0xffffffff, %r12d      # imm = 0xFFFFFFFF
xorl %r13d, %r13d
cmpl 0xc(%rsp), %r12d
sete %r13b
addl %r13d, %r13d
movl %r12d, 0xc(%rsp)
leaq 0x90(%rsp), %rdi
callq 0x455ba
testl %r13d, %r13d
je 0x3df27
jmp 0x3e11a
movl $0x2, %r13d
cmpl $0x2, %r13d
jne 0x3e138
movapd 0x20(%rsp), %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
movq 0x10(%rsp), %rdi
callq 0xf3306
movq 0x10(%rsp), %rax
addq $0x1c8, %rsp            # imm = 0x1C8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x3e157
jmp 0x3e157
jmp 0x3e157
jmp 0x3e157
movq %rax, %rbx
leaq 0x90(%rsp), %rdi
callq 0x455ba
movq %rbx, %rdi
callq 0x28160
nop