fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-05 14:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Quaternion mark const helpers const

Browse Source
This commit is contained in:
Daniel Agar
2018-04-01 17:02:20 -04:00
committed by James Goppert
parent e7c95fa027
commit 21d47424c6
1 changed files with 10 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Quaternion.hpp
+10 -9
View File
@@ -346,9 +346,9 @@ public:
* *
* @return inverted quaternion * @return inverted quaternion
*/ */
Quaternion inversed() Quaternion inversed() const
{ {
Quaternion &q = *this; const Quaternion &q = *this;
Type normSq = q.dot(q); Type normSq = q.dot(q);
return Quaternion( return Quaternion(
q(0)/normSq, q(0)/normSq,
@@ -393,7 +393,8 @@ public:
* @param vec vector to rotate in frame 2 (typically reference frame) * @param vec vector to rotate in frame 2 (typically reference frame)
* @return rotated vector in frame 1 (typically body frame) * @return rotated vector in frame 1 (typically body frame)
*/ */
Vector3f conjugate_inversed(const Vector3f &vec) { Vector3f conjugate_inversed(const Vector3f &vec) const
{
Quaternion q = *this; Quaternion q = *this;
Quaternion v(0, vec(0), vec(1), vec(2)); Quaternion v(0, vec(0), vec(1), vec(2));
Quaternion res = q.inversed()*v*q; Quaternion res = q.inversed()*v*q;
@@ -459,9 +460,9 @@ public:
* *
* @return vector, direction representing rotation axis and norm representing angle * @return vector, direction representing rotation axis and norm representing angle
*/ */
Vector<Type, 3> to_axis_angle() Vector<Type, 3> to_axis_angle() const
{ {
Quaternion &q = *this; const Quaternion &q = *this;
Type axis_magnitude = Type(sqrt(q(1) * q(1) + q(2) * q(2) + q(3) * q(3))); Type axis_magnitude = Type(sqrt(q(1) * q(1) + q(2) * q(2) + q(3) * q(3)));
Vector<Type, 3> vec; Vector<Type, 3> vec;
vec(0) = q(1); vec(0) = q(1);
@@ -479,9 +480,9 @@ public:
/** /**
* Imaginary components of quaternion * Imaginary components of quaternion
*/ */
Vector3<Type> imag() Vector3<Type> imag() const
{ {
Quaternion &q = *this; const Quaternion &q = *this;
return Vector3<Type>(q(1), q(2), q(3)); return Vector3<Type>(q(1), q(2), q(3));
} }
@@ -492,9 +493,9 @@ public:
* == last column of the equivalent rotation matrix * == last column of the equivalent rotation matrix
* but calculated more efficiently than a full conversion * but calculated more efficiently than a full conversion
*/ */
Vector3<Type> dcm_z() Vector3<Type> dcm_z() const
{ {
Quaternion &q = *this; const Quaternion &q = *this;
Vector3<Type> R_z; Vector3<Type> R_z;
const Type a = q(0); const Type a = q(0);
const Type b = q(1); const Type b = q(1);