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

Quaternion mark const helpers const

Browse Source
This commit is contained in:
Daniel Agar 2018-03-31 16:59:03 -04:00 committed by James Goppert
parent e7c95fa027
commit 21d47424c6
1 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
matrix/Quaternion.hpp
View File

@ -346,9 +346,9 @@ public:
*
* @return inverted quaternion
*/
Quaternion inversed()
Quaternion inversed() const
{
Quaternion &q = *this;
const Quaternion &q = *this;
Type normSq = q.dot(q);
return Quaternion(
q(0)/normSq,
@ -393,7 +393,8 @@ public:
* @param vec vector to rotate in frame 2 (typically reference 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 v(0, vec(0), vec(1), vec(2));
Quaternion res = q.inversed()*v*q;
@ -459,9 +460,9 @@ public:
*
* @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)));
Vector<Type, 3> vec;
vec(0) = q(1);
@ -479,9 +480,9 @@ public:
/**
* 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));
}
@ -492,9 +493,9 @@ public:
* == last column of the equivalent rotation matrix
* 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;
const Type a = q(0);
const Type b = q(1);