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

时间:2021-07-25 01:56:35

ARCore中四元数差值算法:

其中t的取值范围为[0, 1],当 t = 0 时,结果为a;当t = 1 时,结果为b。

   public static Quaternion makeInterpolated(Quaternion a, Quaternion b, float t) {
Quaternion out = new Quaternion();
float cosHalfTheta = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
if(cosHalfTheta < 0.0F) {
b = new Quaternion(b);
cosHalfTheta = -cosHalfTheta;
b.x = -b.x;
b.y = -b.y;
b.z = -b.z;
b.w = -b.w;
} float halfTheta = (float)Math.acos((double)cosHalfTheta);
float sinHalfTheta = (float)Math.sqrt((double)(1.0F - cosHalfTheta * cosHalfTheta));
float ratioA;
float ratioB;
if((double)Math.abs(sinHalfTheta) > 0.001D) {
float oneOverSinHalfTheta = 1.0F / sinHalfTheta;
ratioA = (float)Math.sin((double)((1.0F - t) * halfTheta)) * oneOverSinHalfTheta;
ratioB = (float)Math.sin((double)(t * halfTheta)) * oneOverSinHalfTheta;
} else {
ratioA = 1.0F - t;
ratioB = t;
} out.x = ratioA * a.x + ratioB * b.x;
out.y = ratioA * a.y + ratioB * b.y;
out.z = ratioA * a.z + ratioB * b.z;
out.w = ratioA * a.w + ratioB * b.w;
out.normalizeInPlace();
return out;
}