// File: /workspace/github_cpp_makefile_star2_O1/tomfunkhouser[P]gaps/pkgs/GSV/GSVScan.cpp
// Function: gaps::GSVScan::Pose(double) const
GSVPose GSVScan::
Pose(RNScalar timestamp) const
{
  // Find scanlines before and after timestamp
  GSVScanline *scanline1 = FindScanlineBeforeTimestamp(timestamp);
  if (!scanline1) return GSVPose(R3zero_point, R3zero_quaternion);
  int index1 = scanline1->ScanIndex();
  if (index1 < 0) return Scanline(0)->Pose();
  if (index1 >= NScanlines()-1) return Scanline(NScanlines()-1)->Pose();
  int index2 = index1 + 1;
  GSVScanline *scanline2 = Scanline(index2);

  // Compute interpolation parameter (0 <= t <= 1)
  RNScalar timestamp1 = scanline1->Timestamp();
  RNScalar timestamp2 = scanline2->Timestamp();
  RNScalar delta_timestamp = timestamp2 - timestamp1;
  if (delta_timestamp == 0) return scanline1->Pose();
  RNScalar t = (timestamp - timestamp1) / delta_timestamp;

  // Interpolate poses (would be better with quaternions)
  const GSVPose& pose1 = scanline1->Pose();
  const GSVPose& pose2 = scanline2->Pose();
  const R3Point& viewpoint1 = pose1.Viewpoint();
  const R3Point& viewpoint2 = pose2.Viewpoint();
  const R3Quaternion& orientation1 = pose1.Orientation();
  const R3Quaternion& orientation2 = pose2.Orientation();
  R3Point viewpoint = (1-t)*viewpoint1 + t*viewpoint2;
  R3Quaternion orientation = R3QuaternionSlerp(orientation1, orientation2, t);

  // Return interpolated pose
  return GSVPose(viewpoint, orientation);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x58, %rsp
movq %rsi, %r15
movq %rdi, %rbx
movq %rsi, %rdi
movsd %xmm0, (%rsp)
callq 0x3fd80
testq %rax, %rax
je 0x3fe30
movq %rax, %r14
movslq 0x10(%rax), %rax
testq %rax, %rax
js 0x3fe48
movslq 0x34(%r15), %rcx
leal -0x1(%rcx), %esi
movq 0x28(%r15), %rdx
cmpl %esi, %eax
jge 0x3fe51
movq 0x8(%rdx,%rax,8), %r15
movsd 0x78(%r14), %xmm1
movsd 0x78(%r15), %xmm0
subsd %xmm1, %xmm0
leaq 0x40(%r14), %rsi
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm0
jne 0x3fe96
jp 0x3fe96
leaq 0x18f08d(%rip), %rax    # 0x1ceeb0
addq $0x10, %rax
movq %rax, (%rbx)
leaq 0x8(%rbx), %rdi
jmp 0x3fe6c
leaq 0x5a1819(%rip), %rsi    # 0x5e1650
leaq 0x1a0ef2(%rip), %rdx    # 0x1e0d30
movq %rbx, %rdi
callq 0x4546e
jmp 0x3fe84
movq 0x28(%r15), %rax
movq (%rax), %r14
jmp 0x3fe56
movq -0x8(%rdx,%rcx,8), %r14
leaq 0x18f053(%rip), %rax    # 0x1ceeb0
addq $0x10, %rax
movq %rax, (%rbx)
leaq 0x8(%rbx), %rdi
leaq 0x40(%r14), %rsi
callq 0xd5e3e
movq %rbx, %rdi
addq $0x20, %rdi
addq $0x58, %r14
movq %r14, %rsi
callq 0xc2f66
movq %rbx, %rax
addq $0x58, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movsd (%rsp), %xmm2
subsd %xmm1, %xmm2
divsd %xmm0, %xmm2
movsd %xmm2, (%rsp)
leaq 0x40(%r15), %r12
addq $0x58, %r14
addq $0x58, %r15
movsd 0x10e1cc(%rip), %xmm0   # 0x14e088
subsd %xmm2, %xmm0
leaq 0x8(%rsp), %r13
movq %r13, %rdi
callq 0xd6878
leaq 0x28(%rsp), %rbp
movq %rbp, %rdi
movq %r12, %rsi
movsd (%rsp), %xmm0
callq 0xd6878
leaq 0x40(%rsp), %r12
movq %r12, %rdi
movq %r13, %rsi
movq %rbp, %rdx
callq 0xd6832
leaq 0x8(%rsp), %r13
movq %r13, %rdi
movq %r14, %rsi
movq %r15, %rdx
movsd (%rsp), %xmm0
callq 0xc2fbb
movq %rbx, %rdi
movq %r12, %rsi
movq %r13, %rdx
jmp 0x3fe41
nop