// File: 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:
subq $0xf8, %rsp
movq %rdi, (%rsp)
movq %rdi, %rax
movq %rax, 0x8(%rsp)
movq %rdi, 0xf0(%rsp)
movq %rsi, 0xe8(%rsp)
movsd %xmm0, 0xe0(%rsp)
movsd %xmm1, 0xd8(%rsp)
movq 0xe8(%rsp), %rax
movq %rax, 0x10(%rsp)
cmpl $0x0, 0x28(%rax)
jne 0x4d17a
movq 0x10(%rsp), %rax
movsd 0xe0(%rsp), %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0xd0(%rsp)
movsd 0xd8(%rsp), %xmm0
mulsd 0xd8(%rsp), %xmm0
movsd %xmm0, 0xc8(%rsp)
movsd 0xd0(%rsp), %xmm0
addsd 0xc8(%rsp), %xmm0
movsd %xmm0, 0xc0(%rsp)
movsd 0x80(%rax), %xmm0
callq 0x299c0
movsd %xmm0, 0xb8(%rsp)
movsd 0xb8(%rsp), %xmm0
mulsd 0xb8(%rsp), %xmm0
movsd %xmm0, 0xb0(%rsp)
movsd 0xc0(%rsp), %xmm0
ucomisd 0xb0(%rsp), %xmm0
jbe 0x4cfc0
movq (%rsp), %rdi
movsd 0x219055(%rip), %xmm1   # 0x266008
movaps %xmm1, %xmm0
callq 0x1b9a30
jmp 0x4d36b
movq (%rsp), %rdi
movq 0x10(%rsp), %rax
movsd 0xe0(%rsp), %xmm0
mulsd 0xd8(%rsp), %xmm0
movsd %xmm0, 0xa8(%rsp)
movsd 0x58(%rax), %xmm0
movsd 0xc0(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x219012(%rip), %xmm1   # 0x266010
addsd %xmm1, %xmm0
movsd 0x60(%rax), %xmm1
mulsd 0xc0(%rsp), %xmm1
movsd 0xc0(%rsp), %xmm2
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movsd 0x68(%rax), %xmm0
mulsd 0xc0(%rsp), %xmm0
mulsd 0xc0(%rsp), %xmm0
movsd 0xc0(%rsp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xa0(%rsp)
movsd 0xa0(%rsp), %xmm1
movsd 0xe0(%rsp), %xmm2
movsd 0x219044(%rip), %xmm0   # 0x2660b0
mulsd 0x70(%rax), %xmm0
mulsd 0xa8(%rsp), %xmm0
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movsd 0x78(%rax), %xmm0
movsd 0xc0(%rsp), %xmm3
movsd 0xd0(%rsp), %xmm2
addsd %xmm2, %xmm2
addsd %xmm3, %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x98(%rsp)
movsd 0xa0(%rsp), %xmm1
movsd 0xd8(%rsp), %xmm2
movsd 0x70(%rax), %xmm0
movsd 0xc0(%rsp), %xmm4
movsd 0xc8(%rsp), %xmm3
addsd %xmm3, %xmm3
addsd %xmm4, %xmm3
mulsd %xmm3, %xmm0
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movsd 0x218fb9(%rip), %xmm0   # 0x2660b0
mulsd 0x78(%rax), %xmm0
movsd 0xa8(%rsp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x90(%rsp)
movsd 0x38(%rax), %xmm0
movsd 0x98(%rsp), %xmm2
movsd 0x48(%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x88(%rsp)
movsd 0x40(%rax), %xmm0
movsd 0x90(%rsp), %xmm2
movsd 0x50(%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x80(%rsp)
movsd 0x88(%rsp), %xmm0
movsd 0x80(%rsp), %xmm1
callq 0x1b9a30
jmp 0x4d36b
movq 0x10(%rsp), %rax
cmpl $0x1, 0x28(%rax)
jne 0x4d343
movq 0x10(%rsp), %rax
movsd 0xe0(%rsp), %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0x78(%rsp)
movsd 0xd8(%rsp), %xmm0
mulsd 0xd8(%rsp), %xmm0
movsd %xmm0, 0x70(%rsp)
movsd 0x78(%rsp), %xmm0
addsd 0x70(%rsp), %xmm0
movsd %xmm0, 0x68(%rsp)
movsd 0x80(%rax), %xmm0
callq 0x299c0
movsd %xmm0, 0x60(%rsp)
movsd 0x60(%rsp), %xmm0
mulsd 0x60(%rsp), %xmm0
movsd %xmm0, 0x58(%rsp)
movsd 0x68(%rsp), %xmm0
ucomisd 0x58(%rsp), %xmm0
jbe 0x4d21c
movq (%rsp), %rdi
movsd 0x218df9(%rip), %xmm1   # 0x266008
movaps %xmm1, %xmm0
callq 0x1b9a30
jmp 0x4d36b
movsd 0x68(%rsp), %xmm0
callq 0x298d0
movsd %xmm0, 0x50(%rsp)
movsd 0x218ddb(%rip), %xmm0   # 0x266010
movsd %xmm0, 0x48(%rsp)
movsd 0x50(%rsp), %xmm0
movsd 0x219dcf(%rip), %xmm1   # 0x267018
ucomisd %xmm1, %xmm0
jbe 0x4d2c1
movsd 0x50(%rsp), %xmm0
callq 0x2a4f0
movq 0x10(%rsp), %rax
movsd %xmm0, 0x40(%rsp)
movsd 0x40(%rsp), %xmm0
mulsd 0x40(%rsp), %xmm0
movsd %xmm0, 0x38(%rsp)
movsd 0x40(%rsp), %xmm0
movsd 0x58(%rax), %xmm2
movsd 0x38(%rsp), %xmm1
mulsd %xmm1, %xmm2
movsd 0x218d7c(%rip), %xmm1   # 0x266010
addsd %xmm1, %xmm2
movsd 0x60(%rax), %xmm1
mulsd 0x38(%rsp), %xmm1
movsd 0x38(%rsp), %xmm3
mulsd %xmm3, %xmm1
addsd %xmm2, %xmm1
mulsd %xmm1, %xmm0
divsd 0x50(%rsp), %xmm0
movsd %xmm0, 0x48(%rsp)
movq (%rsp), %rdi
movq 0x10(%rsp), %rax
movsd 0x48(%rsp), %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0x30(%rsp)
movsd 0x48(%rsp), %xmm0
mulsd 0xd8(%rsp), %xmm0
movsd %xmm0, 0x28(%rsp)
movsd 0x38(%rax), %xmm0
movsd 0x30(%rsp), %xmm2
movsd 0x48(%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x20(%rsp)
movsd 0x40(%rax), %xmm0
movsd 0x28(%rsp), %xmm2
movsd 0x50(%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x20(%rsp), %xmm0
movsd 0x18(%rsp), %xmm1
callq 0x1b9a30
jmp 0x4d36b
jmp 0x4d345
movq 0x10(%rsp), %rax
movl 0x28(%rax), %esi
leaq 0x219cd7(%rip), %rdi    # 0x26702b
movb $0x0, %al
callq 0x1cbff0
movq (%rsp), %rdi
leaq 0x6b6942(%rip), %rsi    # 0x703ca8
callq 0x1b9a60
movq 0x8(%rsp), %rax
addq $0xf8, %rsp
retq
nopl (%rax,%rax)