Rotation from Quaternions #167

Merged
meggert merged 1 commits from rotation-from-quaternions into develop 2024-01-29 14:11:10 +01:00
2 changed files with 26 additions and 0 deletions

View File

@ -153,3 +153,26 @@ double QuaternionOperations::getAngle(const double* quaternion, bool abs) {
}
}
}
void QuaternionOperations::rotationFromQuaternions(const double qNew[4], const double qOld[4],
const double timeDelta,
double rotRate[3]) {
double qOldInv[4] = {0, 0, 0, 0};
double qDelta[4] = {0, 0, 0, 0};
inverse(qOld, qOldInv);
multiply(qNew, qOldInv, qDelta);
if (VectorOperations<double>::norm(qDelta, 4) != 0.0) {
normalize(qDelta);
}
if (VectorOperations<double>::norm(qDelta, 3) == 0.0) {
rotRate[0] = 0.0;
rotRate[1] = 0.0;
rotRate[2] = 0.0;
return;
}
double rotVec[3] = {0, 0, 0};
double angle = getAngle(qDelta);
VectorOperations<double>::normalize(qDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotRate, 3);
}

View File

@ -25,6 +25,9 @@ class QuaternionOperations {
static void slerp(const double q1[4], const double q2[4], const double weight, double q[4]);
static void rotationFromQuaternions(const double qNew[4], const double qOld[4],
const double timeDelta, double rotRate[3]);
/**
* returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
*/