// File: 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:
subq $0x108, %rsp            # imm = 0x108
movq %rdi, 0x8(%rsp)
movq %rdi, %rax
movq %rax, 0x10(%rsp)
movq %rdi, 0x100(%rsp)
movq %rsi, 0xf8(%rsp)
movsd %xmm0, 0xf0(%rsp)
movq 0xf8(%rsp), %rdi
movq %rdi, 0x18(%rsp)
movsd 0xf0(%rsp), %xmm0
callq 0x548c0
movq %rax, 0xe8(%rsp)
cmpq $0x0, 0xe8(%rsp)
jne 0x54a48
movq 0x8(%rsp), %rdi
leaq 0x6ae869(%rip), %rsi    # 0x7032a0
leaq 0x2ade42(%rip), %rdx    # 0x302880
callq 0x5e450
jmp 0x54cb0
movq 0xe8(%rsp), %rdi
callq 0x565d0
movl %eax, 0xe4(%rsp)
cmpl $0x0, 0xe4(%rsp)
jge 0x54a8c
movq 0x18(%rsp), %rdi
xorl %esi, %esi
callq 0x3a0f0
movq %rax, %rdi
callq 0x44c60
movq 0x8(%rsp), %rdi
movq %rax, %rsi
callq 0x565e0
jmp 0x54cb0
movq 0x18(%rsp), %rdi
movl 0xe4(%rsp), %eax
movl %eax, 0x4(%rsp)
callq 0x3a0d0
movl %eax, %ecx
movl 0x4(%rsp), %eax
subl $0x1, %ecx
cmpl %ecx, %eax
jl 0x54ae1
movq 0x18(%rsp), %rdi
callq 0x3a0d0
movq 0x18(%rsp), %rdi
movl %eax, %esi
subl $0x1, %esi
callq 0x3a0f0
movq %rax, %rdi
callq 0x44c60
movq 0x8(%rsp), %rdi
movq %rax, %rsi
callq 0x565e0
jmp 0x54cb0
movq 0x18(%rsp), %rdi
movl 0xe4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xe0(%rsp)
movl 0xe0(%rsp), %esi
callq 0x3a0f0
movq %rax, 0xd8(%rsp)
movq 0xe8(%rsp), %rdi
callq 0x44c80
movsd %xmm0, 0xd0(%rsp)
movq 0xd8(%rsp), %rdi
callq 0x44c80
movsd %xmm0, 0xc8(%rsp)
movsd 0xc8(%rsp), %xmm0
subsd 0xd0(%rsp), %xmm0
movsd %xmm0, 0xc0(%rsp)
movsd 0xc0(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x54b85
jp 0x54b85
movq 0xe8(%rsp), %rdi
callq 0x44c60
movq 0x8(%rsp), %rdi
movq %rax, %rsi
callq 0x565e0
jmp 0x54cb0
movsd 0xf0(%rsp), %xmm0
subsd 0xd0(%rsp), %xmm0
divsd 0xc0(%rsp), %xmm0
movsd %xmm0, 0xb8(%rsp)
movq 0xe8(%rsp), %rdi
callq 0x44c60
movq %rax, 0xb0(%rsp)
movq 0xd8(%rsp), %rdi
callq 0x44c60
movq %rax, 0xa8(%rsp)
movq 0xb0(%rsp), %rdi
callq 0x3a330
movq %rax, 0xa0(%rsp)
movq 0xa8(%rsp), %rdi
callq 0x3a330
movq %rax, 0x98(%rsp)
movq 0xb0(%rsp), %rdi
callq 0x44c70
movq %rax, 0x90(%rsp)
movq 0xa8(%rsp), %rdi
callq 0x44c70
movq %rax, 0x88(%rsp)
movsd 0x2113e1(%rip), %xmm0   # 0x266010
subsd 0xb8(%rsp), %xmm0
movq 0xa0(%rsp), %rsi
leaq 0x58(%rsp), %rdi
callq 0x4a690
movsd 0xb8(%rsp), %xmm0
movq 0x98(%rsp), %rsi
leaq 0x40(%rsp), %rdi
callq 0x4a690
leaq 0x70(%rsp), %rdi
leaq 0x58(%rsp), %rsi
leaq 0x40(%rsp), %rdx
callq 0x180c80
movq 0x90(%rsp), %rsi
movq 0x88(%rsp), %rdx
movsd 0xb8(%rsp), %xmm0
leaq 0x20(%rsp), %rdi
callq 0x15e4f0
movq 0x8(%rsp), %rdi
leaq 0x70(%rsp), %rsi
leaq 0x20(%rsp), %rdx
callq 0x5e450
movq 0x10(%rsp), %rax
addq $0x108, %rsp            # imm = 0x108
retq
nopl (%rax)