turbotrig_test.cpp
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 #include "common.h"
35 
36 #include <cmath>
37 #include <cstdio>
38 
39 turbomath::Vector random_vectors[25] = {turbomath::Vector(-0.0376278050814, 0.471775699711, -0.336572370974),
40  turbomath::Vector(0.842139998851, -0.113277302409, -0.435361598132),
41  turbomath::Vector(0.402876930871, -0.998517068538, 0.956603957591),
42  turbomath::Vector(0.366004030077, -0.966554559399, 0.236455814495),
43  turbomath::Vector(0.170963581611, -0.892193316086, -0.360102936987),
44  turbomath::Vector(-0.675191763273, -0.794118513048, 0.561367212903),
45  turbomath::Vector(-0.0299477253533, 0.0938177650483, 0.525814272724),
46  turbomath::Vector(-0.676191678521, -0.0780862208203, -0.272955681219),
47  turbomath::Vector(-0.435749833209, -0.673810649938, -0.896559097382),
48  turbomath::Vector(0.709083915552, -0.135067363969, -0.385492450532),
49  turbomath::Vector(-0.38728558039, -0.502219301225, 0.323557018529),
50  turbomath::Vector(-0.186870345154, 0.554827454101, 0.921567682061),
51  turbomath::Vector(-0.142106787605, -0.764876359963, 0.00303689980819),
52  turbomath::Vector(-0.677798963582, -0.664595954482, 0.339274533414),
53  turbomath::Vector(-0.700464041114, 0.325731535871, -0.621492014391),
54  turbomath::Vector(-0.604865828708, 0.270639620454, 0.188624833185),
55  turbomath::Vector(0.464205180183, -0.461504601245, -0.578708441515),
56  turbomath::Vector(0.498899172115, -0.582342366402, -0.694758083436),
57  turbomath::Vector(0.0710544604541, -0.63603887083, -0.521799692437),
58  turbomath::Vector(-0.372025413205, 0.83531212357, 0.232484576742),
59  turbomath::Vector(0.790872496361, -0.89600683592, 0.783984438621),
60  turbomath::Vector(0.236462609786, -0.636362560394, 0.203951290805),
61  turbomath::Vector(0.831924307534, -0.482532468579, 0.0600026189612),
62  turbomath::Vector(0.0562194856302, -0.605799189029, -0.556494338297),
63  turbomath::Vector(-0.85014432598, 0.0632157037573, 0.0272188414114)};
64 
66  turbomath::Quaternion(0.10377420365, -0.583993115868, -0.731526280531, -0.0530049846003),
67  turbomath::Quaternion(-0.00228103177408, -0.506936771567, 0.976002181169, 0.90368722061),
68  turbomath::Quaternion(-0.280191704748, 0.141235897077, 0.770363502952, 0.306427689307),
69  turbomath::Quaternion(0.964538929753, -0.849755381903, 0.36374459234, 0.694507794584),
70  turbomath::Quaternion(0.0176390041681, -0.960155080148, 0.340078582124, -0.119639355159),
71  turbomath::Quaternion(-0.213139865459, -0.91618752978, -0.192746623826, -0.761937711418),
72  turbomath::Quaternion(-0.491440057128, -0.468120646081, -0.0682240789086, -0.779728041272),
73  turbomath::Quaternion(0.00414757516987, -0.980357614738, 0.243315557667, 0.487816606638),
74  turbomath::Quaternion(-0.593742280674, 0.245648066311, 0.682367014935, -0.0659175648814),
75  turbomath::Quaternion(-0.322464011587, 0.706588950729, -0.966024250287, -0.50354344519),
76  turbomath::Quaternion(-0.537023302971, -0.496355850419, -0.326843736039, 0.456606813517),
77  turbomath::Quaternion(-0.581585485434, 0.225766708322, -0.121402082687, 0.160333514827),
78  turbomath::Quaternion(-0.422711480811, 0.894994456476, 0.392582496229, 0.0035135659771),
79  turbomath::Quaternion(0.326380783544, 0.551413227108, 0.89489801397, 0.87883243747),
80  turbomath::Quaternion(0.83500683695, -0.263875030319, -0.1783391105, 0.453727091163),
81  turbomath::Quaternion(-0.30389938019, -0.0744317276089, -0.436917072268, 0.907173926266),
82  turbomath::Quaternion(-0.320066655494, -0.349065285706, 0.0336903431161, 0.573906603454),
83  turbomath::Quaternion(-0.103624452083, -0.82874783662, -0.635967208274, 0.562138574765),
84  turbomath::Quaternion(0.90735669209, -0.611711092446, 0.732474120503, 0.866697480004),
85  turbomath::Quaternion(0.626137839218, 0.41320663394, -0.821473642241, -0.344696411875),
86  turbomath::Quaternion(0.266650461152, -0.784707647527, 0.324347257562, -0.724904312141),
87  turbomath::Quaternion(0.964177603528, -0.378173605577, 0.767349174766, 0.560290218637),
88  turbomath::Quaternion(0.0812716046369, 0.745067180353, -0.476875959113, -0.245887902551),
89  turbomath::Quaternion(-0.177027678376, 0.214558558928, -0.992910369554, 0.592964390132),
90  turbomath::Quaternion(0.0979109306209, 0.121890109199, 0.126418158551, 0.242200145606)};
91 
92 TEST(TurboMath, atan)
93 {
94  for (float i = -200.0; i <= 200.0; i += 0.001)
95  {
96  EXPECT_NEAR(turbomath::atan(i), atan(i), 0.0001);
97  }
98 }
99 
100 TEST(TurboMath, sin_cos)
101 {
102  for (float i = -200.0; i <= 200.0; i += 0.001)
103  {
104  EXPECT_NEAR(turbomath::sin(i), sin(i), 0.0002);
105  EXPECT_NEAR(turbomath::cos(i), cos(i), 0.0002);
106  }
107 }
108 
109 TEST(TurboMath, atan2)
110 {
111  for (float i = -100.0; i <= 100.0; i += 0.1)
112  {
113  for (float j = -1.0; j <= 1.0; j += 0.001)
114  {
115  if (fabs(j) > 0.0001)
116  {
117  EXPECT_NEAR(turbomath::atan2(i, j), atan2(i, j), 0.001);
118  }
119  }
120  }
121 }
122 
123 TEST(TurboMath, asin)
124 {
125  for (float i = -1.0; i <= 1.0; i += 0.001)
126  {
127  if (fabs(i) < 0.95)
128  EXPECT_NEAR(turbomath::asin(i), asin(i), 0.0001);
129  else
130  EXPECT_NEAR(turbomath::asin(i), asin(i), 0.2);
131  }
132 }
133 
134 TEST(TurboMath, fastAlt)
135 {
136  // out of bounds
137  EXPECT_EQ(turbomath::alt(69681), 0.0);
138  EXPECT_EQ(turbomath::alt(10700), 0.0);
139 
140  // all valid int values
141  float trueResult = 0.0;
142  for (int i = 69682; i < 106597; i++)
143  {
144  trueResult = static_cast<float>((1.0 - pow(static_cast<float>(i) / 101325, 0.190284)) * 145366.45)
145  * static_cast<float>(0.3048);
146  EXPECT_NEAR(turbomath::alt(i), trueResult, .15);
147  // arbitrarily chose <= .15m since fast_alt isn't accurate enough for EXPECT_CLOSE,
148  // but being within .15 meters of the correct result seems pretty good to me
149  }
150 }
151 
152 TEST(TurboMath, Vector)
153 {
154  for (int i = 0; i < 24; i++)
155  {
156  turbomath::Vector vec1 = random_vectors[i];
157  turbomath::Vector vec2 = random_vectors[i + 1];
158  Eigen::Vector3f eig2, eig1;
159  eig1 << vec1.x, vec1.y, vec1.z;
160  eig2 << vec2.x, vec2.y, vec2.z;
161 
162  // Test data type
163  EXPECT_VEC3_SUPERCLOSE(vec1, eig1);
164  EXPECT_VEC3_SUPERCLOSE(vec2, eig2);
165 
166  // Test norming operations
168  EXPECT_SUPERCLOSE(vec1.norm(), eig1.norm());
169  EXPECT_SUPERCLOSE(vec1.sqrd_norm(), eig1.squaredNorm());
170  turbomath::Vector vec3 = vec1;
171  Eigen::Vector3f eig3 = eig1;
172 
173  vec3.normalize();
174  eig3.normalize();
175  EXPECT_VEC3_SUPERCLOSE(vec3, eig3);
176  EXPECT_SUPERCLOSE(vec3.norm(), 1.0);
177 
178  // Test add, subtract and multiply
179  EXPECT_VEC3_SUPERCLOSE((vec1 + vec2), (eig1 + eig2));
180  EXPECT_VEC3_SUPERCLOSE((vec1 - vec2), (eig1 - eig2));
181 
182  EXPECT_VEC3_SUPERCLOSE((vec1 * 5.0f), (eig1 * 5.0f));
183  EXPECT_VEC3_SUPERCLOSE((vec1 * -10.0f), (eig1 * -10.0f));
184 
185  vec1 *= -3.0f;
186  EXPECT_VEC3_SUPERCLOSE(vec1, (eig1 * -3.0f));
187  eig1 *= -3.0f;
188  EXPECT_VEC3_SUPERCLOSE(vec1, eig1);
189 
190  EXPECT_VEC3_SUPERCLOSE((vec1 / -10.0f), (eig1 / -10.0f));
191  vec1 /= -3.0f;
192  EXPECT_VEC3_SUPERCLOSE(vec1, (eig1 / -3.0f));
193  eig1 /= -3.0f;
194 
195  // Test Vector Dot Product
196  EXPECT_SUPERCLOSE(vec1.dot(vec2), eig1.transpose() * eig2);
197 
198  // Test Vector Cross Product
199  EXPECT_VEC3_SUPERCLOSE(vec1.cross(vec2), eig1.cross(eig2));
200  }
201 }
202 
203 TEST(TurboMath, Quaternion)
204 {
205  for (int i = 0; i < 24; i++)
206  {
207  turbomath::Quaternion quat1 = random_quaternions[i].normalize();
208  turbomath::Quaternion quat2 = random_quaternions[i + 1].normalize();
209 
210  Eigen::Quaternionf eig1(quat1.w, quat1.x, quat1.y, quat1.z);
211  Eigen::Quaternionf eig2(quat2.w, quat2.x, quat2.y, quat2.z);
212 
213  EXPECT_QUAT_SUPERCLOSE(quat1, eig1);
214  EXPECT_QUAT_SUPERCLOSE(quat2, eig2);
215 
216  // Check normalization
217  EXPECT_SUPERCLOSE(quat1.x * quat1.x + quat1.y * quat1.y + quat1.z * quat1.z + quat1.w * quat1.w, 1.0f);
218  EXPECT_SUPERCLOSE(quat2.x * quat2.x + quat2.y * quat2.y + quat2.z * quat2.z + quat2.w * quat2.w, 1.0f);
219 
220  // Test Quaternion Operators
221  ASSERT_QUAT_SUPERCLOSE((quat1 * quat2), (eig2 * eig1));
222  ASSERT_QUAT_SUPERCLOSE(quat1.inverse(), eig1.inverse());
223 
224  // Test Quaternion Rotate
225  turbomath::Vector vec1 = random_vectors[i];
226  Eigen::Vector3f veig1;
227  veig1 << vec1.x, vec1.y, vec1.z;
228 
229  // Test rotate_vector by rotating vector to new frame
230  turbomath::Vector vec2 = quat1.rotate(vec1);
231  Eigen::Vector3f veig2 = veig1.transpose() * eig1.toRotationMatrix();
232  EXPECT_VEC3_SUPERCLOSE(vec2, veig2); // compare with eigen
233 
234  // And rotate back
235  turbomath::Vector vec3 = quat1.inverse() * vec2;
236  EXPECT_VEC3_SUPERCLOSE(vec3, veig1);
237 
238  // Convert to and from euler angles
239  float p, t, s;
240  quat1.get_RPY(&p, &t, &s);
241 
242  turbomath::Quaternion quat3(p, t, s);
243  Eigen::Vector3f ihat(1, 0, 0);
244  Eigen::Vector3f jhat(0, 1, 0);
245  Eigen::Vector3f khat(0, 0, 1);
246 
247  Eigen::Quaternionf eig3 = Eigen::AngleAxisf(s, khat) * Eigen::AngleAxisf(t, jhat) * Eigen::AngleAxisf(p, ihat);
248 
249  EXPECT_QUAT_SUPERCLOSE(quat3, eig3);
250  EXPECT_SUPERCLOSE(quat1.w, quat3.w);
251  EXPECT_SUPERCLOSE(quat1.x, quat3.x);
252  EXPECT_SUPERCLOSE(quat1.y, quat3.y);
253  EXPECT_SUPERCLOSE(quat1.z, quat3.z);
254  }
255 }
256 
257 TEST(TurboMath, QuatFromTwoVectors)
258 {
259  // Test the "quat_from_two_vectors"
260  turbomath::Vector vec1(1.0f, 0.0f, 0.0f);
261  turbomath::Quaternion quat1(1.0f / sqrt(2.0f), 0.0f, 0.0f, 1.0f / sqrt(2.0f));
262  turbomath::Vector vec2 = quat1.rotate(vec1);
263  turbomath::Quaternion quat_test;
264  quat_test.from_two_unit_vectors(vec2, vec1);
265 
266  ASSERT_TURBOQUAT_SUPERCLOSE(quat1, quat_test);
267 
268  // A bunch of random (off-axes tests)
269  for (int i = 0; i < 25; i++)
270  {
271  turbomath::Vector vec3 = random_vectors[i].normalize();
272  turbomath::Quaternion quat2 = random_quaternions[i].normalize();
273 
274  // Manually rotate this vector with the quaternion
275  turbomath::Vector vec4 = quat2.rotate(vec3);
276 
277  // Extract the "shortest-arc" rotation
278  quat_test.from_two_unit_vectors(vec4, vec3);
279 
280  // Try rotating that same vector with this new quaternion
281  turbomath::Vector vec5 = quat_test.rotate(vec3);
282 
283  // Make sure that the vectors are the same
284  ASSERT_SUPERCLOSE(vec5.x, vec4.x);
285  ASSERT_SUPERCLOSE(vec5.y, vec4.y);
286  ASSERT_SUPERCLOSE(vec5.z, vec4.z);
287  }
288 }
float dot(const Vector &v) const
Definition: turbomath.cpp:120
#define ASSERT_SUPERCLOSE(x, y)
Definition: test/common.h:56
Vector cross(const Vector &v) const
Definition: turbomath.cpp:125
float atan(float x)
Definition: turbomath.cpp:383
float cos(float x)
Definition: turbomath.cpp:348
float sqrd_norm() const
Definition: turbomath.cpp:47
float sin(float x)
Definition: turbomath.cpp:353
float atan2(float y, float x)
Definition: turbomath.cpp:410
XmlRpcServer s
TEST(TurboMath, atan)
Vector & normalize()
Definition: turbomath.cpp:52
float alt(float press)
Definition: turbomath.cpp:470
float fabs(float x)
Definition: turbomath.cpp:491
#define EXPECT_SUPERCLOSE(x, y)
Definition: test/common.h:51
void get_RPY(float *roll, float *pitch, float *yaw) const
Definition: turbomath.cpp:263
#define ASSERT_TURBOQUAT_SUPERCLOSE(q, q2)
Definition: test/common.h:37
#define ASSERT_QUAT_SUPERCLOSE(q, q_eig)
Definition: test/common.h:27
Vector normalized() const
Definition: turbomath.cpp:61
Quaternion & from_two_unit_vectors(const Vector &u, const Vector &v)
Definition: turbomath.cpp:205
float asin(float x)
Definition: turbomath.cpp:449
turbomath::Quaternion random_quaternions[25]
#define EXPECT_VEC3_SUPERCLOSE(vec, eig)
Definition: test/common.h:14
turbomath::Vector random_vectors[25]
Quaternion & normalize()
Definition: turbomath.cpp:144
Quaternion inverse() const
Definition: turbomath.cpp:192
#define EXPECT_QUAT_SUPERCLOSE(q, q_eig)
Definition: test/common.h:18
float norm() const
Definition: turbomath.cpp:42
Vector rotate(const Vector &v) const
Definition: turbomath.cpp:179


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Sat May 9 2020 03:16:54