// File: tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelImage.cpp
// Function: gaps::R3SurfelImage::TransformFromWorldToCamera(gaps::R3Point const&) const
R3Point R3SurfelImage::
TransformFromWorldToCamera(const R3Point& world_position) const
{
  // Initialize result
  R3Point camera_position = pose.InverseMatrix() * world_position;

  // Check if has rolling shutter
  if (HasRollingShutter()) {
    // Search for x coordinate (there is a different camera pose for every column)
    int column_index = ImageWidth() / 2;
    int last_column_index = column_index;
    for (int i = 0; i < ImageWidth()/2; i++) {
      // Transform world point into column's camera coordinate system
      R3CoordSystem cs = RollingShutterPoseAtImagePosition(R2Point(column_index, 0));
      R3Point camera_p = cs.InverseMatrix() * world_position;
      if (RNIsPositiveOrZero(camera_p.Z())) return camera_position;
      camera_position = camera_p;
      
      // Get/check column index
      R2Point distorted_position = TransformFromCameraToImage(camera_position);
      column_index = (int) (distorted_position.X() + 0.5);
      if (column_index < 0) column_index = 0;
      if (column_index >= ImageWidth()-1) column_index = ImageWidth()-1;
      if (column_index == last_column_index) break;
      last_column_index = column_index;
    }
  }

  // Return camera position
  return camera_position;
}

// Assembly:
subq $0x1e8, %rsp            # imm = 0x1E8
movq %rdi, 0x18(%rsp)
movq %rdi, %rax
movq %rax, 0x10(%rsp)
movq %rdi, 0x1e0(%rsp)
movq %rsi, 0x1d8(%rsp)
movq %rdx, 0x1d0(%rsp)
movq 0x1d8(%rsp), %rsi
movq %rsi, 0x20(%rsp)
addq $0x30, %rsi
leaq 0x150(%rsp), %rdi
callq 0xd5640
movq 0x18(%rsp), %rdi
movq 0x1d0(%rsp), %rdx
leaq 0x150(%rsp), %rsi
callq 0xdc230
movq 0x20(%rsp), %rdi
callq 0x31d40
cmpl $0x0, %eax
je 0x694b4
movq 0x20(%rsp), %rdi
callq 0x308f0
movl $0x2, %ecx
cltd
idivl %ecx
movl %eax, 0x14c(%rsp)
movl 0x14c(%rsp), %eax
movl %eax, 0x148(%rsp)
movl $0x0, 0x144(%rsp)
movq 0x20(%rsp), %rdi
movl 0x144(%rsp), %eax
movl %eax, 0xc(%rsp)
callq 0x308f0
movl $0x2, %ecx
cltd
idivl %ecx
movl %eax, %ecx
movl 0xc(%rsp), %eax
cmpl %ecx, %eax
jge 0x694b2
cvtsi2sdl 0x14c(%rsp), %xmm0
leaq 0xd0(%rsp), %rdi
xorps %xmm1, %xmm1
callq 0x133960
movq 0x20(%rsp), %rsi
leaq 0xe0(%rsp), %rdi
leaq 0xd0(%rsp), %rdx
callq 0x694d0
leaq 0x38(%rsp), %rdi
leaq 0xe0(%rsp), %rsi
callq 0xd5640
movq 0x1d0(%rsp), %rdx
leaq 0xb8(%rsp), %rdi
leaq 0x38(%rsp), %rsi
callq 0xdc230
leaq 0xb8(%rsp), %rdi
callq 0x187a0
leaq 0x193171(%rip), %rax    # 0x1fc548
movsd (%rax), %xmm1
callq 0x503b0
cmpl $0x0, %eax
je 0x693ea
jmp 0x694b6
movq 0x18(%rsp), %rdi
leaq 0xb8(%rsp), %rsi
callq 0xf89e0
movq 0x20(%rsp), %rsi
movq 0x18(%rsp), %rdx
leaq 0x28(%rsp), %rdi
callq 0x695f0
leaq 0x28(%rsp), %rdi
callq 0x308e0
movsd 0x13ad36(%rip), %xmm1   # 0x1a4158
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x14c(%rsp)
cmpl $0x0, 0x14c(%rsp)
jge 0x69446
movl $0x0, 0x14c(%rsp)
movq 0x20(%rsp), %rdi
movl 0x14c(%rsp), %eax
movl %eax, 0x8(%rsp)
callq 0x308f0
movl %eax, %ecx
movl 0x8(%rsp), %eax
subl $0x1, %ecx
cmpl %ecx, %eax
jl 0x6947c
movq 0x20(%rsp), %rdi
callq 0x308f0
subl $0x1, %eax
movl %eax, 0x14c(%rsp)
movl 0x14c(%rsp), %eax
cmpl 0x148(%rsp), %eax
jne 0x6948e
jmp 0x694b2
movl 0x14c(%rsp), %eax
movl %eax, 0x148(%rsp)
movl 0x144(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x144(%rsp)
jmp 0x69339
jmp 0x694b4
jmp 0x694b6
movq 0x10(%rsp), %rax
addq $0x1e8, %rsp            # imm = 0x1E8
retq
nopw %cs:(%rax,%rax)