fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 23:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Removed all use of C library from the matrix namespace

Browse Source
This commit is contained in:
Pavel Kirienko
2017-03-17 12:18:39 -04:00
committed by James Goppert
parent 552dad40a1
commit 9ebf5f89db
3 changed files with 17 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/AxisAngle.hpp
+3 -3
View File
@@ -76,9 +76,9 @@ public:
Vector<Type, 3>() Vector<Type, 3>()
{ {
AxisAngle &v = *this; AxisAngle &v = *this;
Type ang = Type(2.0f)*acosf(q(0)); Type ang = Type(2.0f)*acos(q(0));
Type mag = sinf(ang/2.0f); Type mag = sin(ang/2.0f);
if (fabsf(mag) > 0) { if (fabs(mag) > 0) {
v(0) = ang*q(1)/mag; v(0) = ang*q(1)/mag;
v(1) = ang*q(2)/mag; v(1) = ang*q(2)/mag;
v(2) = ang*q(3)/mag; v(2) = ang*q(3)/mag;
matrix/Quaternion.hpp
+9 -9
View File
@@ -97,28 +97,28 @@ public:
Quaternion &q = *this; Quaternion &q = *this;
Type t = R.trace(); Type t = R.trace();
if (t > Type(0)) { if (t > Type(0)) {
t = sqrtf(Type(1) + t); t = sqrt(Type(1) + t);
q(0) = Type(0.5) * t; q(0) = Type(0.5) * t;
t = Type(0.5) / t; t = Type(0.5) / t;
q(1) = (R(2,1) - R(1,2)) * t; q(1) = (R(2,1) - R(1,2)) * t;
q(2) = (R(0,2) - R(2,0)) * t; q(2) = (R(0,2) - R(2,0)) * t;
q(3) = (R(1,0) - R(0,1)) * t; q(3) = (R(1,0) - R(0,1)) * t;
} else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) { } else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) {
t = sqrtf(Type(1) + R(0,0) - R(1,1) - R(2,2)); t = sqrt(Type(1) + R(0,0) - R(1,1) - R(2,2));
q(1) = Type(0.5) * t; q(1) = Type(0.5) * t;
t = Type(0.5) / t; t = Type(0.5) / t;
q(0) = (R(2,1) - R(1,2)) * t; q(0) = (R(2,1) - R(1,2)) * t;
q(2) = (R(1,0) + R(0,1)) * t; q(2) = (R(1,0) + R(0,1)) * t;
q(3) = (R(0,2) + R(2,0)) * t; q(3) = (R(0,2) + R(2,0)) * t;
} else if (R(1,1) > R(2,2)) { } else if (R(1,1) > R(2,2)) {
t = sqrtf(Type(1) - R(0,0) + R(1,1) - R(2,2)); t = sqrt(Type(1) - R(0,0) + R(1,1) - R(2,2));
q(2) = Type(0.5) * t; q(2) = Type(0.5) * t;
t = Type(0.5) / t; t = Type(0.5) / t;
q(0) = (R(0,2) - R(2,0)) * t; q(0) = (R(0,2) - R(2,0)) * t;
q(1) = (R(1,0) + R(0,1)) * t; q(1) = (R(1,0) + R(0,1)) * t;
q(3) = (R(2,1) + R(1,2)) * t; q(3) = (R(2,1) + R(1,2)) * t;
} else { } else {
t = sqrtf(Type(1) - R(0,0) - R(1,1) + R(2,2)); t = sqrt(Type(1) - R(0,0) - R(1,1) + R(2,2));
q(3) = Type(0.5) * t; q(3) = Type(0.5) * t;
t = Type(0.5) / t; t = Type(0.5) / t;
q(0) = (R(1,0) - R(0,1)) * t; q(0) = (R(1,0) - R(0,1)) * t;
@@ -171,8 +171,8 @@ public:
q(0) = Type(1.0); q(0) = Type(1.0);
q(1) = q(2) = q(3) = 0; q(1) = q(2) = q(3) = 0;
} else { } else {
Type magnitude = sinf(angle / 2.0f); Type magnitude = sin(angle / 2.0f);
q(0) = cosf(angle / 2.0f); q(0) = cos(angle / 2.0f);
q(1) = axis(0) * magnitude; q(1) = axis(0) * magnitude;
q(2) = axis(1) * magnitude; q(2) = axis(1) * magnitude;
q(3) = axis(2) * magnitude; q(3) = axis(2) * magnitude;
@@ -389,9 +389,9 @@ public:
q(1) = q(2) = q(3) = 0; q(1) = q(2) = q(3) = 0;
} }
Type magnitude = sinf(theta / 2.0f); Type magnitude = sin(theta / 2.0f);
q(0) = cosf(theta / 2.0f); q(0) = cos(theta / 2.0f);
q(1) = axis(0) * magnitude; q(1) = axis(0) * magnitude;
q(2) = axis(1) * magnitude; q(2) = axis(1) * magnitude;
q(3) = axis(2) * magnitude; q(3) = axis(2) * magnitude;
@@ -418,7 +418,7 @@ public:
if (axis_magnitude >= Type(1e-10)) { if (axis_magnitude >= Type(1e-10)) {
vec = vec / axis_magnitude; vec = vec / axis_magnitude;
vec = vec * wrap_pi(Type(2.0) * atan2f(axis_magnitude, q(0))); vec = vec * wrap_pi(Type(2.0) * atan2(axis_magnitude, q(0)));
} }
return vec; return vec;
matrix/SquareMatrix.hpp
+5 -5
View File
@@ -136,12 +136,12 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
for (size_t n = 0; n < M; n++) { for (size_t n = 0; n < M; n++) {
// if diagonal is zero, swap with row below // if diagonal is zero, swap with row below
if (fabsf(static_cast<float>(U(n, n))) < 1e-8f) { if (fabs(static_cast<float>(U(n, n))) < 1e-8f) {
//printf("trying pivot for row %d\n",n); //printf("trying pivot for row %d\n",n);
for (size_t i = n + 1; i < M; i++) { for (size_t i = n + 1; i < M; i++) {
//printf("\ttrying row %d\n",i); //printf("\ttrying row %d\n",i);
if (fabsf(static_cast<float>(U(i, n))) > 1e-8f) { if (fabs(static_cast<float>(U(i, n))) > 1e-8f) {
//printf("swapped %d\n",i); //printf("swapped %d\n",i);
U.swapRows(i, n); U.swapRows(i, n);
P.swapRows(i, n); P.swapRows(i, n);
@@ -157,11 +157,11 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
//printf("U:\n"); U.print(); //printf("U:\n"); U.print();
//printf("P:\n"); P.print(); //printf("P:\n"); P.print();
//fflush(stdout); //fflush(stdout);
//ASSERT(fabsf(U(n, n)) > 1e-8f); //ASSERT(fabs(U(n, n)) > 1e-8f);
#endif #endif
// failsafe, return zero matrix // failsafe, return zero matrix
if (fabsf(static_cast<float>(U(n, n))) < 1e-8f) { if (fabs(static_cast<float>(U(n, n))) < 1e-8f) {
return false; return false;
} }
@@ -280,7 +280,7 @@ SquareMatrix <Type, M> cholesky(const SquareMatrix<Type, M> & A)
if (res <= 0) { if (res <= 0) {
L(j, j) = 0; L(j, j) = 0;
} else { } else {
L(j, j) = sqrtf(res); L(j, j) = sqrt(res);
} }
} else { } else {
float sum = 0; float sum = 0;