fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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 13:27:58 +01:00
parent af2610ec04
commit f835d39017
3 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
matrix/Quaternion.hpp
View File

@ -189,18 +189,19 @@ public:
/**
* Quaternion from two vectors
* 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 src source vector (no need to normalize)
* @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>()
{
Quaternion &q = *this;
Vector3<Type> cr = src.cross(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) {
cr = src.abs();
if (cr(0) < cr(1)) {
@ -219,6 +220,7 @@ public:
q(0) = Type(0);
cr = src.cross(cr);
} else {
/* Half-Way Quaternion Solution */
q(0) = src.dot(dst) + sqrt(src.norm_squared() * dst.norm_squared());
}
q(1) = cr(0);

4
matrix/Vector.hpp
View File

@ -73,7 +73,7 @@ public:
Type norm_squared() const {
const Vector &a(*this);
return Type(a.dot(a));
return a.dot(a);
}
inline Type length() const {
@ -88,7 +88,7 @@ public:
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();
if (n > eps) {
return (*this) / n;

9
test/attitude.cpp
View File

@ -40,12 +40,11 @@ int main()
TEST(fabs(q(3) - 4) < eps);
// quaternion ctor: vector to vector
Vector3f v1(0.f, 0.f, 1.f);
// identity & default destination vector test
Quatf quat_v(v1);
TEST(isEqual(quat_v.conjugate(v1), v1));
// identity test
Quatf quat_v(v,v);
TEST(isEqual(quat_v.conjugate(v), v));
// 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);
TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v));
// special 180 degree case 1