Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: added normalize() method to Matrix3f

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-02 18:10:00 +10:00
parent 914b91af9e
commit 24b051565b
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Math/matrix3.cpp
View File

@ -117,6 +117,21 @@ void Matrix3<T>::rotateXYinv(const Vector3<T> &g)
(*this) += temp_matrix;
}
/*
re-normalise a rotation matrix
*/
template <typename T>
void Matrix3<T>::normalize(void)
{
float error = a * b;
Vector3<T> t0 = a - (b * (0.5f * error));
Vector3<T> t1 = b - (a * (0.5f * error));
Vector3<T> t2 = t0 % t1;
a = t0 * (1.0f / t0.length());
b = t1 * (1.0f / t1.length());
c = t2 * (1.0f / t2.length());
}
// multiplication by a vector
template <typename T>
Vector3<T> Matrix3<T>::operator *(const Vector3<T> &v) const
@ -181,6 +196,7 @@ template void Matrix3<float>::zero(void);
template void Matrix3<float>::rotate(const Vector3<float> &g);
template void Matrix3<float>::rotateXY(const Vector3<float> &g);
template void Matrix3<float>::rotateXYinv(const Vector3<float> &g);
template void Matrix3<float>::normalize(void);
template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw) const;
template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) const;

3
libraries/AP_Math/matrix3.h
View File

@ -219,6 +219,9 @@ public:
// apply an additional inverse rotation to a rotation matrix but
// only use X, Y elements from rotation vector
void rotateXYinv(const Vector3<T> &g);
// normalize a rotation matrix
void normalize(void);
};
typedef Matrix3<int16_t> Matrix3i;