Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: change quaternion class to use const references where optimal

Browse Source
This commit is contained in:
Jonathan Challinger 2014-10-19 12:13:53 -07:00 committed by Andrew Tridgell
parent 70845882a7
commit 3befe74afa
2 changed files with 22 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_Math/quaternion.cpp
View File

@ -125,7 +125,7 @@ void Quaternion::from_axis_angle(Vector3f v) {
from_axis_angle(v,theta); from_axis_angle(v,theta);
} }
void Quaternion::from_axis_angle(Vector3f axis, float theta) { void Quaternion::from_axis_angle(const Vector3f &axis, float theta) {
if(theta == 0.0f) { if(theta == 0.0f) {
q1 = 1.0f; q1 = 1.0f;
q2=q3=q4=0.0f; q2=q3=q4=0.0f;
@ -138,7 +138,7 @@ void Quaternion::from_axis_angle(Vector3f axis, float theta) {
q4 = axis.z * st2; q4 = axis.z * st2;
} }
void Quaternion::rotate(Vector3f v) { void Quaternion::rotate(const Vector3f &v) {
Quaternion r; Quaternion r;
r.from_axis_angle(v); r.from_axis_angle(v);
(*this) *= r; (*this) *= r;
@ -163,7 +163,7 @@ void Quaternion::from_axis_angle_fast(Vector3f v) {
from_axis_angle_fast(v,theta); from_axis_angle_fast(v,theta);
} }
void Quaternion::from_axis_angle_fast(Vector3f axis, float theta) { void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta) {
float t2 = theta/2.0f; float t2 = theta/2.0f;
float sqt2 = sq(t2); float sqt2 = sq(t2);
float st2 = t2-sqt2*t2/6.0f; float st2 = t2-sqt2*t2/6.0f;
@ -232,17 +232,17 @@ void Quaternion::normalize(void)
} }
} }
Quaternion Quaternion::operator*(Quaternion v) { Quaternion Quaternion::operator*(const Quaternion &v) {
Quaternion ret; Quaternion ret;
float &w1 = q1; float &w1 = q1;
float &x1 = q2; float &x1 = q2;
float &y1 = q3; float &y1 = q3;
float &z1 = q4; float &z1 = q4;
float &w2 = v.q1; float w2 = v.q1;
float &x2 = v.q2; float x2 = v.q2;
float &y2 = v.q3; float y2 = v.q3;
float &z2 = v.q4; float z2 = v.q4;
ret.q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2; ret.q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
ret.q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2; ret.q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
@ -252,16 +252,16 @@ Quaternion Quaternion::operator*(Quaternion v) {
return ret; return ret;
} }
Quaternion &Quaternion::operator*=(Quaternion v) { Quaternion &Quaternion::operator*=(const Quaternion &v) {
float w1 = q1; float &w1 = q1;
float x1 = q2; float &x1 = q2;
float y1 = q3; float &y1 = q3;
float z1 = q4; float &z1 = q4;
float &w2 = v.q1; float w2 = v.q1;
float &x2 = v.q2; float x2 = v.q2;
float &y2 = v.q3; float y2 = v.q3;
float &z2 = v.q4; float z2 = v.q4;
q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2; q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2; q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;

10
libraries/AP_Math/quaternion.h
View File

@ -68,13 +68,13 @@ public:
void from_axis_angle(Vector3f v); void from_axis_angle(Vector3f v);
void from_axis_angle(Vector3f axis, float theta); void from_axis_angle(const Vector3f &axis, float theta);
void rotate(Vector3f v); void rotate(const Vector3f &v);
void from_axis_angle_fast(Vector3f v); void from_axis_angle_fast(Vector3f v);
void from_axis_angle_fast(Vector3f axis, float theta); void from_axis_angle_fast(const Vector3f &axis, float theta);
void rotate_fast(const Vector3f &v); void rotate_fast(const Vector3f &v);
@ -104,7 +104,7 @@ public:
return _v[i]; return _v[i];
} }
Quaternion operator*(Quaternion v); Quaternion operator*(const Quaternion &v);
Quaternion &operator*=(Quaternion v); Quaternion &operator*=(const Quaternion &v);
}; };
#endif // QUATERNION_H #endif // QUATERNION_H