// File: /workspace/github_cpp_makefile_star2_O2/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 %rbp
pushq %r15
pushq %r14
pushq %rbx
subq $0xe8, %rsp
movq %rsi, %r15
movq %rdi, %rbx
movq %rsi, %rdi
movsd %xmm0, (%rsp)
callq 0x3bd4c
testq %rax, %rax
je 0x3be53
movq %rax, %r14
movslq 0x10(%rax), %rax
testq %rax, %rax
js 0x3be6b
movslq 0x34(%r15), %rcx
leal -0x1(%rcx), %esi
movq 0x28(%r15), %rdx
cmpl %esi, %eax
jge 0x3be74
movq 0x8(%rdx,%rax,8), %r15
pushq $-0x1
popq %rbp
movq %r14, %rdi
movl %ebp, %esi
callq 0x3adba
movsd %xmm0, 0x8(%rsp)
movq %r15, %rdi
movl %ebp, %esi
callq 0x3adba
movsd 0x8(%rsp), %xmm1
subsd %xmm1, %xmm0
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm0
jne 0x3be95
jp 0x3be95
pushq $-0x1
popq %rdx
movq %rbx, %rdi
movq %r14, %rsi
jmp 0x3be7f
leaq 0x58c806(%rip), %rsi    # 0x5c8660
leaq 0x18bebf(%rip), %rdx    # 0x1c7d20
movq %rbx, %rdi
callq 0x41d1e
jmp 0x3be84
movq 0x28(%r15), %rax
movq (%rax), %rsi
jmp 0x3be79
movq -0x8(%rdx,%rcx,8), %rsi
pushq $-0x1
popq %rdx
movq %rbx, %rdi
callq 0x3acd2
movq %rbx, %rax
addq $0xe8, %rsp
popq %rbx
popq %r14
popq %r15
popq %rbp
retq
movsd %xmm0, 0x10(%rsp)
leaq 0xa8(%rsp), %rdi
pushq $-0x1
popq %rbp
movq %r14, %rsi
movl %ebp, %edx
callq 0x3acd2
leaq 0x68(%rsp), %rdi
movq %r15, %rsi
movl %ebp, %edx
callq 0x3acd2
movsd (%rsp), %xmm1
subsd 0x8(%rsp), %xmm1
divsd 0x10(%rsp), %xmm1
leaq 0xb0(%rsp), %rsi
movsd 0xef190(%rip), %xmm0    # 0x12b070
movsd %xmm1, (%rsp)
subsd %xmm1, %xmm0
leaq 0x18(%rsp), %rdi
callq 0xc1570
leaq 0x70(%rsp), %rsi
leaq 0x38(%rsp), %rdi
movsd (%rsp), %xmm0
callq 0xc1570
leaq 0x50(%rsp), %rdi
leaq 0x18(%rsp), %rsi
leaq 0x38(%rsp), %rdx
callq 0xc152a
leaq 0xc8(%rsp), %rsi
leaq 0x88(%rsp), %rdx
leaq 0x18(%rsp), %rdi
movsd (%rsp), %xmm0
callq 0xb0615
leaq 0x50(%rsp), %rsi
leaq 0x18(%rsp), %rdx
movq %rbx, %rdi
callq 0x41d1e
leaq 0x68(%rsp), %rdi
callq 0x41e6a
leaq 0xa8(%rsp), %rdi
callq 0x41e6a
jmp 0x3be84
jmp 0x3bf71
movq %rax, %rbx
jmp 0x3bf7e
jmp 0x3bf71
movq %rax, %rbx
leaq 0x68(%rsp), %rdi
callq 0x41e6a
leaq 0xa8(%rsp), %rdi
callq 0x41e6a
movq %rbx, %rdi
callq 0x28170
nop