mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
34 lines
751 B
C++
34 lines
751 B
C++
#include "test_macros.hpp"
|
|
#include <matrix/math.hpp>
|
|
|
|
using matrix::Matrix3f;
|
|
using matrix::eye;
|
|
|
|
int main()
|
|
{
|
|
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
|
|
Matrix3f A(data);
|
|
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
|
|
Matrix3f A_I(data_check);
|
|
Matrix3f I;
|
|
I.setIdentity();
|
|
Matrix3f R = A * A_I;
|
|
TEST(isEqual(R, I));
|
|
|
|
Matrix3f R2 = A;
|
|
R2 *= A_I;
|
|
TEST(isEqual(R2, I));
|
|
|
|
|
|
Matrix3f A2 = eye<float, 3>()*2;
|
|
Matrix3f B = A2.emult(A2);
|
|
Matrix3f B_check = eye<float, 3>()*4;
|
|
Matrix3f C_check = eye<float, 3>()*2;
|
|
TEST(isEqual(B, B_check));
|
|
Matrix3f C = B_check.edivide(C_check);
|
|
TEST(isEqual(C, C_check));
|
|
return 0;
|
|
}
|
|
|
|
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|