43 q0[0] * q1[0] + q0[1] * q1[1] + q0[2] * q1[2] + q0[3] * q1[3];
45 if (std::abs(cosHalfTheta) >= 1.0)
50 bool reverse_q1 =
false;
54 cosHalfTheta = -cosHalfTheta;
57 const double halfTheta = acos(cosHalfTheta);
58 const double sinHalfTheta = std::sqrt(1.0 -
mrpt::square(cosHalfTheta));
61 if (std::abs(sinHalfTheta) < 0.001)
64 for (
int i = 0; i < 4; i++)
q[i] = (1 -
t) * q0[i] +
t * q1[i];
66 for (
int i = 0; i < 4; i++)
q[i] = (1 -
t) * q0[i] -
t * q1[i];
69 const double A = sin((1 -
t) * halfTheta) / sinHalfTheta;
70 const double B = sin(
t * halfTheta) / sinHalfTheta;
72 for (
int i = 0; i < 4; i++)
q[i] = A * q0[i] + B * q1[i];
74 for (
int i = 0; i < 4; i++)
q[i] = A * q0[i] - B * q1[i];
85 void slerp(
const TPose3D& q0,
const TPose3D& q1,
const double t, TPose3D&
p);