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


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