// File: 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:
subq $0x1c8, %rsp            # imm = 0x1C8
movq %rdi, 0x38(%rsp)
movq %rdi, %rax
movq %rax, 0x40(%rsp)
movq %rdi, 0x1c0(%rsp)
movq %rsi, 0x1b8(%rsp)
movsd %xmm0, 0x1b0(%rsp)
movq 0x1b8(%rsp), %rdi
movq %rdi, 0x48(%rsp)
movsd 0x1b0(%rsp), %xmm0
callq 0x51ec0
movq %rax, 0x1a8(%rsp)
cmpq $0x0, 0x1a8(%rsp)
jne 0x52058
movq 0x38(%rsp), %rdi
leaq 0x6b1259(%rip), %rsi    # 0x7032a0
leaq 0x2b0812(%rip), %rdx    # 0x302860
callq 0x5e450
jmp 0x523a6
movq 0x1a8(%rsp), %rdi
callq 0x51760
movl %eax, 0x1a4(%rsp)
cmpl $0x0, 0x1a4(%rsp)
jge 0x52099
movq 0x48(%rsp), %rdi
xorl %esi, %esi
callq 0x4a650
movq 0x38(%rsp), %rdi
movq %rax, %rsi
movl $0xffffffff, %edx       # imm = 0xFFFFFFFF
callq 0x4faf0
jmp 0x523a6
movq 0x48(%rsp), %rdi
movl 0x1a4(%rsp), %eax
movl %eax, 0x34(%rsp)
callq 0x44d20
movl %eax, %ecx
movl 0x34(%rsp), %eax
subl $0x1, %ecx
cmpl %ecx, %eax
jl 0x520eb
movq 0x48(%rsp), %rdi
callq 0x44d20
movq 0x48(%rsp), %rdi
movl %eax, %esi
subl $0x1, %esi
callq 0x4a650
movq 0x38(%rsp), %rdi
movq %rax, %rsi
movl $0xffffffff, %edx       # imm = 0xFFFFFFFF
callq 0x4faf0
jmp 0x523a6
movq 0x48(%rsp), %rdi
movl 0x1a4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x1a0(%rsp)
movl 0x1a0(%rsp), %esi
callq 0x4a650
movq %rax, 0x198(%rsp)
movq 0x1a8(%rsp), %rdi
movl $0xffffffff, %esi       # imm = 0xFFFFFFFF
callq 0x4fc90
movsd %xmm0, 0x190(%rsp)
movq 0x198(%rsp), %rdi
movl $0xffffffff, %esi       # imm = 0xFFFFFFFF
callq 0x4fc90
movsd %xmm0, 0x188(%rsp)
movsd 0x188(%rsp), %xmm0
subsd 0x190(%rsp), %xmm0
movsd %xmm0, 0x180(%rsp)
movsd 0x180(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x52196
jp 0x52196
movq 0x38(%rsp), %rdi
movq 0x1a8(%rsp), %rsi
movl $0xffffffff, %edx       # imm = 0xFFFFFFFF
callq 0x4faf0
jmp 0x523a6
movsd 0x1b0(%rsp), %xmm0
movsd 0x190(%rsp), %xmm1
subsd %xmm1, %xmm0
movsd 0x180(%rsp), %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, 0x178(%rsp)
movq 0x1a8(%rsp), %rsi
leaq 0x130(%rsp), %rdi
movq %rdi, 0x28(%rsp)
movl $0xffffffff, %edx       # imm = 0xFFFFFFFF
movl %edx, 0x30(%rsp)
callq 0x4faf0
movq 0x28(%rsp), %rax
movl 0x30(%rsp), %edx
movq %rax, 0x170(%rsp)
movq 0x198(%rsp), %rsi
leaq 0xe8(%rsp), %rdi
callq 0x4faf0
jmp 0x5220d
leaq 0xe8(%rsp), %rax
movq %rax, 0x128(%rsp)
movq 0x170(%rsp), %rdi
callq 0x3a330
movq %rax, 0x20(%rsp)
jmp 0x52231
movq 0x20(%rsp), %rax
movq %rax, 0xd0(%rsp)
movq 0x128(%rsp), %rdi
callq 0x3a330
movq %rax, 0x18(%rsp)
jmp 0x52252
movq 0x18(%rsp), %rax
movq %rax, 0xc8(%rsp)
movq 0x170(%rsp), %rdi
callq 0x44c70
movq %rax, 0x10(%rsp)
jmp 0x52273
movq 0x10(%rsp), %rax
movq %rax, 0xc0(%rsp)
movq 0x128(%rsp), %rdi
callq 0x44c70
movq %rax, 0x8(%rsp)
jmp 0x52294
movq 0x8(%rsp), %rax
movq %rax, 0xb8(%rsp)
movsd 0x178(%rsp), %xmm1
movsd 0x213d5e(%rip), %xmm0   # 0x266010
subsd %xmm1, %xmm0
movq 0xd0(%rsp), %rsi
leaq 0x88(%rsp), %rdi
callq 0x4a690
jmp 0x522cd
movsd 0x178(%rsp), %xmm0
movq 0xc8(%rsp), %rsi
leaq 0x70(%rsp), %rdi
callq 0x4a690
jmp 0x522ea
leaq 0xa0(%rsp), %rdi
leaq 0x88(%rsp), %rsi
leaq 0x70(%rsp), %rdx
callq 0x180c80
jmp 0x52306
movq 0xc0(%rsp), %rsi
movq 0xb8(%rsp), %rdx
movsd 0x178(%rsp), %xmm0
leaq 0x50(%rsp), %rdi
callq 0x15e4f0
jmp 0x5232b
movq 0x38(%rsp), %rdi
leaq 0xa0(%rsp), %rsi
leaq 0x50(%rsp), %rdx
callq 0x5e450
jmp 0x52344
leaq 0xe8(%rsp), %rdi
callq 0x5e680
leaq 0x130(%rsp), %rdi
callq 0x5e680
jmp 0x523a6
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xe0(%rsp)
movl %eax, 0xdc(%rsp)
jmp 0x52397
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xe0(%rsp)
movl %eax, 0xdc(%rsp)
leaq 0xe8(%rsp), %rdi
callq 0x5e680
leaq 0x130(%rsp), %rdi
callq 0x5e680
jmp 0x523b3
movq 0x40(%rsp), %rax
addq $0x1c8, %rsp            # imm = 0x1C8
retq
movq 0xe0(%rsp), %rdi
callq 0x29c10