Merge pull request 'Function to prevent Quaternion Sign Jump' (#171) from quat-no-sign-jump into develop
Reviewed-on: #171 Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
commit
43ea29cb84
@ -72,6 +72,15 @@ void QuaternionOperations::slerp(const double q1[4], const double q2[4], const d
|
||||
normalize(q);
|
||||
}
|
||||
|
||||
void QuaternionOperations::preventSignJump(double qNew[4], const double qOld[4]) {
|
||||
double qDiff[4] = {0, 0, 0, 0}, qSum[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(qOld, qNew, qDiff, 4);
|
||||
VectorOperations<double>::add(qOld, qNew, qSum, 4);
|
||||
if (VectorOperations<double>::norm(qDiff, 4) > VectorOperations<double>::norm(qSum, 4)) {
|
||||
VectorOperations<double>::mulScalar(qNew, -1, qNew, 4);
|
||||
}
|
||||
}
|
||||
|
||||
QuaternionOperations::QuaternionOperations() {}
|
||||
|
||||
void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) {
|
||||
|
@ -28,6 +28,8 @@ class QuaternionOperations {
|
||||
static void rotationFromQuaternions(const double qNew[4], const double qOld[4],
|
||||
const double timeDelta, double rotRate[3]);
|
||||
|
||||
static void preventSignJump(double qNew[4], const double qOld[4]);
|
||||
|
||||
/**
|
||||
* returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
|
||||
*/
|
||||
|
Loading…
Reference in New Issue
Block a user