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

Add helpers based on Slice: row(), col(), xy()

Browse Source
This commit is contained in:
Julian Kent 2019-09-16 15:55:51 +02:00
parent 82d565f4d9
commit b817e8677d
5 changed files with 102 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
matrix/Dcm.hpp
View File

@ -166,9 +166,9 @@ public:
void renormalize()
{
/* renormalize rows */
for (size_t row = 0; row < 3; row++) {
matrix::Vector3f rvec(this->_data[row]);
this->setRow(row, rvec.normalized());
for (size_t r = 0; r < 3; r++) {
matrix::Vector3<Type> rvec(Matrix<Type,1,3>(this->Matrix<Type,3,3>::row(r)).transpose());
this->Matrix<Type,3,3>::row(r) = rvec.normalized();
}
}
};

35
matrix/Matrix.hpp
View File

@ -381,20 +381,35 @@ public:
return Slice<Type, P, Q, M, N>(x0, y0, this);
}
void setRow(size_t i, const Matrix<Type, N, 1> &row)
const Slice<Type, 1, N, M, N> row(size_t i) const
{
Matrix<Type, M, N> &self = *this;
for (size_t j = 0; j < N; j++) {
self(i, j) = row(j, 0);
}
return slice<1, N>(i,0);
}
void setCol(size_t j, const Matrix<Type, M, 1> &col)
Slice<Type, 1, N, M, N> row(size_t i)
{
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
self(i, j) = col(i, 0);
}
return slice<1, N>(i,0);
}
const Slice<Type, M, 1, M, N> col(size_t j) const
{
return slice<M, 1>(0,j);
}
Slice<Type, M, 1, M, N> col(size_t j)
{
return slice<M, 1>(0,j);
}
void setRow(size_t i, const Matrix<Type, N, 1> &row_in)
{
slice<1,N>(i,0) = row_in.transpose();
}
void setCol(size_t j, const Matrix<Type, M, 1> &column)
{
slice<M,1>(0,j) = column;
}
void setZero()

10
matrix/Vector3.hpp
View File

@ -109,6 +109,16 @@ public:
return unit();
}
const Slice<Type, 2, 1, 3, 1> xy() const
{
return Slice<Type, 2, 1, 3, 1>(0, 0, this);
}
Slice<Type, 2, 1, 3, 1> xy()
{
return Slice<Type, 2, 1, 3, 1>(0, 0, this);
}
Dcm<Type> hat() const { // inverse to Dcm.vee() operation
const Vector3 &v(*this);

41
test/slice.cpp
View File

@ -56,6 +56,47 @@ int main()
Matrix<float, 3, 3> D(data_2_check);
TEST(isEqual(A, D));
//Test writing to slices
Matrix<float, 3, 1> E;
E(0,0) = -1;
E(1,0) = 1;
E(2,0) = 3;
Matrix<float, 2, 1> F;
F(0,0) = 9;
F(1,0) = 11;
E.slice<2,1>(0,0) = F;
float data_3_check[3] = {9, 11, 3};
Matrix<float, 3, 1> G (data_3_check);
TEST(isEqual(E, G));
TEST(isEqual(E, Matrix<float,3,1>(E.slice<3,1>(0,0))));
Matrix<float, 2, 1> H = E.slice<2,1>(0,0);
TEST(isEqual(H,F));
float data_4_check[5] = {3, 11, 9, 0, 0};
{ // assigning row slices to each other
const Matrix<float, 3, 1> J (data_3_check);
Matrix<float, 5, 1> K;
K.row(2) = J.row(0);
K.row(1) = J.row(1);
K.row(0) = J.row(2);
Matrix<float, 5, 1> K_check(data_4_check);
TEST(isEqual(K, K_check));
}
{ // assigning col slices to each other
const Matrix<float, 1, 3> J (data_3_check);
Matrix<float, 1, 5> K;
K.col(2) = J.col(0);
K.col(1) = J.col(1);
K.col(0) = J.col(2);
Matrix<float, 1, 5> K_check(data_4_check);
TEST(isEqual(K, K_check));
}
return 0;
}

23
test/vector3.cpp
View File

@ -27,6 +27,29 @@ int main()
TEST(isEqual(a.unit(), a.normalized()));
TEST(isEqual(a*2.0, Vector3f(2, 0, 0)));
Vector2f g2(1,3);
Vector3f g3(7, 11, 17);
g3.xy() = g2;
TEST(isEqual(g3, Vector3f(1, 3, 17)));
const Vector3f g4(g3);
Vector2f g5 = g4.xy();
TEST(isEqual(g5,g2));
TEST(isEqual(g2,Vector2f(g4.xy())));
Vector3f h;
TEST(isEqual(h,Vector3f(0,0,0)));
Vector<float, 4> j;
j(0) = 1;
j(1) = 2;
j(2) = 3;
j(3) = 4;
Vector3f k = j.slice<3,1>(0,0);
Vector3f k_test(1,2,3);
TEST(isEqual(k,k_test));
return 0;
}