// File: /workspace/github_cpp_makefile_star2_O2/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 0x39a63
testl %esi, %esi
jne 0x39ab1
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 0x39a9d
movsd 0x10(%rsp), %xmm8
movapd %xmm8, %xmm4
movsd 0x8(%rsp), %xmm7
mulsd %xmm7, %xmm4
movsd 0x58(%r14), %xmm1
mulsd %xmm6, %xmm1
addsd 0xf1695(%rip), %xmm1    # 0x12b070
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 0x39b3e
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 0x39ad3
movsd 0xf15c3(%rip), %xmm0    # 0x12b068
movq %rbx, %rdi
movapd %xmm0, %xmm1
jmp 0x39b59
leaq 0xf247b(%rip), %rdi     # 0x12bf33
xorl %eax, %eax
callq 0xe434a
leaq 0x58f45a(%rip), %rsi    # 0x5c8f20
movq %rbx, %rdi
callq 0xdb262
jmp 0x39b5e
sqrtsd %xmm1, %xmm0
ucomisd 0xf2441(%rip), %xmm0    # 0x12bf20
jbe 0x39b22
movsd %xmm0, (%rsp)
callq 0x27790
movapd %xmm0, %xmm2
mulsd %xmm0, %xmm2
movsd 0x58(%r14), %xmm3
mulsd %xmm2, %xmm3
addsd 0xf156b(%rip), %xmm3    # 0x12b070
movsd 0x60(%r14), %xmm4
mulsd %xmm2, %xmm4
mulsd %xmm2, %xmm4
addsd %xmm3, %xmm4
mulsd %xmm0, %xmm4
divsd (%rsp), %xmm4
jmp 0x39b2a
movsd 0xf1546(%rip), %xmm4    # 0x12b070
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 0xdb258
movq %rbx, %rax
addq $0x28, %rsp
popq %rbx
popq %r14
retq
nop