aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use more correct implementation of toEulerZYX fixing some yaw issues

Browse Source 
We actually need to use Tait–Bryan Z-Y-X angles, not classic Euler's
This commit is contained in:
Oleg Kalachev 2024-02-20 04:47:13 +03:00
parent 455729fdb4
commit 85182ac2b8
1 changed files with 22 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
flix/quaternion.h
View File

@ -68,10 +68,28 @@ public:
} }
Vector toEulerZYX() const { Vector toEulerZYX() const {
return Vector( // https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L87
atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)), Vector euler;
asin(2 * (w * y - z * x)), float sqx = x * x;
atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))); float sqy = y * y;
float sqz = z * z;
float sqw = w * w;
// Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
float sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
if (sarg <= -0.99999) {
euler.x = 0;
euler.y = -0.5 * PI;
euler.z = -2 * atan2(y, x);
} else if (sarg >= 0.99999) {
euler.x = 0;
euler.y = 0.5 * PI;
euler.z = 2 * atan2(y, x);
} else {
euler.x = atan2(2 * (y * z + w * x), sqw - sqx - sqy + sqz);
euler.y = asin(sarg);
euler.z = atan2(2 * (x * y + w * z), sqw + sqx - sqy - sqz);
}
return euler;
} }
float getYaw() const { float getYaw() const {