// File: tomfunkhouser[P]gaps/pkgs/R3Shapes/R3Quaternion.cpp
// Function: gaps::R3QuaternionSlerp(gaps::R3Quaternion const&, gaps::R3Quaternion const&, double)
R3Quaternion 
R3QuaternionSlerp(const R3Quaternion& quaternion1, const R3Quaternion& quaternion2, RNScalar t)
{
  // Get convenient variables
  RNScalar q1[4] = { quaternion1[0], quaternion1[1],  quaternion1[2], quaternion1[3] };  
  RNScalar q2[4] = { quaternion2[0], quaternion2[1],  quaternion2[2], quaternion2[3] };  

  // Make sure going short way around sphere (q2 is same rotation as -q2)
  RNScalar dot = q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3];
  if (dot < 0) { 
    q2[0] = -q2[0]; 
    q2[1] = -q2[1]; 
    q2[2] = -q2[2]; 
    q2[3] = -q2[3]; 
    dot = -dot; 
  }

  // Compute/check angle on 4D sphere
  RNAngle theta = (dot < 1) ? acos(dot) : 0;
  RNScalar denom = sin(theta);
  if (RNIsZero(denom)) {
    // Small angle, use linear interpolation
    RNScalar q[4];
    q[0] = (1-t)*q1[0] + t*q2[0];
    q[1] = (1-t)*q1[1] + t*q2[1];
    q[2] = (1-t)*q1[2] + t*q2[2];
    q[3] = (1-t)*q1[3] + t*q2[3];
    return R3Quaternion(q);
  }
  else {
    // Spherical interpolation
    RNScalar q[4];
    RNScalar t1 = sin((1.0-t)*theta) / denom;
    RNScalar t2 = sin(t*theta) / denom;
    q[0] = t1*q1[0] + t2*q2[0];
    q[1] = t1*q1[1] + t2*q2[1];
    q[2] = t1*q1[2] + t2*q2[2];
    q[3] = t1*q1[3] + t2*q2[3];
    return R3Quaternion(q);
  }
}

