// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelImage.cpp
// Function: gaps::R3SurfelImage::DistortImagePosition(gaps::R2Point const&) const
R2Point R3SurfelImage::
DistortImagePosition(const R2Point& undistorted_image_position) const
{
  // Check distortion parameters
  if (distortion_type == R3_SURFEL_NO_DISTORTION)
    return undistorted_image_position;

  // Check distortion parameters
  if ((radial_distortion[0] == 0) && (radial_distortion[1] == 0) && (radial_distortion[2] == 0) &&
      (tangential_distortion[0] == 0) && (tangential_distortion[1] == 0))
    return undistorted_image_position;

  // Check focal lengths
  if ((xfocal <= 0) || (yfocal <= 0))
    return undistorted_image_position;
  
  // Check distortion type
  if (distortion_type == R3_SURFEL_PERSPECTIVE_DISTORTION) {
    // Convert to normalized coordinates
    double ux = (undistorted_image_position.X() - XCenter()) / xfocal;
    double uy = (undistorted_image_position.Y() - YCenter()) / yfocal;

    // Get convenient variables
    const RNScalar *k = radial_distortion;
    const RNScalar *p = tangential_distortion;
    double x2 = ux * ux;
    double y2 = uy * uy;
    double xy = ux * uy;
    double r2 = x2 + y2;

    // Distort position
    double scale = 1 + k[0]*r2 + k[1]*r2*r2 + k[2]*r2*r2*r2;
    double dx = scale * ux + 2 * p[0] * xy + p[1] * (r2 + 2 * x2);
    double dy = scale * uy + p[0] * ( r2 + 2 * y2 ) + 2 * p[1] * xy;

    // Convert back to image coordinates
    double distorted_x = xfocal * dx + XCenter();
    double distorted_y = yfocal * dy + YCenter();

    // Return distorted position
    return R2Point(distorted_x, distorted_y);
  }
  else if (distortion_type == R3_SURFEL_FISHEYE_DISTORTION) {
    // Convert to normalized coordinates
    double ux = (undistorted_image_position.X() - XCenter()) / xfocal;
    double uy = (undistorted_image_position.Y() - YCenter()) / yfocal;

    // Get convenient variables
    const RNScalar *k = radial_distortion;
    double x2 = ux * ux;
    double y2 = uy * uy;
    double r2 = x2 + y2;
    double r = sqrt ( r2 );
    if (RNIsZero(r)) return undistorted_image_position;

    // Distort position
    double theta = atan( r );
    double theta2 = theta * theta;
    double theta_tilde = theta * ( 1 + k[0] * theta2 + k[1] * theta2*theta2 ) / r;
    double dx = theta_tilde * ux;
    double dy = theta_tilde * uy;

    // Convert back to image coordinates
    double distorted_x = xfocal * dx + XCenter();
    double distorted_y = yfocal * dy + YCenter();

    // Return distorted position
    return R2Point(distorted_x, distorted_y);
  }

  // Otherwise, apply no distortion
  return undistorted_image_position;
}

