ARCore中四元数的插值算法实现

 1 　　public static Quaternion makeInterpolated(Quaternion a, Quaternion b, float t) {
 2         Quaternion out = new Quaternion();
 3         float cosHalfTheta = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
 4         if(cosHalfTheta < 0.0F) {
 5             b = new Quaternion(b);
 6             cosHalfTheta = -cosHalfTheta;
 7             b.x = -b.x;
 8             b.y = -b.y;
 9             b.z = -b.z;
10             b.w = -b.w;
11         }
12 
13         float halfTheta = (float)Math.acos((double)cosHalfTheta);
14         float sinHalfTheta = (float)Math.sqrt((double)(1.0F - cosHalfTheta * cosHalfTheta));
15         float ratioA;
16         float ratioB;
17         if((double)Math.abs(sinHalfTheta) > 0.001D) {
18             float oneOverSinHalfTheta = 1.0F / sinHalfTheta;
19             ratioA = (float)Math.sin((double)((1.0F - t) * halfTheta)) * oneOverSinHalfTheta;
20             ratioB = (float)Math.sin((double)(t * halfTheta)) * oneOverSinHalfTheta;
21         } else {
22             ratioA = 1.0F - t;
23             ratioB = t;
24         }
25 
26         out.x = ratioA * a.x + ratioB * b.x;
27         out.y = ratioA * a.y + ratioB * b.y;
28         out.z = ratioA * a.z + ratioB * b.z;
29         out.w = ratioA * a.w + ratioB * b.w;
30         out.normalizeInPlace();
31         return out;
32     }