turbomath.h
Go to the documentation of this file.
1 /*
2  *
3  * BSD 3-Clause License
4  *
5  * Copyright (c) 2017, James Jackson BYU MAGICC Lab, Provo UT
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions are met:
10  *
11  * * Redistributions of source code must retain the above copyright notice, this
12  * list of conditions and the following disclaimer.
13  *
14  * * Redistributions in binary form must reproduce the above copyright notice,
15  * this list of conditions and the following disclaimer in the documentation
16  * and/or other materials provided with the distribution.
17  *
18  * * Neither the name of the copyright holder nor the names of its
19  * contributors may be used to endorse or promote products derived from
20  * this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
23  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
24  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
25  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #ifndef TURBOMATH_TURBOMATH_H
35 #define TURBOMATH_TURBOMATH_H
36 
37 #include <cstdint>
38 
39 namespace turbomath
40 {
41 
42 // float-based wrappers
43 float cos(float x);
44 float sin(float x);
45 float asin(float x);
46 float atan2(float y, float x);
47 float atan(float x);
48 float fsign(float y);
49 
50 // turbo-speed approximation of (1.0 - pow(pressure/101325.0, 0.1902631)) * 39097.63
51 // Used for calculating altitude in m from atmospheric pressure in Pa
52 float alt(float x);
53 
54 float inv_sqrt(float x);
55 float fabs(float x);
56 
58 {
59  float fvalue;
60  int32_t ivalue;
61 };
62 
63 class Vector
64 {
65 public:
66  float x;
67  float y;
68  float z;
69 
70  Vector();
71  Vector(float x_, float y_, float z_);
72 
73  float norm() const;
74  float sqrd_norm() const;
75  Vector& normalize();
76  Vector normalized() const;
77 
78  float dot(const Vector& v) const;
79  Vector cross(const Vector& v) const;
80 
81  Vector operator* (float s) const;
82  Vector operator/ (float s) const;
83  Vector& operator*= (float s);
84  Vector& operator/= (float s);
85  Vector operator+ (const Vector& v) const;
86  Vector operator- (const Vector& v) const;
87  Vector& operator+= (const Vector& v);
88  Vector& operator-= (const Vector& v);
89 };
90 
91 inline Vector operator* (float s, const Vector& v) { return v * s; }
92 inline Vector operator/ (float s, const Vector& v) { return v / s; }
93 
94 
96 {
97 public:
98  float w;
99  float x;
100  float y;
101  float z;
102 
103  Quaternion();
104  Quaternion(float w_, float x_, float y_, float z_);
105  Quaternion(const Vector& u, const Vector& v);
106  Quaternion(float roll, float pitch, float yaw);
107 
108  Vector rotate(const Vector& v) const;
109  Quaternion& normalize();
110  Quaternion inverse() const;
111  Quaternion& invert();
112  Quaternion& from_two_unit_vectors(const Vector& u, const Vector& v);
113  Quaternion& from_RPY(float roll, float pitch, float yaw);
114  void get_RPY(float *roll, float *pitch, float *yaw) const;
115 
116  Vector operator* (const Vector& v) const;
117  Quaternion operator* (const Quaternion& q) const;
118  Quaternion& operator*= (const Quaternion& q);
119  Vector boxminus(const Quaternion& q) const;
120  static Vector log(const Quaternion &q)
121  {
122  Vector v{q.x, q.y, q.z};
123  float norm_v = v.norm();
124 
125  Vector out;
126  if (norm_v < 1e-8)
127  {
128  out.x = out.y = out.z = 0.0;
129  }
130  else
131  {
132  out = 2.0*atan2(norm_v, q.w)*v/norm_v;
133  }
134  return out;
135  }
136 
137  Vector operator-(const Quaternion& q) const {return boxminus(q);}
138 };
139 
140 } // namespace turbomath
141 
142 #endif // TURBOMATH_TURBOMATH_H
Vector operator*(float s, const Vector &v)
Definition: turbomath.h:91
float atan(float x)
Definition: turbomath.cpp:433
float cos(float x)
Definition: turbomath.cpp:397
float sin(float x)
Definition: turbomath.cpp:402
float atan2(float y, float x)
Definition: turbomath.cpp:459
float fsign(float y)
Definition: turbomath.cpp:392
float alt(float press)
Definition: turbomath.cpp:518
float fabs(float x)
Definition: turbomath.cpp:538
Vector operator-(const Quaternion &q) const
Definition: turbomath.h:137
float asin(float x)
Definition: turbomath.cpp:499
float inv_sqrt(float x)
Definition: turbomath.cpp:547
Vector operator/(float s, const Vector &v)
Definition: turbomath.h:92
static Vector log(const Quaternion &q)
Definition: turbomath.h:120


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:20