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

Added unit function that returns normalized vector.

Browse Source
This commit is contained in:
James Goppert 2016-03-15 18:09:48 -04:00
parent 0e14e11183
commit 711b57d2d4
2 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
matrix/Vector.hpp
View File

@ -66,8 +66,12 @@ public:
return Type(sqrt(a.dot(a)));
}
Vector normalize() const {
return (*this) / norm();
inline void normalize() {
(*this) /= norm();
}
Vector unit() const {
return (*this) / norm();
}
Vector pow(Type v) const {

6
test/attitude.cpp
View File

@ -42,16 +42,18 @@ int main()
TEST(isEqual(euler_copy, euler_check));
// quaternion ctor
Quatf q(1, 2, 3, 4);
Quatf q0(1, 2, 3, 4);
Quatf q(q0);
TEST(fabs(q(0) - 1) < eps);
TEST(fabs(q(1) - 2) < eps);
TEST(fabs(q(2) - 3) < eps);
TEST(fabs(q(3) - 4) < eps);
// quat normalization
q = q.normalize();
q.normalize();
TEST(isEqual(q, Quatf(0.18257419f, 0.36514837f,
0.54772256f, 0.73029674f)));
TEST(isEqual(q0.unit(), q));
// quat default ctor
q = Quatf();