fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 01:57:34 +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 &v = *this;
Type ang = Type(2.0f)*acos(q(0));
Type ang = Type(2) * acos(q(0));
Type mag = sin(ang/2.0f);
if (fabs(mag) > 0) {
v(0) = ang*q(1)/mag;
matrix/Euler.hpp
+2 -2
View File
@@ -97,11 +97,11 @@ public:
Type pi = Type(M_PI);
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)));
} 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)));
}
matrix/Matrix.hpp
+1 -1
View File
@@ -278,7 +278,7 @@ public:
void operator/=(Type scalar)
{
Matrix<Type, M, N> &self = *this;
self = self * (Type(1.0f) / scalar);
self = self * (Type(1) / scalar);
}
inline void operator+=(Type scalar)
matrix/Quaternion.hpp
+13 -13
View File
@@ -143,12 +143,12 @@ public:
Quaternion(const Euler<Type> &euler)
{
Quaternion &q = *this;
Type cosPhi_2 = Type(cos(euler.phi() / Type(2.0)));
Type cosTheta_2 = Type(cos(euler.theta() / Type(2.0)));
Type cosPsi_2 = Type(cos(euler.psi() / Type(2.0)));
Type sinPhi_2 = Type(sin(euler.phi() / Type(2.0)));
Type sinTheta_2 = Type(sin(euler.theta() / Type(2.0)));
Type sinPsi_2 = Type(sin(euler.psi() / Type(2.0)));
Type cosPhi_2 = Type(cos(euler.phi() / Type(2)));
Type cosTheta_2 = Type(cos(euler.theta() / Type(2)));
Type cosPsi_2 = Type(cos(euler.psi() / Type(2)));
Type sinPhi_2 = Type(sin(euler.phi() / Type(2)));
Type sinTheta_2 = Type(sin(euler.theta() / Type(2)));
Type sinPsi_2 = Type(sin(euler.psi() / Type(2)));
q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2;
q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
@@ -170,10 +170,10 @@ public:
Type angle = aa.norm();
Vector<Type, 3> axis = aa.unit();
if (angle < Type(1e-10)) {
q(0) = Type(1.0);
q(0) = Type(1);
q(1) = q(2) = q(3) = 0;
} else {
Type magnitude = sin(angle / 2.0f);
Type magnitude = sin(angle / Type(2));
q(0) = cos(angle / 2.0f);
q(1) = axis(0) * magnitude;
q(2) = axis(1) * magnitude;
@@ -368,7 +368,7 @@ public:
Quaternion canonical() const
{
const Quaternion &q = *this;
if(q(0)<Type(0)) {
if (q(0) < Type(0)) {
return Quaternion(-q(0),-q(1),-q(2),-q(3));
} else {
return Quaternion(q(0),q(1),q(2),q(3));
@@ -434,7 +434,7 @@ public:
Type theta = vec.norm();
if (theta < Type(1e-10)) {
q(0) = Type(1.0);
q(0) = Type(1);
q(1) = q(2) = q(3) = 0;
return;
}
@@ -456,8 +456,8 @@ public:
Quaternion &q = *this;
if (theta < Type(1e-10)) {
q(0) = Type(1.0);
q(1) = q(2) = q(3) = 0;
q(0) = Type(1);
q(1) = q(2) = q(3) = Type(0);
}
Type magnitude = sin(theta / 2.0f);
@@ -489,7 +489,7 @@ public:
if (axis_magnitude >= Type(1e-10)) {
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;