mirror of https://github.com/ArduPilot/ardupilot
AP_Math: make quaternion divide by zero protection more conservative
This commit is contained in:
parent
fb8da1b2d8
commit
3812773485
|
@ -141,7 +141,7 @@ void Quaternion::from_vector312(float roll ,float pitch, float yaw)
|
|||
|
||||
void Quaternion::from_axis_angle(Vector3f v) {
|
||||
float theta = v.length();
|
||||
if(theta == 0.0f) {
|
||||
if(theta < 1.0e-12f) {
|
||||
q1 = 1.0f;
|
||||
q2=q3=q4=0.0f;
|
||||
return;
|
||||
|
@ -151,7 +151,7 @@ void Quaternion::from_axis_angle(Vector3f v) {
|
|||
}
|
||||
|
||||
void Quaternion::from_axis_angle(const Vector3f &axis, float theta) {
|
||||
if(theta == 0.0f) {
|
||||
if(theta < 1.0e-12f) {
|
||||
q1 = 1.0f;
|
||||
q2=q3=q4=0.0f;
|
||||
}
|
||||
|
@ -172,7 +172,7 @@ void Quaternion::rotate(const Vector3f &v) {
|
|||
void Quaternion::to_axis_angle(Vector3f &v) {
|
||||
float l = sqrt(sq(q2)+sq(q3)+sq(q4));
|
||||
v = Vector3f(q2,q3,q4);
|
||||
if(l != 0) {
|
||||
if(l >= 1.0e-12f) {
|
||||
v /= l;
|
||||
v *= wrap_PI(2.0f * atan2(l,q1));
|
||||
}
|
||||
|
@ -180,7 +180,7 @@ void Quaternion::to_axis_angle(Vector3f &v) {
|
|||
|
||||
void Quaternion::from_axis_angle_fast(Vector3f v) {
|
||||
float theta = v.length();
|
||||
if(theta == 0.0f) {
|
||||
if(theta < 1.0e-12f) {
|
||||
q1 = 1.0f;
|
||||
q2=q3=q4=0.0f;
|
||||
}
|
||||
|
@ -201,7 +201,7 @@ void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta) {
|
|||
|
||||
void Quaternion::rotate_fast(const Vector3f &v) {
|
||||
float theta = v.length();
|
||||
if(theta == 0.0f) return;
|
||||
if(theta < 1.0e-12f) return;
|
||||
float t2 = theta/2.0f;
|
||||
float sqt2 = sq(t2);
|
||||
float st2 = t2-sqt2*t2/6.0f;
|
||||
|
|
Loading…
Reference in New Issue