fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 05:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix some template type conversions and style

Browse Source
This commit is contained in:
Matthias Grob
2019-09-23 08:54:41 +02:00
committed by Beat Küng
parent c34e8dc98f
commit 973999a4d3
4 changed files with 17 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/AxisAngle.hpp
+1 -1
View File
@@ -69,7 +69,7 @@ public:
AxisAngle(const Quaternion<Type> &q) AxisAngle(const Quaternion<Type> &q)
{ {
AxisAngle &v = *this; AxisAngle &v = *this;
Type ang = Type(2.0f)*acos(q(0)); Type ang = Type(2) * acos(q(0));
Type mag = sin(ang/2.0f); Type mag = sin(ang/2.0f);
if (fabs(mag) > 0) { if (fabs(mag) > 0) {
v(0) = ang*q(1)/mag; v(0) = ang*q(1)/mag;
matrix/Euler.hpp
+2 -2
View File
@@ -97,11 +97,11 @@ public:
Type pi = Type(M_PI); Type pi = Type(M_PI);
if (Type(fabs(theta_val - pi / Type(2))) < Type(1.0e-3)) { if (Type(fabs(theta_val - pi / Type(2))) < Type(1.0e-3)) {
phi_val = Type(0.0); phi_val = Type(0);
psi_val = Type(atan2(dcm(1, 2), dcm(0, 2))); psi_val = Type(atan2(dcm(1, 2), dcm(0, 2)));
} else if (Type(fabs(theta_val + pi / Type(2))) < Type(1.0e-3)) { } else if (Type(fabs(theta_val + pi / Type(2))) < Type(1.0e-3)) {
phi_val = Type(0.0); phi_val = Type(0);
psi_val = Type(atan2(-dcm(1, 2), -dcm(0, 2))); psi_val = Type(atan2(-dcm(1, 2), -dcm(0, 2)));
} }
matrix/Matrix.hpp
+1 -1
View File
@@ -278,7 +278,7 @@ public:
void operator/=(Type scalar) void operator/=(Type scalar)
{ {
Matrix<Type, M, N> &self = *this; Matrix<Type, M, N> &self = *this;
self = self * (Type(1.0f) / scalar); self = self * (Type(1) / scalar);
} }
inline void operator+=(Type scalar) inline void operator+=(Type scalar)
matrix/Quaternion.hpp
+13 -13
View File
@@ -143,12 +143,12 @@ public:
Quaternion(const Euler<Type> &euler) Quaternion(const Euler<Type> &euler)
{ {
Quaternion &q = *this; Quaternion &q = *this;
Type cosPhi_2 = Type(cos(euler.phi() / Type(2.0))); Type cosPhi_2 = Type(cos(euler.phi() / Type(2)));
Type cosTheta_2 = Type(cos(euler.theta() / Type(2.0))); Type cosTheta_2 = Type(cos(euler.theta() / Type(2)));
Type cosPsi_2 = Type(cos(euler.psi() / Type(2.0))); Type cosPsi_2 = Type(cos(euler.psi() / Type(2)));
Type sinPhi_2 = Type(sin(euler.phi() / Type(2.0))); Type sinPhi_2 = Type(sin(euler.phi() / Type(2)));
Type sinTheta_2 = Type(sin(euler.theta() / Type(2.0))); Type sinTheta_2 = Type(sin(euler.theta() / Type(2)));
Type sinPsi_2 = Type(sin(euler.psi() / Type(2.0))); Type sinPsi_2 = Type(sin(euler.psi() / Type(2)));
q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 + q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2; sinPhi_2 * sinTheta_2 * sinPsi_2;
q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 - q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
@@ -170,10 +170,10 @@ public:
Type angle = aa.norm(); Type angle = aa.norm();
Vector<Type, 3> axis = aa.unit(); Vector<Type, 3> axis = aa.unit();
if (angle < Type(1e-10)) { if (angle < Type(1e-10)) {
q(0) = Type(1.0); q(0) = Type(1);
q(1) = q(2) = q(3) = 0; q(1) = q(2) = q(3) = 0;
} else { } else {
Type magnitude = sin(angle / 2.0f); Type magnitude = sin(angle / Type(2));
q(0) = cos(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;
@@ -368,7 +368,7 @@ public:
Quaternion canonical() const Quaternion canonical() const
{ {
const Quaternion &q = *this; const Quaternion &q = *this;
if(q(0)<Type(0)) { if (q(0) < Type(0)) {
return Quaternion(-q(0),-q(1),-q(2),-q(3)); return Quaternion(-q(0),-q(1),-q(2),-q(3));
} else { } else {
return Quaternion(q(0),q(1),q(2),q(3)); return Quaternion(q(0),q(1),q(2),q(3));
@@ -434,7 +434,7 @@ public:
Type theta = vec.norm(); Type theta = vec.norm();
if (theta < Type(1e-10)) { if (theta < Type(1e-10)) {
q(0) = Type(1.0); q(0) = Type(1);
q(1) = q(2) = q(3) = 0; q(1) = q(2) = q(3) = 0;
return; return;
} }
@@ -456,8 +456,8 @@ public:
Quaternion &q = *this; Quaternion &q = *this;
if (theta < Type(1e-10)) { if (theta < Type(1e-10)) {
q(0) = Type(1.0); q(0) = Type(1);
q(1) = q(2) = q(3) = 0; q(1) = q(2) = q(3) = Type(0);
} }
Type magnitude = sin(theta / 2.0f); Type magnitude = sin(theta / 2.0f);
@@ -489,7 +489,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) * atan2(axis_magnitude, q(0))); vec = vec * wrap_pi(Type(2) * atan2(axis_magnitude, q(0)));
} }
return vec; return vec;