mirror of https://github.com/ArduPilot/ardupilot
AP_Math: Add quaternion division
This commit is contained in:
parent
79017096e2
commit
17445d03f0
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -106,5 +106,6 @@ public:
|
|||
|
||||
Quaternion operator*(const Quaternion &v);
|
||||
Quaternion &operator*=(const Quaternion &v);
|
||||
Quaternion operator/(const Quaternion &v);
|
||||
};
|
||||
#endif // QUATERNION_H
|
||||
|
|
Loading…
Reference in New Issue