// File: 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:
subq $0x168, %rsp            # imm = 0x168
movq %rdi, 0x60(%rsp)
movq %rdi, %rax
movq %rax, 0x68(%rsp)
movq %rdi, 0x160(%rsp)
movq %rsi, 0x158(%rsp)
movq %rdx, 0x150(%rsp)
movq 0x158(%rsp), %rax
movq %rax, 0x70(%rsp)
cmpl $0x0, 0xc0(%rax)
jne 0x69869
movq 0x60(%rsp), %rdi
movq 0x150(%rsp), %rsi
callq 0x133990
jmp 0x69f2c
movq 0x70(%rsp), %rax
movsd 0xc8(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x698f8
jp 0x698f8
movq 0x70(%rsp), %rax
movsd 0xd0(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x698f8
jp 0x698f8
movq 0x70(%rsp), %rax
movsd 0xd8(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x698f8
jp 0x698f8
movq 0x70(%rsp), %rax
movsd 0xe0(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x698f8
jp 0x698f8
movq 0x70(%rsp), %rax
movsd 0xe8(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x698f8
jp 0x698f8
movq 0x60(%rsp), %rdi
movq 0x150(%rsp), %rsi
callq 0x133990
jmp 0x69f2c
movq 0x70(%rsp), %rax
xorps %xmm0, %xmm0
ucomisd 0xb0(%rax), %xmm0
jae 0x6991c
movq 0x70(%rsp), %rax
xorps %xmm0, %xmm0
ucomisd 0xb8(%rax), %xmm0
jb 0x69933
movq 0x60(%rsp), %rdi
movq 0x150(%rsp), %rsi
callq 0x133990
jmp 0x69f2c
movq 0x70(%rsp), %rax
cmpl $0x1, 0xc0(%rax)
jne 0x69c64
movq 0x150(%rsp), %rdi
callq 0x308e0
movq 0x70(%rsp), %rdi
movsd %xmm0, 0x30(%rsp)
callq 0x6ba60
movq 0x70(%rsp), %rax
movaps %xmm0, %xmm1
movsd 0x30(%rsp), %xmm0
subsd %xmm1, %xmm0
divsd 0xb0(%rax), %xmm0
movsd %xmm0, 0x148(%rsp)
movq 0x150(%rsp), %rdi
callq 0x30910
movq 0x70(%rsp), %rdi
movsd %xmm0, 0x38(%rsp)
callq 0x6ba80
movq 0x70(%rsp), %rdi
movaps %xmm0, %xmm1
movsd 0x38(%rsp), %xmm0
subsd %xmm1, %xmm0
divsd 0xb8(%rdi), %xmm0
movsd %xmm0, 0x140(%rsp)
movq %rdi, %rax
addq $0xc8, %rax
movq %rax, 0x138(%rsp)
movq %rdi, %rax
addq $0xe0, %rax
movq %rax, 0x130(%rsp)
movsd 0x148(%rsp), %xmm0
mulsd 0x148(%rsp), %xmm0
movsd %xmm0, 0x128(%rsp)
movsd 0x140(%rsp), %xmm0
mulsd 0x140(%rsp), %xmm0
movsd %xmm0, 0x120(%rsp)
movsd 0x148(%rsp), %xmm0
mulsd 0x140(%rsp), %xmm0
movsd %xmm0, 0x118(%rsp)
movsd 0x128(%rsp), %xmm0
addsd 0x120(%rsp), %xmm0
movsd %xmm0, 0x110(%rsp)
movq 0x138(%rsp), %rax
movsd (%rax), %xmm0
movsd 0x110(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x13a6f4(%rip), %xmm1   # 0x1a4168
addsd %xmm1, %xmm0
movq 0x138(%rsp), %rax
movsd 0x8(%rax), %xmm1
mulsd 0x110(%rsp), %xmm1
movsd 0x110(%rsp), %xmm2
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movq 0x138(%rsp), %rax
movsd 0x10(%rax), %xmm0
mulsd 0x110(%rsp), %xmm0
mulsd 0x110(%rsp), %xmm0
movsd 0x110(%rsp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x108(%rsp)
movsd 0x108(%rsp), %xmm1
movsd 0x148(%rsp), %xmm2
movq 0x130(%rsp), %rax
movsd 0x13a67e(%rip), %xmm0   # 0x1a4178
mulsd (%rax), %xmm0
mulsd 0x118(%rsp), %xmm0
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movq 0x130(%rsp), %rax
movsd 0x8(%rax), %xmm0
movsd 0x110(%rsp), %xmm3
movsd 0x128(%rsp), %xmm2
addsd %xmm2, %xmm2
addsd %xmm3, %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x100(%rsp)
movsd 0x108(%rsp), %xmm1
movsd 0x140(%rsp), %xmm2
movq 0x130(%rsp), %rax
movsd (%rax), %xmm0
movsd 0x110(%rsp), %xmm4
movsd 0x120(%rsp), %xmm3
addsd %xmm3, %xmm3
addsd %xmm4, %xmm3
mulsd %xmm3, %xmm0
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movq 0x130(%rsp), %rax
movsd 0x13a5dd(%rip), %xmm0   # 0x1a4178
mulsd 0x8(%rax), %xmm0
movsd 0x118(%rsp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xf8(%rsp)
movsd 0xb0(%rdi), %xmm0
movsd %xmm0, 0x48(%rsp)
movsd 0x100(%rsp), %xmm0
movsd %xmm0, 0x40(%rsp)
callq 0x6ba60
movsd 0x40(%rsp), %xmm2
movq 0x70(%rsp), %rdi
movaps %xmm0, %xmm1
movsd 0x48(%rsp), %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xf0(%rsp)
movsd 0xb8(%rdi), %xmm0
movsd %xmm0, 0x58(%rsp)
movsd 0xf8(%rsp), %xmm0
movsd %xmm0, 0x50(%rsp)
callq 0x6ba80
movsd 0x50(%rsp), %xmm2
movq 0x60(%rsp), %rdi
movaps %xmm0, %xmm1
movsd 0x58(%rsp), %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xe8(%rsp)
movsd 0xf0(%rsp), %xmm0
movsd 0xe8(%rsp), %xmm1
callq 0x133960
jmp 0x69f2c
movq 0x70(%rsp), %rax
cmpl $0x2, 0xc0(%rax)
jne 0x69f18
movq 0x150(%rsp), %rdi
callq 0x308e0
movq 0x70(%rsp), %rdi
movsd %xmm0, 0x20(%rsp)
callq 0x6ba60
movq 0x70(%rsp), %rax
movaps %xmm0, %xmm1
movsd 0x20(%rsp), %xmm0
subsd %xmm1, %xmm0
divsd 0xb0(%rax), %xmm0
movsd %xmm0, 0xe0(%rsp)
movq 0x150(%rsp), %rdi
callq 0x30910
movq 0x70(%rsp), %rdi
movsd %xmm0, 0x28(%rsp)
callq 0x6ba80
movq 0x70(%rsp), %rax
movaps %xmm0, %xmm1
movsd 0x28(%rsp), %xmm0
subsd %xmm1, %xmm0
divsd 0xb8(%rax), %xmm0
movsd %xmm0, 0xd8(%rsp)
addq $0xc8, %rax
movq %rax, 0xd0(%rsp)
movsd 0xe0(%rsp), %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0xc8(%rsp)
movsd 0xd8(%rsp), %xmm0
mulsd 0xd8(%rsp), %xmm0
movsd %xmm0, 0xc0(%rsp)
movsd 0xc8(%rsp), %xmm0
addsd 0xc0(%rsp), %xmm0
movsd %xmm0, 0xb8(%rsp)
movsd 0xb8(%rsp), %xmm0
callq 0x13420
movsd %xmm0, 0xb0(%rsp)
movsd 0xb0(%rsp), %xmm0
leaq 0x1927cc(%rip), %rax    # 0x1fc548
movsd (%rax), %xmm1
callq 0x50350
cmpl $0x0, %eax
je 0x69da1
movq 0x60(%rsp), %rdi
movq 0x150(%rsp), %rsi
callq 0x133990
jmp 0x69f2c
movsd 0xb0(%rsp), %xmm0
callq 0x13480
movq 0x70(%rsp), %rdi
movsd %xmm0, 0xa8(%rsp)
movsd 0xa8(%rsp), %xmm0
mulsd 0xa8(%rsp), %xmm0
movsd %xmm0, 0xa0(%rsp)
movsd 0xa8(%rsp), %xmm0
movq 0xd0(%rsp), %rax
movsd (%rax), %xmm2
movsd 0xa0(%rsp), %xmm1
mulsd %xmm1, %xmm2
movsd 0x13a366(%rip), %xmm1   # 0x1a4168
addsd %xmm1, %xmm2
movq 0xd0(%rsp), %rax
movsd 0x8(%rax), %xmm1
mulsd 0xa0(%rsp), %xmm1
movsd 0xa0(%rsp), %xmm3
mulsd %xmm3, %xmm1
addsd %xmm2, %xmm1
mulsd %xmm1, %xmm0
divsd 0xb0(%rsp), %xmm0
movsd %xmm0, 0x98(%rsp)
movsd 0x98(%rsp), %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0x90(%rsp)
movsd 0x98(%rsp), %xmm0
mulsd 0xd8(%rsp), %xmm0
movsd %xmm0, 0x88(%rsp)
movsd 0xb0(%rdi), %xmm0
movsd %xmm0, 0x8(%rsp)
movsd 0x90(%rsp), %xmm0
movsd %xmm0, (%rsp)
callq 0x6ba60
movsd (%rsp), %xmm2
movq 0x70(%rsp), %rdi
movaps %xmm0, %xmm1
movsd 0x8(%rsp), %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x80(%rsp)
movsd 0xb8(%rdi), %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x88(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
callq 0x6ba80
movsd 0x10(%rsp), %xmm2
movq 0x60(%rsp), %rdi
movaps %xmm0, %xmm1
movsd 0x18(%rsp), %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x78(%rsp)
movsd 0x80(%rsp), %xmm0
movsd 0x78(%rsp), %xmm1
callq 0x133960
jmp 0x69f2c
jmp 0x69f1a
movq 0x60(%rsp), %rdi
movq 0x150(%rsp), %rsi
callq 0x133990
movq 0x68(%rsp), %rax
addq $0x168, %rsp            # imm = 0x168
retq
nopl (%rax)