41 q0[0] * q1[0] + q0[1] * q1[1] + q0[2] * q1[2] + q0[3] * q1[3];
43 if (std::abs(cosHalfTheta) >= 1.0)
48 bool reverse_q1 =
false;
52 cosHalfTheta = -cosHalfTheta;
55 const double halfTheta = acos(cosHalfTheta);
56 const double sinHalfTheta = std::sqrt(1.0 -
mrpt::square(cosHalfTheta));
59 if (std::abs(sinHalfTheta) < 0.001)
62 for (
int i = 0; i < 4; i++) q[i] = (1 - t) * q0[i] + t * q1[i];
64 for (
int i = 0; i < 4; i++) q[i] = (1 - t) * q0[i] - t * q1[i];
67 const double A = sin((1 - t) * halfTheta) / sinHalfTheta;
68 const double B = sin(t * halfTheta) / sinHalfTheta;
70 for (
int i = 0; i < 4; i++) q[i] =
A * q0[i] + B * q1[i];
72 for (
int i = 0; i < 4; i++) q[i] =
A * q0[i] - B * q1[i];
83 void slerp(
const TPose3D& q0,
const TPose3D& q1,
const double t, TPose3D& p);