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

Matrix: add copyTo copying data to an array and copyToColumnMajor which does the same but with column-major order

Browse Source 
same functionality explicitly for quaternions can be deleted
This commit is contained in:
Matthias Grob 2017-11-15 22:15:13 +01:00
parent 3bd94fcd6f
commit 1dffd5930b
2 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
matrix/Matrix.hpp
View File

@ -82,6 +82,22 @@ public:
return (*this);
}
void copyTo(Type (&dst)[M*N]) const
{
memcpy(dst, _data, sizeof(dst));
}
void copyToColumnMajor(Type (&dst)[M*N]) const
{
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
dst[i+(j*M)] = self(i, j);
}
}
}
/**
* Matrix Operations
*/

14
matrix/Quaternion.hpp
View File

@ -260,20 +260,6 @@ public:
q = q * scalar;
}
/**
* Copy quaternion to a float array
*
* @param dst array of 4 floats
*/
void copyTo(float (&dst)[4])
{
const Quaternion &q = *this;
dst[0] = q(0);
dst[1] = q(1);
dst[2] = q(2);
dst[3] = q(3);
}
/**
* Computes the derivative of q_21 when
* rotated with angular velocity expressed in frame 1