fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 21:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

add renormalize method (#32)

Browse Source
This commit is contained in:
James Goppert
2016-11-02 12:10:30 -04:00
committed by GitHub
parent db4374882b
commit 34fccdd680
2 changed files with 67 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Dcm.hpp
+24 -11
View File
@@ -81,16 +81,17 @@ public:
*
* @param q quaternion to set dcm to
*/
Dcm(const Quaternion<Type> & q) {
Dcm(const Quaternion<Type> &q)
{
Dcm &dcm = *this;
Type a = q(0);
Type b = q(1);
Type c = q(2);
Type d = q(3);
Type aSq = a*a;
Type bSq = b*b;
Type cSq = c*c;
Type dSq = d*d;
Type aSq = a * a;
Type bSq = b * b;
Type cSq = c * c;
Type dSq = d * d;
dcm(0, 0) = aSq + bSq - cSq - dSq;
dcm(0, 1) = 2 * (b * c - a * d);
dcm(0, 2) = 2 * (a * c + b * d);
@@ -111,7 +112,8 @@ public:
*
* @param euler euler angle instance
*/
Dcm(const Euler<Type> & euler) {
Dcm(const Euler<Type> &euler)
{
Dcm &dcm = *this;
Type cosPhi = Type(cos(euler.phi()));
Type sinPhi = Type(sin(euler.phi()));
@@ -143,19 +145,30 @@ public:
*
* @param euler euler angle instance
*/
Dcm(const AxisAngle<Type> & aa) {
Dcm(const AxisAngle<Type> &aa)
{
Dcm &dcm = *this;
dcm = Quaternion<Type>(aa);
}
Vector<Type, 3> vee() const { // inverse to Vector.hat() operation
Vector<Type, 3> vee() const // inverse to Vector.hat() operation
{
const Dcm &A(*this);
Vector<Type, 3> v;
v(0) = -A(1,2);
v(1) = A(0,2);
v(2) = -A(0,1);
v(0) = -A(1, 2);
v(1) = A(0, 2);
v(2) = -A(0, 1);
return v;
}
void renormalize()
{
/* renormalize rows */
for (int row = 0; row < 3; row++) {
matrix::Vector3f rvec(this->_data[row]);
this->setRow(row, rvec.normalized());
}
}
};
typedef Dcm<float> Dcmf;