Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -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);
}
void Quaternion::from_axis_angle(Vector3f axis, float theta) {
void Quaternion::from_axis_angle(const Vector3f &axis, float theta) {
if(theta == 0.0f) {
q1 = 1.0f;
q2=q3=q4=0.0f;
@ -138,7 +138,7 @@ void Quaternion::from_axis_angle(Vector3f axis, float theta) {
q4 = axis.z * st2;
}
void Quaternion::rotate(Vector3f v) {
void Quaternion::rotate(const Vector3f &v) {
Quaternion r;
r.from_axis_angle(v);
(*this) *= r;
@ -163,7 +163,7 @@ void Quaternion::from_axis_angle_fast(Vector3f v) {
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 sqt2 = sq(t2);
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;
float &w1 = q1;
float &x1 = q2;
float &y1 = q3;
float &z1 = q4;
float &w2 = v.q1;
float &x2 = v.q2;
float &y2 = v.q3;
float &z2 = v.q4;
float w2 = v.q1;
float x2 = v.q2;
float y2 = v.q3;
float z2 = v.q4;
ret.q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
ret.q2 = w1*x2 + x1*w2 + y1*z2 - z1*y2;
@ -252,16 +252,16 @@ Quaternion Quaternion::operator*(Quaternion v) {
return ret;
}
Quaternion &Quaternion::operator*=(Quaternion v) {
float w1 = q1;
float x1 = q2;
float y1 = q3;
float z1 = q4;
Quaternion &Quaternion::operator*=(const Quaternion &v) {
float &w1 = q1;
float &x1 = q2;
float &y1 = q3;
float &z1 = q4;
float &w2 = v.q1;
float &x2 = v.q2;
float &y2 = v.q3;
float &z2 = v.q4;
float w2 = v.q1;
float x2 = v.q2;
float y2 = v.q3;
float z2 = v.q4;
q1 = w1*w2 - x1*x2 - y1*y2 - z1*z2;
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 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 axis, float theta);
void from_axis_angle_fast(const Vector3f &axis, float theta);
void rotate_fast(const Vector3f &v);
@ -104,7 +104,7 @@ public:
return _v[i];
}
Quaternion operator*(Quaternion v);
Quaternion &operator*=(Quaternion v);
Quaternion operator*(const Quaternion &v);
Quaternion &operator*=(const Quaternion &v);
};
#endif // QUATERNION_H