fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 05:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Quaternion: rename function to rotate vectors

Browse Source
This commit is contained in:
Matthias Grob
2022-03-07 20:03:54 -05:00
committed by Daniel Agar
parent 5affa693f2
commit 68a0414622
7 changed files with 27 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/matrix/Quaternion.hpp
+2 -2
View File
@@ -485,7 +485,7 @@ public:
* @param vec vector to rotate in frame 1 (typically body frame)
* @return rotated vector in frame 2 (typically reference frame)
*/
Vector3<Type> conjugate(const Vector3<Type> &vec) const
Vector3<Type> rotateVector(const Vector3<Type> &vec) const
{
const Quaternion &q = *this;
Quaternion v(Type(0), vec(0), vec(1), vec(2));
@@ -502,7 +502,7 @@ public:
* @param vec vector to rotate in frame 2 (typically reference frame)
* @return rotated vector in frame 1 (typically body frame)
*/
Vector3<Type> conjugate_inversed(const Vector3<Type> &vec) const
Vector3<Type> rotateVectorInverse(const Vector3<Type> &vec) const
{
const Quaternion &q = *this;
Quaternion v(Type(0), vec(0), vec(1), vec(2));
src/lib/matrix/test/attitude.cpp
+8 -8
View File
@@ -57,27 +57,27 @@ int main()
// quaternion ctor: vector to vector
// identity test
Quatf quat_v(v, v);
TEST(isEqual(quat_v.conjugate(v), v));
TEST(isEqual(quat_v.rotateVector(v), v));
// random test (vector norm can not be preserved with a pure rotation)
Vector3f v1(-80.1f, 1.5f, -6.89f);
quat_v = Quatf(v1, v);
TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v));
TEST(isEqual(quat_v.rotateVector(v1).normalized() * v.norm(), v));
// special 180 degree case 1
v1 = Vector3f(0.f, 1.f, 1.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
TEST(isEqual(quat_v.rotateVector(v1), -v1));
// special 180 degree case 2
v1 = Vector3f(1.f, 2.f, 0.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
TEST(isEqual(quat_v.rotateVector(v1), -v1));
// special 180 degree case 3
v1 = Vector3f(0.f, 0.f, 1.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
TEST(isEqual(quat_v.rotateVector(v1), -v1));
// special 180 degree case 4
v1 = Vector3f(1.f, 1.f, 1.f);
quat_v = Quatf(v1, -v1);
TEST(isEqual(quat_v.conjugate(v1), -v1));
TEST(isEqual(quat_v.rotateVector(v1), -v1));
// quat normalization
q.normalize();
@@ -447,8 +447,8 @@ int main()
// conjugate
v = Vector3f(1.5f, 2.2f, 3.2f);
TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));
TEST(isEqual(q.rotateVectorInverse(v1), Dcmf(q).T()*v1));
TEST(isEqual(q.rotateVector(v1), Dcmf(q)*v1));
AxisAnglef aa_q_init(q);
TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
src/lib/matrix/test/dual.cpp
+1 -1
View File
@@ -28,7 +28,7 @@ Vector<Scalar, 3> positionError(const Vector<Scalar, 3> &positionState,
Scalar dt
)
{
return positionMeasurement - (positionState + bodyOrientation.conjugate(velocityStateBody) * dt);
return positionMeasurement - (positionState + bodyOrientation.rotateVector(velocityStateBody) * dt);
}
int main()