// Assembly:
subq $0xf8, %rsp
movq %rdi, 0x10(%rsp)
movq %rdi, %rax
movq %rax, 0x18(%rsp)
movq %rdi, 0xf0(%rsp)
movq %rsi, 0xe8(%rsp)
movq %rdx, 0xe0(%rsp)
movsd %xmm0, 0xd8(%rsp)
movq 0xe8(%rsp), %rdi
xorl %esi, %esi
callq 0x51af0
movsd %xmm0, 0xb0(%rsp)
movq 0xe8(%rsp), %rdi
movl $0x1, %esi
callq 0x51af0
movsd %xmm0, 0xb8(%rsp)
movq 0xe8(%rsp), %rdi
movl $0x2, %esi
callq 0x51af0
movsd %xmm0, 0xc0(%rsp)
movq 0xe8(%rsp), %rdi
movl $0x3, %esi
callq 0x51af0
movsd %xmm0, 0xc8(%rsp)
movq 0xe0(%rsp), %rdi
xorl %esi, %esi
callq 0x51af0
movsd %xmm0, 0x90(%rsp)
movq 0xe0(%rsp), %rdi
movl $0x1, %esi
callq 0x51af0
movsd %xmm0, 0x98(%rsp)
movq 0xe0(%rsp), %rdi
movl $0x2, %esi
callq 0x51af0
movsd %xmm0, 0xa0(%rsp)
movq 0xe0(%rsp), %rdi
movl $0x3, %esi
callq 0x51af0
movsd %xmm0, 0xa8(%rsp)
movsd 0xb0(%rsp), %xmm0
movsd 0x90(%rsp), %xmm2
movsd 0xb8(%rsp), %xmm1
mulsd 0x98(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd 0xc0(%rsp), %xmm1
movsd 0xa0(%rsp), %xmm2
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movsd 0xc8(%rsp), %xmm0
movsd 0xa8(%rsp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x88(%rsp)
xorps %xmm0, %xmm0
ucomisd 0x88(%rsp), %xmm0
jbe 0x4faef
movsd 0x90(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, 0x90(%rsp)
movsd 0x98(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, 0x98(%rsp)
movsd 0xa0(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, 0xa0(%rsp)
movsd 0xa8(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, 0xa8(%rsp)
movsd 0x88(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, 0x88(%rsp)
movsd 0xe3511(%rip), %xmm0    # 0x133008
ucomisd 0x88(%rsp), %xmm0
jbe 0x4fb18
movsd 0x88(%rsp), %xmm0
callq 0xf470
movsd %xmm0, 0x8(%rsp)
jmp 0x4fb23
xorps %xmm0, %xmm0
movsd %xmm0, 0x8(%rsp)
jmp 0x4fb23
movsd 0x8(%rsp), %xmm0
movsd %xmm0, 0x80(%rsp)
movsd 0x80(%rsp), %xmm0
callq 0xf100
movsd %xmm0, 0x78(%rsp)
movsd 0x78(%rsp), %xmm0
leaq 0x12898d(%rip), %rax    # 0x1784e0
movsd (%rax), %xmm1
callq 0x36b80
cmpl $0x0, %eax
je 0x4fc61
movq 0x10(%rsp), %rdi
movsd 0xe3496(%rip), %xmm0    # 0x133008
subsd 0xd8(%rsp), %xmm0
movsd 0xb0(%rsp), %xmm2
movsd 0xd8(%rsp), %xmm1
mulsd 0x90(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x50(%rsp)
movsd 0xe345c(%rip), %xmm0    # 0x133008
subsd 0xd8(%rsp), %xmm0
movsd 0xb8(%rsp), %xmm2
movsd 0xd8(%rsp), %xmm1
mulsd 0x98(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x58(%rsp)
movsd 0xe3422(%rip), %xmm0    # 0x133008
subsd 0xd8(%rsp), %xmm0
movsd 0xc0(%rsp), %xmm2
movsd 0xd8(%rsp), %xmm1
mulsd 0xa0(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x60(%rsp)
movsd 0xe33e8(%rip), %xmm0    # 0x133008
subsd 0xd8(%rsp), %xmm0
movsd 0xc8(%rsp), %xmm2
movsd 0xd8(%rsp), %xmm1
mulsd 0xa8(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x68(%rsp)
leaq 0x50(%rsp), %rsi
callq 0x50890
jmp 0x4fd6e
movsd 0xe339f(%rip), %xmm0    # 0x133008
subsd 0xd8(%rsp), %xmm0
mulsd 0x80(%rsp), %xmm0
callq 0xf100
divsd 0x78(%rsp), %xmm0
movsd %xmm0, 0x28(%rsp)
movsd 0xd8(%rsp), %xmm0
mulsd 0x80(%rsp), %xmm0
callq 0xf100
movq 0x10(%rsp), %rdi
divsd 0x78(%rsp), %xmm0
movsd %xmm0, 0x20(%rsp)
movsd 0x28(%rsp), %xmm0
movsd 0xb0(%rsp), %xmm2
movsd 0x20(%rsp), %xmm1
mulsd 0x90(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x30(%rsp)
movsd 0x28(%rsp), %xmm0
movsd 0xb8(%rsp), %xmm2
movsd 0x20(%rsp), %xmm1
mulsd 0x98(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x38(%rsp)
movsd 0x28(%rsp), %xmm0
movsd 0xc0(%rsp), %xmm2
movsd 0x20(%rsp), %xmm1
mulsd 0xa0(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x40(%rsp)
movsd 0x28(%rsp), %xmm0
movsd 0xc8(%rsp), %xmm2
movsd 0x20(%rsp), %xmm1
mulsd 0xa8(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x48(%rsp)
leaq 0x30(%rsp), %rsi
callq 0x50890
movq 0x18(%rsp), %rax
addq $0xf8, %rsp
retq
nopl (%rax,%rax)