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 arr[3];
67  float &x;
68  float &y;
69  float &z;
70 
71  Vector();
72  Vector(const Vector &v);
73  Vector(float x_, float y_, float z_);
74 
75  float norm() const;
76  float sqrd_norm() const;
77  Vector &normalize();
78  Vector normalized() const;
79 
80  float dot(const Vector &v) const;
81  Vector cross(const Vector &v) const;
82 
83  Vector &operator= (const Vector &v);
84  Vector operator* (float s) const;
85  Vector operator/ (float s) const;
86  Vector &operator*= (float s);
87  Vector &operator/= (float s);
88  Vector operator+ (const Vector &v) const;
89  Vector operator- (const Vector &v) const;
90  Vector &operator+= (const Vector &v);
91  Vector &operator-= (const Vector &v);
92 };
93 
94 inline Vector operator* (float s, const Vector &v)
95 {
96  return v * s;
97 }
98 inline Vector operator/ (float s, const Vector &v)
99 {
100  return v / s;
101 }
102 
103 
105 {
106 public:
107  float w;
108  float x;
109  float y;
110  float z;
111 
112  Quaternion();
113  Quaternion(float w_, float x_, float y_, float z_);
114  Quaternion(const Vector &u, const Vector &v);
115  Quaternion(float roll, float pitch, float yaw);
116 
117  Vector rotate(const Vector &v) const;
118  Quaternion &normalize();
119  Quaternion inverse() const;
120  Quaternion &invert();
121  Quaternion &from_two_unit_vectors(const Vector &u, const Vector &v);
122  Quaternion &from_RPY(float roll, float pitch, float yaw);
123  void get_RPY(float *roll, float *pitch, float *yaw) const;
124 
125  Vector operator* (const Vector &v) const;
126  Quaternion operator* (const Quaternion &q) const;
127  Quaternion &operator*= (const Quaternion &q);
128  Vector boxminus(const Quaternion &q) const;
129  static Vector log(const Quaternion &q)
130  {
131  Vector v{q.x, q.y, q.z};
132  float norm_v = v.norm();
133 
134  Vector out;
135  if (norm_v < 1e-8)
136  {
137  out.x = out.y = out.z = 0.0;
138  }
139  else
140  {
141  out = 2.0*atan2(norm_v, q.w)*v/norm_v;
142  }
143  return out;
144  }
145 
146  Vector operator-(const Quaternion &q) const
147  {
148  return boxminus(q);
149  }
150 };
151 
152 } // namespace turbomath
153 
154 #endif // TURBOMATH_TURBOMATH_H
Vector operator*(float s, const Vector &v)
Definition: turbomath.h:94
float atan(float x)
Definition: turbomath.cpp:460
float cos(float x)
Definition: turbomath.cpp:422
float sin(float x)
Definition: turbomath.cpp:427
float atan2(float y, float x)
Definition: turbomath.cpp:488
float fsign(float y)
Definition: turbomath.cpp:417
float alt(float press)
Definition: turbomath.cpp:549
float fabs(float x)
Definition: turbomath.cpp:571
Vector operator-(const Quaternion &q) const
Definition: turbomath.h:146
float asin(float x)
Definition: turbomath.cpp:528
float inv_sqrt(float x)
Definition: turbomath.cpp:580
Vector operator/(float s, const Vector &v)
Definition: turbomath.h:98
static Vector log(const Quaternion &q)
Definition: turbomath.h:129


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Wed Jul 3 2019 19:59:26