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

Quaternion/Vector: Small refactor for review: put more comments, switched type conversions, took out default destination vector because confusing

Browse Source
This commit is contained in:
Matthias Grob
2017-12-04 15:34:13 +01:00
parent af2610ec04
commit f835d39017
3 changed files with 10 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Quaternion.hpp
+4 -2
View File
@@ -189,18 +189,19 @@ public:
/** /**
* Quaternion from two vectors * Quaternion from two vectors
* Generates shortest rotation from source to destination vector * Generates shortest rotation from source to destination vector
* Default destination if not specified is [0,0,1]
* *
* @param dst destination vector (no need to normalize) * @param dst destination vector (no need to normalize)
* @param src source vector (no need to normalize) * @param src source vector (no need to normalize)
* @param eps epsilon threshold which decides if a value is considered zero * @param eps epsilon threshold which decides if a value is considered zero
*/ */
Quaternion(const Vector3<Type> &src, const Vector<Type, 3> dst = Vector3<Type>(0, 0, 1), const Type eps = 1e-5f) : Quaternion(const Vector3<Type> &src, const Vector3<Type> &dst, const Type eps = Type(1e-5)) :
Vector<Type, 4>() Vector<Type, 4>()
{ {
Quaternion &q = *this; Quaternion &q = *this;
Vector3<Type> cr = src.cross(dst); Vector3<Type> cr = src.cross(dst);
float dt = src.dot(dst); float dt = src.dot(dst);
/* If the two vectors are parallel, cross product is zero
* If they point opposite, the dot product is negative */
if (cr.norm() < eps && dt < 0) { if (cr.norm() < eps && dt < 0) {
cr = src.abs(); cr = src.abs();
if (cr(0) < cr(1)) { if (cr(0) < cr(1)) {
@@ -219,6 +220,7 @@ public:
q(0) = Type(0); q(0) = Type(0);
cr = src.cross(cr); cr = src.cross(cr);
} else { } else {
/* Half-Way Quaternion Solution */
q(0) = src.dot(dst) + sqrt(src.norm_squared() * dst.norm_squared()); q(0) = src.dot(dst) + sqrt(src.norm_squared() * dst.norm_squared());
} }
q(1) = cr(0); q(1) = cr(0);
matrix/Vector.hpp
+2 -2
View File
@@ -73,7 +73,7 @@ public:
Type norm_squared() const { Type norm_squared() const {
const Vector &a(*this); const Vector &a(*this);
return Type(a.dot(a)); return a.dot(a);
} }
inline Type length() const { inline Type length() const {
@@ -88,7 +88,7 @@ public:
return (*this) / norm(); return (*this) / norm();
} }
Vector unit_or_zero(const Type eps = 1e-5f) { Vector unit_or_zero(const Type eps = Type(1e-5)) {
const Type n = norm(); const Type n = norm();
if (n > eps) { if (n > eps) {
return (*this) / n; return (*this) / n;
test/attitude.cpp
+4 -5
View File
@@ -40,12 +40,11 @@ int main()
TEST(fabs(q(3) - 4) < eps); TEST(fabs(q(3) - 4) < eps);
// quaternion ctor: vector to vector // quaternion ctor: vector to vector
Vector3f v1(0.f, 0.f, 1.f); // identity test
// identity & default destination vector test Quatf quat_v(v,v);
Quatf quat_v(v1); TEST(isEqual(quat_v.conjugate(v), v));
TEST(isEqual(quat_v.conjugate(v1), v1));
// random test (vector norm can not be preserved with a pure rotation) // random test (vector norm can not be preserved with a pure rotation)
v1 = Vector3f(-80.1f, 1.5f, -6.89f); Vector3f v1(-80.1f, 1.5f, -6.89f);
quat_v = Quatf(v1, v); quat_v = Quatf(v1, v);
TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v)); TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v));
// special 180 degree case 1 // special 180 degree case 1