| 27 | } |
| 28 | |
| 29 | Quaternionf PoseEstimQueue::getQuatIn(float deltaT) { |
| 30 | return pos[idx] * (pos[mod3(idx - 1)].inverse() * pos[idx]) |
| 31 | .slerp(deltaT / avgDeltaT / 2 / t[idx], Quaternionf::Identity()); |
| 32 | // return pos[idx] * Quaternionf::Identity().slerp(deltaT / avgDeltaT / 2 / t[idx], |
| 33 | // (pos[mod3(idx - 1)].inverse() * pos[idx]).inverse()); |
| 34 | // * Quaternionf::Identity().slerp(deltaT * deltaT / avgDeltaT / avgDeltaT / 2 / t[idx], |
| 35 | // spd[mod3(idx - 1)].inverse() * spd[idx]); |
| 36 | } |
| 37 | |
| 38 | Quaternionf PVRMat34ToQuat(float (*mat)[3][4]) { |
| 39 | Matrix3f emat; |