// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/GSV/GSVCamera.cpp
// Function: gaps::GSVCamera::DistortedPositionFromUndistortedNormalizedPosition(double, double) const
R2Point GSVCamera::
DistortedPositionFromUndistortedNormalizedPosition(RNAngle x_dir, RNAngle y_dir) const
{
  // (xdir, y_dir) is undistorted position given in normalized coordinates
  // (x_pixel, y_pixel) is distorted position given in pixel coordinates 

  if (distortion_type == 0) { // PERSPECTIVE
    double x2 = x_dir * x_dir;
    double y2 = y_dir * y_dir;
    double r2 = x2 + y2;
    
    double r_max = tan( fov_max );
    double r2_max = r_max * r_max;
    if ( r2 > r2_max ) return R2Point(-1,-1);
    
    double xy = x_dir * y_dir;
    
    double dist_scale = 1 + k1*r2 + k2*r2*r2 + k3*r2*r2*r2;
    double x_distorted = dist_scale * x_dir + 2 * p1 * xy + p2 * (r2 + 2 * x2);
    double y_distorted = dist_scale * y_dir + p1 * ( r2 + 2 * y2 ) + 2 * p2 * xy;
    
    double x_pixel = focal_x * x_distorted + center_x;
    double y_pixel = focal_y * y_distorted + center_y;

    return R2Point(x_pixel, y_pixel);
  }
  else if (distortion_type == 1) {  // FISHEYE
    double x2 = x_dir * x_dir;
    double y2 = y_dir * y_dir;
    double r2 = x2 + y2;

    double r_max = tan( fov_max );
    double r2_max = r_max * r_max;
    if ( r2 > r2_max ) return R2Point(-1,-1);

    double r = sqrt ( r2 );

    double theta_tilde = 1;
    if (r > 1e-8) {
      double theta = atan( r );
      double theta2 = theta * theta;
      theta_tilde = theta * ( 1 + k1 * theta2 + k2 * theta2*theta2 ) / r;
    }

    double x_distorted = theta_tilde * x_dir;
    double y_distorted = theta_tilde * y_dir;

    double x_pixel = focal_x * x_distorted + center_x;
    double y_pixel = focal_y * y_distorted + center_y;

    return R2Point(x_pixel, y_pixel);
  }

  // Should never get here
  RNFail("Unrecognized distortion type: %d\n", distortion_type);
  return R2zero_point;
}

// Assembly:
pushq %r14
pushq %rbx
subq $0x28, %rsp
movapd %xmm0, %xmm2
movq %rsi, %r14
movq %rdi, %rbx
movl 0x28(%rsi), %esi
cmpl $0x1, %esi
je 0x3bd11
testl %esi, %esi
jne 0x3bd5f
movapd %xmm2, %xmm0
mulsd %xmm2, %xmm0
movapd %xmm1, %xmm3
mulsd %xmm1, %xmm3
movsd %xmm0, 0x20(%rsp)
movsd %xmm3, 0x18(%rsp)
addsd %xmm3, %xmm0
movsd %xmm0, (%rsp)
movsd 0x80(%r14), %xmm0
movsd %xmm1, 0x8(%rsp)
movsd %xmm2, 0x10(%rsp)
callq 0x274e0
movsd (%rsp), %xmm6
mulsd %xmm0, %xmm0
ucomisd %xmm0, %xmm6
ja 0x3bd4b
movsd 0x10(%rsp), %xmm8
movapd %xmm8, %xmm4
movsd 0x8(%rsp), %xmm7
mulsd %xmm7, %xmm4
movsd 0x58(%r14), %xmm1
mulsd %xmm6, %xmm1
addsd 0x1103ef(%rip), %xmm1   # 0x14c078
movsd 0x60(%r14), %xmm3
mulsd %xmm6, %xmm3
mulsd %xmm6, %xmm3
addsd %xmm1, %xmm3
movsd 0x68(%r14), %xmm2
mulsd %xmm6, %xmm2
mulsd %xmm6, %xmm2
mulsd %xmm6, %xmm2
addsd %xmm3, %xmm2
movsd 0x70(%r14), %xmm3
movsd 0x78(%r14), %xmm1
movsd 0x18(%rsp), %xmm5
addsd %xmm5, %xmm5
addsd %xmm6, %xmm5
mulsd %xmm3, %xmm5
addsd %xmm3, %xmm3
mulsd %xmm4, %xmm3
mulsd %xmm2, %xmm8
addsd %xmm3, %xmm8
movsd 0x20(%rsp), %xmm0
addsd %xmm0, %xmm0
addsd %xmm6, %xmm0
mulsd %xmm1, %xmm0
addsd %xmm8, %xmm0
mulsd %xmm7, %xmm2
addsd %xmm5, %xmm2
addsd %xmm1, %xmm1
mulsd %xmm4, %xmm1
addsd %xmm2, %xmm1
jmp 0x3bdef
movsd %xmm2, 0x10(%rsp)
mulsd %xmm2, %xmm2
movsd %xmm1, 0x8(%rsp)
mulsd %xmm1, %xmm1
addsd %xmm2, %xmm1
movsd %xmm1, (%rsp)
movsd 0x80(%r14), %xmm0
callq 0x274e0
mulsd %xmm0, %xmm0
movsd (%rsp), %xmm1
ucomisd %xmm0, %xmm1
jbe 0x3bd81
movsd 0x11031d(%rip), %xmm0   # 0x14c070
movq %rbx, %rdi
movapd %xmm0, %xmm1
jmp 0x3be0a
leaq 0x1111cd(%rip), %rdi    # 0x14cf33
xorl %eax, %eax
callq 0xfb6f2
leaq 0x5a418c(%rip), %rsi    # 0x5dff00
movq %rbx, %rdi
callq 0xf171c
jmp 0x3be0f
xorps %xmm0, %xmm0
sqrtsd %xmm1, %xmm0
ucomisd 0x111190(%rip), %xmm0   # 0x14cf20
jbe 0x3bdd3
movsd %xmm0, (%rsp)
callq 0x27790
movapd %xmm0, %xmm2
mulsd %xmm0, %xmm2
movsd 0x58(%r14), %xmm3
mulsd %xmm2, %xmm3
addsd 0x1102c2(%rip), %xmm3   # 0x14c078
movsd 0x60(%r14), %xmm4
mulsd %xmm2, %xmm4
mulsd %xmm2, %xmm4
addsd %xmm3, %xmm4
mulsd %xmm0, %xmm4
divsd (%rsp), %xmm4
jmp 0x3bddb
movsd 0x11029d(%rip), %xmm4   # 0x14c078
movsd 0x8(%rsp), %xmm1
movsd 0x10(%rsp), %xmm0
mulsd %xmm4, %xmm0
mulsd %xmm4, %xmm1
mulsd 0x38(%r14), %xmm0
addsd 0x48(%r14), %xmm0
mulsd 0x40(%r14), %xmm1
addsd 0x50(%r14), %xmm1
movq %rbx, %rdi
callq 0xf1712
movq %rbx, %rax
addq $0x28, %rsp
popq %rbx
popq %r14
retq