Browse Source

Handle some divide by zero edge cases on init.

master
James Goppert 8 years ago
parent
commit
74a120f554
  1. 14
      matrix/AxisAngle.hpp
  2. 6
      matrix/Quaternion.hpp
  3. 4
      test/attitude.cpp

14
matrix/AxisAngle.hpp

@ -78,14 +78,14 @@ public: @@ -78,14 +78,14 @@ public:
AxisAngle &v = *this;
Type ang = (Type)2.0f*acosf(q(0));
Type mag = sinf(ang/2.0f);
if (fabsf(ang) < 1e-10f) {
v(0) = 1e-10f;
v(1) = 0;
v(2) = 0;
} else {
if (fabsf(mag) > 0) {
v(0) = ang*q(1)/mag;
v(1) = ang*q(2)/mag;
v(2) = ang*q(3)/mag;
} else {
v(0) = 0;
v(1) = 0;
v(2) = 0;
}
}
@ -156,7 +156,11 @@ public: @@ -156,7 +156,11 @@ public:
Vector<Type, 3> axis() {
if (Vector<Type, 3>::norm() > 0) {
return Vector<Type, 3>::unit();
} else {
return Vector3<Type>(1, 0, 0);
}
}
Type angle() {

6
matrix/Quaternion.hpp

@ -94,6 +94,11 @@ public: @@ -94,6 +94,11 @@ public:
Vector<Type, 4>()
{
Quaternion &q = *this;
q(0) = 1;
q(1) = 0;
q(2) = 0;
q(3) = 0;
if (((Type(1) + dcm(0, 0) + dcm(1, 1) + dcm(2, 2)) > 0) & (fabsf(q(0)) > 0) ) {
q(0) = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
@ -103,6 +108,7 @@ public: @@ -103,6 +108,7 @@ public:
q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
(Type(4) * q(0)));
}
}
/**
* Constructor from euler angles

4
test/attitude.cpp

@ -283,6 +283,10 @@ int main() @@ -283,6 +283,10 @@ int main()
AxisAnglef aa_data_init(aa_data);
TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f)));
AxisAnglef aa_norm_check(Vector3f(0.0f, 0.0f, 0.0f));
TEST(isEqual(aa_norm_check.axis(), Vector3f(1, 0, 0)));
TEST(isEqualF(aa_norm_check.angle(), 0.0f));
q = Quatf(-0.29555112749297824f, 0.25532186f, 0.51064372f, 0.76596558f);
TEST(isEqual(q.imag(), Vector3f(0.25532186f, 0.51064372f, 0.76596558f)));

Loading…
Cancel
Save