AP_Math: Add quaternion division

This commit is contained in:
Paul Riseborough 2015-01-31 09:14:46 +11:00 committed by Andrew Tridgell
parent 79017096e2
commit 17445d03f0
2 changed files with 20 additions and 0 deletions

View File

@ -270,3 +270,22 @@ Quaternion &Quaternion::operator*=(const Quaternion &v) {
return *this;
}
Quaternion Quaternion::operator/(const Quaternion &v) {
Quaternion ret;
float &quat0 = q1;
float &quat1 = q2;
float &quat2 = q3;
float &quat3 = q4;
float rquat0 = v.q1;
float rquat1 = v.q2;
float rquat2 = v.q3;
float rquat3 = v.q4;
ret.q1 = (rquat0*quat0 + rquat1*quat1 + rquat2*quat2 + rquat3*quat3);
ret.q1 = (rquat0*quat1 - rquat1*quat0 - rquat2*quat3 + rquat3*quat2);
ret.q1 = (rquat0*quat2 + rquat1*quat3 - rquat2*quat0 - rquat3*quat1);
ret.q1 = (rquat0*quat3 - rquat1*quat2 + rquat2*quat1 - rquat3*quat0);
return ret;
}

View File

@ -106,5 +106,6 @@ public:
Quaternion operator*(const Quaternion &v);
Quaternion &operator*=(const Quaternion &v);
Quaternion operator/(const Quaternion &v);
};
#endif // QUATERNION_H