Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: quaternion: fix style

Browse Source 
Remove trailing spaces and run astyle.
This commit is contained in:
Gustavo Jose de Sousa 2016-05-04 10:33:57 -03:00 committed by Andrew Tridgell
parent 8c4c9ccfa6
commit 39f72610e2
2 changed files with 60 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Math/quaternion.cpp
View File

@ -126,9 +126,10 @@ void Quaternion::from_vector312(float roll ,float pitch, float yaw)
from_rotation_matrix(m);
}
void Quaternion::from_axis_angle(Vector3f v) {
void Quaternion::from_axis_angle(Vector3f v)
{
float theta = v.length();
if(theta < 1.0e-12f) {
if (theta < 1.0e-12f) {
q1 = 1.0f;
q2=q3=q4=0.0f;
return;
@ -137,8 +138,9 @@ void Quaternion::from_axis_angle(Vector3f v) {
from_axis_angle(v,theta);
}
void Quaternion::from_axis_angle(const Vector3f &axis, float theta) {
if(theta < 1.0e-12f) {
void Quaternion::from_axis_angle(const Vector3f &axis, float theta)
{
if (theta < 1.0e-12f) {
q1 = 1.0f;
q2=q3=q4=0.0f;
}
@ -150,24 +152,27 @@ void Quaternion::from_axis_angle(const Vector3f &axis, float theta) {
q4 = axis.z * st2;
}
void Quaternion::rotate(const Vector3f &v) {
void Quaternion::rotate(const Vector3f &v)
{
Quaternion r;
r.from_axis_angle(v);
(*this) *= r;
}
void Quaternion::to_axis_angle(Vector3f &v) {
void Quaternion::to_axis_angle(Vector3f &v)
{
float l = sqrt(sq(q2)+sq(q3)+sq(q4));
v = Vector3f(q2,q3,q4);
if(l >= 1.0e-12f) {
if (l >= 1.0e-12f) {
v /= l;
v *= wrap_PI(2.0f * atan2f(l,q1));
}
}
void Quaternion::from_axis_angle_fast(Vector3f v) {
void Quaternion::from_axis_angle_fast(Vector3f v)
{
float theta = v.length();
if(theta < 1.0e-12f) {
if (theta < 1.0e-12f) {
q1 = 1.0f;
q2=q3=q4=0.0f;
}
@ -175,7 +180,8 @@ void Quaternion::from_axis_angle_fast(Vector3f v) {
from_axis_angle_fast(v,theta);
}
void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta) {
void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta)
{
float t2 = theta/2.0f;
float sqt2 = sq(t2);
float st2 = t2-sqt2*t2/6.0f;
@ -186,9 +192,12 @@ void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta) {
q4 = axis.z * st2;
}
void Quaternion::rotate_fast(const Vector3f &v) {
void Quaternion::rotate_fast(const Vector3f &v)
{
float theta = v.length();
if(theta < 1.0e-12f) return;
if (theta < 1.0e-12f) {
return;
}
float t2 = theta/2.0f;
float sqt2 = sq(t2);
float st2 = t2-sqt2*t2/6.0f;
@ -260,8 +269,7 @@ Quaternion Quaternion::inverse(void) const
void Quaternion::normalize(void)
{
float quatMag = length();
if (quatMag > 1e-16f)
{
if (quatMag > 1e-16f) {
float quatMagInv = 1.0f/quatMag;
q1 *= quatMagInv;
q2 *= quatMagInv;
@ -270,7 +278,8 @@ void Quaternion::normalize(void)
}
}
Quaternion Quaternion::operator*(const Quaternion &v) const {
Quaternion Quaternion::operator*(const Quaternion &v) const
{
Quaternion ret;
const float &w1 = q1;
const float &x1 = q2;
@ -290,7 +299,8 @@ Quaternion Quaternion::operator*(const Quaternion &v) const {
return ret;
}
Quaternion &Quaternion::operator*=(const Quaternion &v) {
Quaternion &Quaternion::operator*=(const Quaternion &v)
{
float w1 = q1;
float x1 = q2;
float y1 = q3;
@ -309,7 +319,8 @@ Quaternion &Quaternion::operator*=(const Quaternion &v) {
return *this;
}
Quaternion Quaternion::operator/(const Quaternion &v) const {
Quaternion Quaternion::operator/(const Quaternion &v) const
{
Quaternion ret;
const float &quat0 = q1;
const float &quat1 = q2;

31
libraries/AP_Math/quaternion.h
View File

@ -23,26 +23,31 @@
#include <assert.h>
#endif
class Quaternion
{
class Quaternion {
public:
float q1, q2, q3, q4;
// constructor creates a quaternion equivalent
// to roll=0, pitch=0, yaw=0
Quaternion() {
q1 = 1; q2 = q3 = q4 = 0;
Quaternion()
{
q1 = 1;
q2 = q3 = q4 = 0;
}
// setting constructor
Quaternion(const float _q1, const float _q2, const float _q3, const float _q4) :
q1(_q1), q2(_q2), q3(_q3), q4(_q4) {
q1(_q1), q2(_q2), q3(_q3), q4(_q4)
{
}
// function call operator
void operator ()(const float _q1, const float _q2, const float _q3, const float _q4)
void operator()(const float _q1, const float _q2, const float _q3, const float _q4)
{
q1 = _q1; q2 = _q2; q3 = _q3; q4 = _q4;
q1 = _q1;
q2 = _q2;
q3 = _q3;
q4 = _q4;
}
// check if any elements are NAN
@ -97,12 +102,17 @@ public:
void normalize();
// initialise the quaternion to no rotation
void initialise() { q1 = 1.0f; q2 = q3 = q4 = 0.0f; }
void initialise()
{
q1 = 1.0f;
q2 = q3 = q4 = 0.0f;
}
Quaternion inverse(void) const;
// allow a quaternion to be used as an array, 0 indexed
float & operator[](uint8_t i) {
float & operator[](uint8_t i)
{
float *_v = &q1;
#if MATH_CHECK_INDEXES
assert(i < 4);
@ -110,7 +120,8 @@ public:
return _v[i];
}
const float & operator[](uint8_t i) const {
const float & operator[](uint8_t i) const
{
const float *_v = &q1;
#if MATH_CHECK_INDEXES
assert(i < 4);