// Assembly:
pushq %r14
pushq %rbx
subq $0x28, %rsp
movq %rdi, %rbx
movl 0xc0(%rsi), %eax
testl %eax, %eax
je 0x3d069
movq %rsi, %r14
movupd 0xc8(%rsi), %xmm6
movupd 0xd8(%rsi), %xmm3
movsd 0xe8(%rsi), %xmm1
xorpd %xmm0, %xmm0
movapd %xmm3, %xmm2
cmpneqpd %xmm0, %xmm2
cmpneqpd %xmm6, %xmm0
shufps $0x88, %xmm2, %xmm0     # xmm0 = xmm0[0,2],xmm2[0,2]
movmskps %xmm0, %ecx
testl %ecx, %ecx
jne 0x3cedb
xorps %xmm0, %xmm0
ucomisd %xmm0, %xmm1
jne 0x3cedb
jnp 0x3d069
movsd 0xb0(%r14), %xmm2
xorps %xmm0, %xmm0
ucomisd %xmm2, %xmm0
jae 0x3d069
movsd 0xb8(%r14), %xmm4
ucomisd %xmm4, %xmm0
jae 0x3d069
cmpl $0x2, %eax
je 0x3d012
cmpl $0x1, %eax
jne 0x3d069
movupd (%rdx), %xmm8
movupd 0xa0(%r14), %xmm5
subpd %xmm5, %xmm8
movapd %xmm2, %xmm0
unpcklpd %xmm4, %xmm0            # xmm0 = xmm0[0],xmm4[0]
divpd %xmm0, %xmm8
movapd %xmm8, %xmm0
mulpd %xmm8, %xmm0
movapd %xmm8, %xmm9
unpckhpd %xmm8, %xmm9            # xmm9 = xmm9[1],xmm8[1]
movapd %xmm8, %xmm7
mulsd %xmm9, %xmm7
movapd %xmm0, %xmm10
unpckhpd %xmm0, %xmm10           # xmm10 = xmm10[1],xmm0[1]
movapd %xmm0, %xmm11
addsd %xmm10, %xmm11
movapd %xmm6, %xmm12
mulsd %xmm11, %xmm12
addsd 0xa01cd(%rip), %xmm12   # 0xdd148
unpckhpd %xmm6, %xmm6            # xmm6 = xmm6[1,1]
mulsd %xmm11, %xmm6
mulsd %xmm11, %xmm6
addsd %xmm12, %xmm6
movapd %xmm3, %xmm12
mulsd %xmm11, %xmm12
mulsd %xmm11, %xmm12
mulsd %xmm11, %xmm12
addsd %xmm6, %xmm12
unpckhpd %xmm3, %xmm3            # xmm3 = xmm3[1,1]
movapd %xmm3, %xmm6
addsd %xmm3, %xmm6
mulsd %xmm7, %xmm6
mulsd %xmm12, %xmm8
addsd %xmm6, %xmm8
addsd %xmm0, %xmm0
addsd %xmm11, %xmm0
mulsd %xmm1, %xmm0
addsd %xmm8, %xmm0
addsd %xmm10, %xmm10
addsd %xmm11, %xmm10
mulsd %xmm3, %xmm10
mulsd %xmm9, %xmm12
addsd %xmm10, %xmm12
addsd %xmm1, %xmm1
mulsd %xmm7, %xmm1
addsd %xmm12, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm5, %xmm0
unpckhpd %xmm5, %xmm5            # xmm5 = xmm5[1,1]
mulsd %xmm4, %xmm1
addsd %xmm5, %xmm1
jmp 0x3d106
movupd (%rdx), %xmm3
movupd 0xa0(%r14), %xmm0
subpd %xmm0, %xmm3
unpcklpd %xmm4, %xmm2            # xmm2 = xmm2[0],xmm4[0]
divpd %xmm2, %xmm3
movapd %xmm3, %xmm0
mulpd %xmm3, %xmm0
movapd %xmm0, %xmm1
unpckhpd %xmm0, %xmm1            # xmm1 = xmm1[1],xmm0[1]
addsd %xmm0, %xmm1
xorps %xmm0, %xmm0
sqrtsd %xmm1, %xmm0
leaq 0xf6bbb(%rip), %rax     # 0x133c08
movsd (%rax), %xmm2
movapd 0xa0347(%rip), %xmm1    # 0xdd3a0
xorpd %xmm2, %xmm1
ucomisd %xmm1, %xmm0
jb 0x3d07f
ucomisd %xmm0, %xmm2
jb 0x3d07f
movq %rbx, %rdi
movq %rdx, %rsi
callq 0xa254c
movq %rbx, %rax
addq $0x28, %rsp
popq %rbx
popq %r14
retq
movapd %xmm3, 0x10(%rsp)
movsd %xmm0, 0x8(%rsp)
callq 0x13410
movapd %xmm0, %xmm1
mulsd %xmm0, %xmm1
movsd 0xc8(%r14), %xmm2
mulsd %xmm1, %xmm2
addsd 0xa009b(%rip), %xmm2    # 0xdd148
movsd 0xd0(%r14), %xmm3
mulsd %xmm1, %xmm3
mulsd %xmm1, %xmm3
addsd %xmm2, %xmm3
mulsd %xmm0, %xmm3
divsd 0x8(%rsp), %xmm3
movapd 0x10(%rsp), %xmm1
movapd %xmm1, %xmm0
mulsd %xmm3, %xmm0
unpckhpd %xmm1, %xmm1            # xmm1 = xmm1[1,1]
mulsd %xmm3, %xmm1
mulsd 0xb0(%r14), %xmm0
addsd 0xa0(%r14), %xmm0
mulsd 0xb8(%r14), %xmm1
addsd 0xa8(%r14), %xmm1
movq %rbx, %rdi
callq 0xa2542
jmp 0x3d074
nop