mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
40 lines
907 B
C++
40 lines
907 B
C++
#include "Quaternion.hpp"
|
|
#include <cassert>
|
|
#include <cstdio>
|
|
|
|
using namespace matrix;
|
|
|
|
int main()
|
|
{
|
|
// test default ctor
|
|
Quatf q;
|
|
assert(q(0) == 1);
|
|
assert(q(1) == 0);
|
|
assert(q(2) == 0);
|
|
assert(q(3) == 0);
|
|
|
|
q = Quatf(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
|
|
assert(q(0) == 0.1825742f);
|
|
assert(q(1) == 0.3651484f);
|
|
assert(q(2) == 0.5477226f);
|
|
assert(q(3) == 0.7302967f);
|
|
|
|
// test euler ctor
|
|
q = Quatf(Eulerf(0.1f, 0.2f, 0.3f));
|
|
assert((q - Quatf(0.983347f, 0.034271f, 0.106021f, 0.143572f)).norm() < 1e-5);
|
|
|
|
// test dcm ctor
|
|
q = Quatf(Dcmf());
|
|
assert(q == Quatf(1.0f, 0.0f, 0.0f, 0.0f));
|
|
// TODO test derivative
|
|
// test accessors
|
|
q(0) = 0.1f;
|
|
q(1) = 0.2f;
|
|
q(2) = 0.3f;
|
|
q(3) = 0.4f;
|
|
assert(q == Quatf(0.1f, 0.2f, 0.3f, 0.4f));
|
|
return 0;
|
|
}
|
|
|
|
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|