// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/GSV/GSVTapestry.cpp
// Function: gaps::GSVTapestry::Pose(double) const
GSVPose GSVTapestry::
Pose(RNScalar timestamp) const
{
  // Find images before and after timestamp
  GSVImage *image1 = FindImageBeforeTimestamp(timestamp);
  if (!image1) return GSVPose(R3zero_point, R3null_quaternion);
  int index1 = image1->TapestryIndex();
  if (index1 < 0) return Image(0)->Pose();
  if (index1 >= NImages()-1) return Image(NImages()-1)->Pose();
  int index2 = index1 + 1;
  GSVImage *image2 = Image(index2);

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

  // Interpolate poses (would be better with quaternions)
  const GSVPose& pose1 = image1->Pose();
  const GSVPose& pose2 = image2->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 %r15
pushq %r14
pushq %rbx
subq $0xf0, %rsp
movq %rsi, %r15
movq %rdi, %rbx
movq %rsi, %rdi
movsd %xmm0, 0x8(%rsp)
callq 0x3e4fa
testq %rax, %rax
je 0x3e605
movq %rax, %r14
movslq 0x10(%rax), %rax
testq %rax, %rax
js 0x3e61d
movslq 0x34(%r15), %rcx
leal -0x1(%rcx), %esi
movq 0x28(%r15), %rdx
cmpl %esi, %eax
jge 0x3e626
movq 0x8(%rdx,%rax,8), %r15
movq %r14, %rdi
movl $0xffffffff, %esi       # imm = 0xFFFFFFFF
callq 0x3d3f8
movsd %xmm0, 0x10(%rsp)
movq %r15, %rdi
movl $0xffffffff, %esi       # imm = 0xFFFFFFFF
callq 0x3d3f8
movsd 0x10(%rsp), %xmm1
subsd %xmm1, %xmm0
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm0
jne 0x3e648
jp 0x3e648
movq %rbx, %rdi
movq %r14, %rsi
jmp 0x3e62e
leaq 0x5a1034(%rip), %rsi    # 0x5df640
leaq 0x1a06ed(%rip), %rdx    # 0x1ded00
movq %rbx, %rdi
callq 0x44fa2
jmp 0x3e638
movq 0x28(%r15), %rax
movq (%rax), %rsi
jmp 0x3e62b
movq -0x8(%rdx,%rcx,8), %rsi
movq %rbx, %rdi
movl $0xffffffff, %edx       # imm = 0xFFFFFFFF
callq 0x3d2fe
movq %rbx, %rax
addq $0xf0, %rsp
popq %rbx
popq %r14
popq %r15
retq
movsd %xmm0, 0x18(%rsp)
leaq 0xb0(%rsp), %rdi
movq %r14, %rsi
movl $0xffffffff, %edx       # imm = 0xFFFFFFFF
callq 0x3d2fe
leaq 0x70(%rsp), %rdi
movq %r15, %rsi
movl $0xffffffff, %edx       # imm = 0xFFFFFFFF
callq 0x3d2fe
movsd 0x8(%rsp), %xmm1
subsd 0x10(%rsp), %xmm1
divsd 0x18(%rsp), %xmm1
leaq 0xb8(%rsp), %rsi
movsd 0x10d9e1(%rip), %xmm0   # 0x14c078
movsd %xmm1, 0x8(%rsp)
subsd %xmm1, %xmm0
leaq 0x20(%rsp), %rdi
callq 0xd4e22
leaq 0x78(%rsp), %rsi
leaq 0x40(%rsp), %rdi
movsd 0x8(%rsp), %xmm0
callq 0xd4e22
leaq 0x58(%rsp), %rdi
leaq 0x20(%rsp), %rsi
leaq 0x40(%rsp), %rdx
callq 0xd4ddc
leaq 0xd0(%rsp), %rsi
leaq 0x90(%rsp), %rdx
leaq 0x20(%rsp), %rdi
movsd 0x8(%rsp), %xmm0
callq 0xc18bb
leaq 0x58(%rsp), %rsi
leaq 0x20(%rsp), %rdx
movq %rbx, %rdi
callq 0x44fa2
leaq 0x70(%rsp), %rdi
callq 0x450ee
leaq 0xb0(%rsp), %rdi
callq 0x450ee
jmp 0x3e638
jmp 0x3e72b
movq %rax, %rbx
jmp 0x3e738
jmp 0x3e72b
movq %rax, %rbx
leaq 0x70(%rsp), %rdi
callq 0x450ee
leaq 0xb0(%rsp), %rdi
callq 0x450ee
movq %rbx, %rdi
callq 0x28150
nop