// helper function to run slerp with 2.0 as t value
Quaternion<double> Interpolator::Double(Quaternion<double> p, Quaternion<double> q)
{
// students should implement this
Quaternion<double> result;
double pDotq = q.Gets()*p.Gets() + q.Getx()*p.Getx() + q.Gety()*p.Gety() + q.Getz()*p.Getz();
result = (2*pDotq)*q - p;
return result;
}
// SLERP function for quaternions
Quaternion<double> Interpolator::Slerp(double t, Quaternion<double> & qStart, Quaternion<double> & qEnd_)
{
// students should implement this
Quaternion<double> result;
Quaternion<double> q3;
double qStartDotqEnd = qStart.Gets()*qEnd_.Gets() + qStart.Getx()*qEnd_.Getx() + qStart.Gety()*qEnd_.Gety() + qStart.Getz()*qEnd_.Getz();
// need to check for the great arc that is the smallest, and change the quaternion accordingly if chosen wrong one
if (qStartDotqEnd < 0)
{
q3 = -1*qEnd_;
qStartDotqEnd = -qStartDotqEnd;
}
else
{
q3 = qEnd_;
}
// cos(theta) = s1s2 + x1x2 + y1y2 + z1z2
double theta = acos(qStartDotqEnd);
if (sin(theta) == 0)
{
return qStart;
}
// slerp formula
result = (sin((1-t)*theta)/sin(theta))*qStart + (sin(t*theta)/sin(theta))*q3;
result.Normalize();
return result;
}