// File: /workspace/github_cpp_makefile_star2_O3/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:
pushq %rbx
subq $0x80, %rsp
movaps %xmm0, 0x30(%rsp)
movq %rdi, %rbx
movupd (%rsi), %xmm1
movupd 0x10(%rsi), %xmm2
movupd (%rdx), %xmm4
movupd 0x10(%rdx), %xmm3
movapd %xmm1, %xmm0
mulpd %xmm4, %xmm0
unpckhpd %xmm0, %xmm0            # xmm0 = xmm0[1,1]
movapd %xmm1, 0x60(%rsp)
mulsd %xmm4, %xmm1
addsd %xmm0, %xmm1
movapd %xmm2, %xmm0
mulsd %xmm3, %xmm0
addsd %xmm1, %xmm0
movapd %xmm2, 0x70(%rsp)
movapd %xmm2, %xmm1
unpckhpd %xmm2, %xmm1            # xmm1 = xmm1[1],xmm2[1]
movapd %xmm3, %xmm2
unpckhpd %xmm3, %xmm2            # xmm2 = xmm2[1],xmm3[1]
mulsd %xmm1, %xmm2
addsd %xmm0, %xmm2
movapd 0x76d93(%rip), %xmm1    # 0xa2020
movapd %xmm2, %xmm0
xorpd %xmm1, %xmm0
ucomisd %xmm2, %xmm0
maxsd %xmm2, %xmm0
ja 0x2b2a7
movapd %xmm4, 0x10(%rsp)
jmp 0x2b2b5
xorpd %xmm1, %xmm4
movapd %xmm4, 0x10(%rsp)
xorpd %xmm1, %xmm3
movapd %xmm3, 0x20(%rsp)
xorpd %xmm2, %xmm2
movsd 0x76d71(%rip), %xmm1    # 0xa2038
ucomisd %xmm0, %xmm1
jbe 0x2b2d6
callq 0xe3f0
movapd %xmm0, %xmm2
movsd %xmm2, 0x8(%rsp)
movapd %xmm2, %xmm0
callq 0xe0f0
movapd %xmm0, %xmm2
leaq 0xba8a8(%rip), %rax     # 0xe5b98
movsd (%rax), %xmm0
movapd 0x76d24(%rip), %xmm1    # 0xa2020
xorpd %xmm0, %xmm1
ucomisd %xmm1, %xmm2
movsd 0x76d2c(%rip), %xmm3    # 0xa2038
subsd 0x30(%rsp), %xmm3
jb 0x2b322
ucomisd %xmm2, %xmm0
jb 0x2b322
unpcklpd 0x30(%rsp), %xmm3       # xmm3 = xmm3[0],mem[0]
jmp 0x2b366
mulsd 0x8(%rsp), %xmm3
movapd %xmm3, %xmm0
movapd %xmm2, 0x50(%rsp)
callq 0xe0f0
movapd %xmm0, 0x40(%rsp)
movapd 0x30(%rsp), %xmm0
mulsd 0x8(%rsp), %xmm0
callq 0xe0f0
movapd 0x40(%rsp), %xmm3
unpcklpd %xmm0, %xmm3            # xmm3 = xmm3[0],xmm0[0]
movapd 0x50(%rsp), %xmm0
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
divpd %xmm0, %xmm3
movapd 0x20(%rsp), %xmm1
movapd 0x10(%rsp), %xmm2
movapd %xmm3, %xmm0
unpckhpd %xmm3, %xmm0            # xmm0 = xmm0[1],xmm3[1]
mulpd %xmm0, %xmm2
mulpd %xmm0, %xmm1
unpcklpd %xmm3, %xmm3            # xmm3 = xmm3[0,0]
movapd 0x60(%rsp), %xmm0
mulpd %xmm3, %xmm0
addpd %xmm2, %xmm0
movupd %xmm0, (%rbx)
mulpd 0x70(%rsp), %xmm3
addpd %xmm1, %xmm3
movupd %xmm3, 0x10(%rbx)
movq %rbx, %rdi
callq 0x2b146
movq %rbx, %rax
addq $0x80, %rsp
popq %rbx
retq
nop