yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

Rebase of changes to sensor_hil_fixedwing branch.

Browse Source
This commit is contained in:
James Goppert
2013-01-13 17:09:22 -05:00
parent 0fdf962356
commit 464c5245f2
16 changed files with 618 additions and 256 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
apps/mathlib/math/Quaternion.cpp
View File

@@ -79,32 +79,34 @@ Quaternion::Quaternion(const Vector &v) :
Quaternion::Quaternion(const Dcm &dcm) :
Vector(4)
{
setA(0.5f * sqrtf(1 + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
setB((dcm(2, 1) - dcm(1, 2)) /
(4 * getA()));
setC((dcm(0, 2) - dcm(2, 0)) /
(4 * getA()));
setD((dcm(1, 0) - dcm(0, 1)) /
(4 * getA()));
// avoiding singularities by not using
// division equations
setA(0.5 * sqrt(1.0 +
double( dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
setB(0.5 * sqrt(1.0 +
double( dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
setC(0.5 * sqrt(1.0 +
double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
setD(0.5 * sqrt(1.0 +
double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
}
Quaternion::Quaternion(const EulerAngles &euler) :
Vector(4)
{
float cosPhi_2 = cosf(euler.getPhi() / 2.0f);
float cosTheta_2 = cosf(euler.getTheta() / 2.0f);
float cosPsi_2 = cosf(euler.getPsi() / 2.0f);
float sinPhi_2 = sinf(euler.getPhi() / 2.0f);
float sinTheta_2 = sinf(euler.getTheta() / 2.0f);
float sinPsi_2 = sinf(euler.getPsi() / 2.0f);
double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2);
setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
cosPhi_2 * sinTheta_2 * sinPsi_2);
setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
sinPhi_2 * cosTheta_2 * sinPsi_2);
setD(cosPhi_2 * cosTheta_2 * sinPsi_2 +
setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
sinPhi_2 * sinTheta_2 * cosPsi_2);
}
@@ -147,33 +149,24 @@ int __EXPORT quaternionTest()
ASSERT(equal(q.getC(), 0));
ASSERT(equal(q.getD(), 0));
// test float ctor
q = Quaternion(0, 1, 0, 0);
ASSERT(equal(q.getA(), 0));
ASSERT(equal(q.getB(), 1));
ASSERT(equal(q.getC(), 0));
ASSERT(equal(q.getD(), 0));
q = Quaternion(0.1825742, 0.3651484, 0.5477226, 0.7302967);
ASSERT(equal(q.getA(), 0.1825742));
ASSERT(equal(q.getB(), 0.3651484));
ASSERT(equal(q.getC(), 0.5477226));
ASSERT(equal(q.getD(), 0.7302967));
// test euler ctor
q = Quaternion(EulerAngles(0, 0, 0));
ASSERT(equal(q.getA(), 1));
ASSERT(equal(q.getB(), 0));
ASSERT(equal(q.getC(), 0));
ASSERT(equal(q.getD(), 0));
q = Quaternion(EulerAngles(0.1, 0.2, 0.3));
ASSERT(vectorEqual(q, Quaternion(0.983347, 0.034271, 0.106021, 0.143572)));
// test dcm ctor
q = Quaternion(Dcm());
ASSERT(equal(q.getA(), 1));
ASSERT(equal(q.getB(), 0));
ASSERT(equal(q.getC(), 0));
ASSERT(equal(q.getD(), 0));
ASSERT(vectorEqual(q, Quaternion(1, 0, 0, 0)));
// TODO test derivative
// test accessors
q.setA(0.1);
q.setB(0.2);
q.setC(0.3);
q.setD(0.4);
ASSERT(equal(q.getA(), 0.1));
ASSERT(equal(q.getB(), 0.2));
ASSERT(equal(q.getC(), 0.3));
ASSERT(equal(q.getD(), 0.4));
ASSERT(vectorEqual(q, Quaternion(0.1, 0.2, 0.3, 0.4)));
printf("PASS\n");
return 0;
}