Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: make quaternion divide by zero protection more conservative

Browse Source
This commit is contained in:
Jonathan Challinger 2015-04-23 14:19:47 -07:00 committed by Andrew Tridgell
parent fb8da1b2d8
commit 3812773485
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Math/quaternion.cpp
View File

@ -141,7 +141,7 @@ void Quaternion::from_vector312(float roll ,float pitch, float yaw)
void Quaternion::from_axis_angle(Vector3f v) { void Quaternion::from_axis_angle(Vector3f v) {
float theta = v.length(); float theta = v.length();
if(theta == 0.0f) { if(theta < 1.0e-12f) {
q1 = 1.0f; q1 = 1.0f;
q2=q3=q4=0.0f; q2=q3=q4=0.0f;
return; return;
@ -151,7 +151,7 @@ void Quaternion::from_axis_angle(Vector3f v) {
} }
void Quaternion::from_axis_angle(const Vector3f &axis, float theta) { void Quaternion::from_axis_angle(const Vector3f &axis, float theta) {
if(theta == 0.0f) { if(theta < 1.0e-12f) {
q1 = 1.0f; q1 = 1.0f;
q2=q3=q4=0.0f; q2=q3=q4=0.0f;
} }
@ -172,7 +172,7 @@ void Quaternion::rotate(const Vector3f &v) {
void Quaternion::to_axis_angle(Vector3f &v) { void Quaternion::to_axis_angle(Vector3f &v) {
float l = sqrt(sq(q2)+sq(q3)+sq(q4)); float l = sqrt(sq(q2)+sq(q3)+sq(q4));
v = Vector3f(q2,q3,q4); v = Vector3f(q2,q3,q4);
if(l != 0) { if(l >= 1.0e-12f) {
v /= l; v /= l;
v *= wrap_PI(2.0f * atan2(l,q1)); 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) { void Quaternion::from_axis_angle_fast(Vector3f v) {
float theta = v.length(); float theta = v.length();
if(theta == 0.0f) { if(theta < 1.0e-12f) {
q1 = 1.0f; q1 = 1.0f;
q2=q3=q4=0.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) { void Quaternion::rotate_fast(const Vector3f &v) {
float theta = v.length(); float theta = v.length();
if(theta == 0.0f) return; if(theta < 1.0e-12f) return;
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;