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 // float-based wrappers
42 float cos(float x);
43 float sin(float x);
44 float asin(float x);
45 float atan2(float y, float x);
46 float atan(float x);
47 float fsign(float y);
48 
49 // turbo-speed approximation of (1.0 - pow(pressure/101325.0, 0.1902631)) * 39097.63
50 // Used for calculating altitude in m from atmospheric pressure in Pa
51 float alt(float x);
52 
53 float inv_sqrt(float x);
54 float fabs(float x);
55 
57 {
58  float fvalue;
59  int32_t ivalue;
60 };
61 
62 class Vector
63 {
64 public:
65  float x;
66  float y;
67  float z;
68 
69  Vector();
70  Vector(float x_, float y_, float z_);
71 
72  float norm() const;
73  float sqrd_norm() const;
74  Vector& normalize();
75  Vector normalized() const;
76 
77  float dot(const Vector& v) const;
78  Vector cross(const Vector& v) const;
79 
80  Vector operator*(float s) const;
81  Vector operator/(float s) const;
82  Vector& operator*=(float s);
83  Vector& operator/=(float s);
84  Vector operator+(const Vector& v) const;
85  Vector operator-(const Vector& v) const;
86  Vector& operator+=(const Vector& v);
87  Vector& operator-=(const Vector& v);
88 };
89 
90 inline Vector operator*(float s, const Vector& v)
91 {
92  return v * s;
93 }
94 inline Vector operator/(float s, const Vector& v)
95 {
96  return v / s;
97 }
98 
100 {
101 public:
102  float w;
103  float x;
104  float y;
105  float z;
106 
107  Quaternion();
108  Quaternion(float w_, float x_, float y_, float z_);
109  Quaternion(const Vector& u, const Vector& v);
110  Quaternion(float roll, float pitch, float yaw);
111 
112  Vector rotate(const Vector& v) const;
113  Quaternion& normalize();
114  Quaternion inverse() const;
115  Quaternion& invert();
116  Quaternion& from_two_unit_vectors(const Vector& u, const Vector& v);
117  Quaternion& from_RPY(float roll, float pitch, float yaw);
118  void get_RPY(float* roll, float* pitch, float* yaw) const;
119 
120  Vector operator*(const Vector& v) const;
121  Quaternion operator*(const Quaternion& q) const;
122  Quaternion& operator*=(const Quaternion& q);
123  Vector boxminus(const Quaternion& q) const;
124  static Vector log(const Quaternion& q)
125  {
126  Vector v{q.x, q.y, q.z};
127  float norm_v = v.norm();
128 
129  Vector out;
130  if (norm_v < 1e-8)
131  {
132  out.x = out.y = out.z = 0.0;
133  }
134  else
135  {
136  out = 2.0 * atan2(norm_v, q.w) * v / norm_v;
137  }
138  return out;
139  }
140 
141  Vector operator-(const Quaternion& q) const { return boxminus(q); }
142 };
143 
144 } // namespace turbomath
145 
146 #endif // TURBOMATH_TURBOMATH_H
Vector operator*(float s, const Vector &v)
Definition: turbomath.h:90
float atan(float x)
Definition: turbomath.cpp:383
float cos(float x)
Definition: turbomath.cpp:348
float sin(float x)
Definition: turbomath.cpp:353
float atan2(float y, float x)
Definition: turbomath.cpp:410
float fsign(float y)
Definition: turbomath.cpp:343
float alt(float press)
Definition: turbomath.cpp:470
float fabs(float x)
Definition: turbomath.cpp:491
Vector operator-(const Quaternion &q) const
Definition: turbomath.h:141
float asin(float x)
Definition: turbomath.cpp:449
float inv_sqrt(float x)
Definition: turbomath.cpp:499
Vector operator/(float s, const Vector &v)
Definition: turbomath.h:94
static Vector log(const Quaternion &q)
Definition: turbomath.h:124


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Apr 15 2021 05:07:49